The evolution of aerial robotics has pushed drone formation flight from a theoretical concept to a critical operational paradigm. The coordinated flight of multiple Unmanned Aerial Vehicles (UAVs) offers significant advantages, including increased operational coverage, resource sharing, and enhanced system robustness through redundancy. However, the core challenge in maintaining a tight drone formation, especially during close-proximity maneuvers, is guaranteeing collision-free operations. Navigation inaccuracies, atmospheric disturbances, and communication latencies can cause unpredictable deviations, transforming a coordinated unit into a hazardous cluster. Therefore, developing a reliable, real-time anti-collision system is not merely an enhancement but a fundamental safety requirement for the practical deployment of autonomous drone formations.
This work presents the design and validation of a decentralized anti-collision system tailored for a two-agent drone formation. The primary objective is to maintain a safe separation distance between a leader and a wingman UAV by predicting potential conflict states and executing timely evasion maneuvers. At the heart of this system lies an Adaptive Kalman Filter (AKF) algorithm, chosen for its ability to provide optimal state estimates in the presence of noise while dynamically adjusting to changing system conditions, thereby suppressing filter divergence. To rigorously test the algorithm under realistic constraints, a Hardware-in-the-Loop (HIL) simulation platform was constructed. This platform utilizes ultrasonic sensors for inter-agent ranging and a Zigbee-based wireless network for communication, emulating the core sensory and data-link functions of actual UAVs.
The fundamental challenge in drone formation collision avoidance stems from the need for precise, real-time relative state estimation under uncertainty. The system must continuously answer: What is the current relative distance, velocity, and acceleration between the drones? Simple measurement from sensors like ultrasonic rangefinders is corrupted by noise. Furthermore, the dynamics of a maneuvering drone formation are inherently time-varying; a filter model that is optimal for steady, level flight may perform poorly during aggressive turns or acceleration phases, leading to lagging estimates or even divergence. This can be catastrophic. A non-adaptive filter might underestimate closing speed, delaying the collision avoidance command until it is too late. Therefore, the core algorithmic challenge is twofold: 1) to optimally filter noisy sensor data to obtain the best possible estimate of the relative state, and 2) to ensure the filtering process remains robust and stable as the formation’s flight dynamics change.
To address this, we model the relative motion of the wingman with respect to the leader. We define the state vector at time step \(k\) as:
$$ \mathbf{X}_k = [r_k, u_k, a_k]^T $$
where \(r_k\) is the relative distance, \(u_k\) is the relative velocity, and \(a_k\) is the relative acceleration. The system is assumed to follow a discretized constant acceleration model (within a short prediction horizon), leading to the state transition equation:
$$ \mathbf{X}_{k+1} = \mathbf{\Phi} \mathbf{X}_k + \mathbf{w}_k $$
Here, \(\mathbf{\Phi}\) is the state transition matrix. For a sampling period \(T\), it is defined as:
$$ \mathbf{\Phi} = \begin{bmatrix} 1 & T & T^2/2 \\ 0 & 1 & T \\ 0 & 0 & 1 \end{bmatrix} $$
The term \(\mathbf{w}_k\) represents the process noise, accounting for unmodeled dynamics and disturbances, with covariance matrix \(\mathbf{Q}\).
The measurement from the ultrasonic sensor is the relative distance, giving us the measurement equation:
$$ z_k = \mathbf{H} \mathbf{X}_k + v_k $$
where \(\mathbf{H} = [1, 0, 0]\) is the measurement matrix, and \(v_k\) is the measurement noise with variance \(R\).
The standard Kalman Filter provides the optimal recursive solution for estimating \(\mathbf{X}_k\) given this linear model and Gaussian noises. The recursive steps are:
Prediction:
State Prediction: $$\hat{\mathbf{X}}_{k|k-1} = \mathbf{\Phi} \hat{\mathbf{X}}_{k-1|k-1}$$
Error Covariance Prediction: $$\mathbf{P}_{k|k-1} = \mathbf{\Phi} \mathbf{P}_{k-1|k-1} \mathbf{\Phi}^T + \mathbf{Q}$$
Update:
Kalman Gain: $$\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}^T (\mathbf{H} \mathbf{P}_{k|k-1} \mathbf{H}^T + R)^{-1}$$
State Update: $$\hat{\mathbf{X}}_{k|k} = \hat{\mathbf{X}}_{k|k-1} + \mathbf{K}_k (z_k – \mathbf{H} \hat{\mathbf{X}}_{k|k-1})$$
Covariance Update: $$\mathbf{P}_{k|k} = (\mathbf{I} – \mathbf{K}_k \mathbf{H}) \mathbf{P}_{k|k-1}$$
However, the performance of this filter is highly sensitive to the prior knowledge of \(\mathbf{Q}\) and \(R\). In a dynamic drone formation, these noise characteristics are not constant. An incorrect \(\mathbf{Q}\) can cause the filter to be overconfident in its model (leading to divergence) or too sluggish. The Adaptive Kalman Filter enhances robustness by tuning the filter parameters online. The core idea is to treat the sequence of state estimates \(\hat{\mathbf{X}}_{k|k}\) as a “desired signal” that is passed through an adaptive filter to refine the overall estimation process, effectively learning the optimal weighting for incoming measurements versus model predictions.
We conceptualize the standard KF output \(\hat{\mathbf{X}}_{k|k}\) as the input to an adaptive transversal filter. The output of this adaptive filter, \(y_k\), aims to track \(\hat{\mathbf{X}}_{k|k}\) itself, but through a weighted combination of past state estimates. The weights \(\mathbf{W}_k\) are adjusted to minimize the mean-square error. The derivation leads to a secondary estimation problem for the optimal weight vector \(\mathbf{W}^*\). The innovation—the difference between the new state estimate and the adaptive filter’s prediction—is used to adjust \(\mathbf{W}_k\). This mechanism allows the filter to adapt to changes in the statistical properties of the signal (the state trajectory), which correspond to changes in the maneuver intensity of the drone formation.
The key adaptive update equations for the weight vector are derived from the minimum mean-square error (MMSE) criterion. The optimal weight vector \(\mathbf{W}^*_k\) is modeled as a constant state with a measurement model based on past state data \(\mathbf{B}_k\) (a vector containing recent state estimates):
$$ \mathbf{W}^*_k = \mathbf{W}^*_{k-1} $$
$$ \hat{\mathbf{X}}_{k|k} \approx \mathbf{B}_k^T \mathbf{W}^*_k + e_k $$
where \(e_k\) is the irreducible minimum error. We then apply a Kalman-like recursion to estimate \(\mathbf{W}^*_k\):
Weight Prediction: $$\hat{\mathbf{W}}_{k|k-1} = \hat{\mathbf{W}}_{k-1|k-1}$$
Innovation for Weights: $$\boldsymbol{\nu}_k^w = \hat{\mathbf{X}}_{k|k} – \mathbf{B}_k^T \hat{\mathbf{W}}_{k|k-1}$$
Gain for Weights: $$\mathbf{K}_k^w = \mathbf{P}^w_{k-1} \mathbf{B}_k (\mathbf{B}_k^T \mathbf{P}^w_{k-1} \mathbf{B}_k + \xi_{min})^{-1}$$
Weight Update: $$\hat{\mathbf{W}}_{k|k} = \hat{\mathbf{W}}_{k|k-1} + \mathbf{K}_k^w \boldsymbol{\nu}_k^w$$
Covariance Update for Weights: $$\mathbf{P}^w_k = \mathbf{P}^w_{k-1} – \mathbf{K}_k^w \mathbf{B}_k^T \mathbf{P}^w_{k-1}$$
Here, \(\xi_{min}\) is a small constant representing the minimum achievable mean-square error, typically set between \(10^{-2}\) and \(10^{-3}\). This adaptive layer provides a corrective signal that modulates the primary Kalman filter’s behavior, making the combined Adaptive Kalman Filter highly resilient to model inaccuracies in the drone formation‘s flight envelope.
The anti-collision logic is straightforward but critical. A safety threshold \(r_{safe}\) and a warning threshold \(r_{warn} (r_{warn} > r_{safe})\) are defined. The adaptive filter continuously provides estimates \(\hat{r}_k, \hat{u}_k\). As long as \(\hat{r}_k > r_{warn}\), the wingman executes its nominal formation-keeping controller. Once \(\hat{r}_k \leq r_{warn}\), the anti-collision system overrides the nominal controller and commands the wingman to execute a pre-defined evasion maneuver (e.g., a turn or deceleration) to increase separation. The system resets to formation-keeping mode once \(\hat{r}_k > r_{warn}\) and the relative velocity is non-threatening.
The selection of noise covariance matrices \(\mathbf{Q}\) and \(R\) is crucial for initial filter tuning. Based on the characteristics of the drone formation platform and sensors, typical values are summarized below:
| Parameter | Description | Typical Value/Range | Rationale |
|---|---|---|---|
| \(\mathbf{Q}\) (diag) | Process Noise Covariance | \(diag([0.1, 0.5, 0.2])\) | Reflects uncertainty in acceleration and velocity models. Higher values on velocity/acceleration allow the filter to track maneuvers more quickly. |
| \(R\) | Measurement Noise Variance | \(0.5 \, \text{m}^2\) to \(2.0 \, \text{m}^2\) | Derived from ultrasonic sensor accuracy specs and real-world noise tests. |
| \(\xi_{min}\) | Adaptive Filter Minimum MSE | \(1 \times 10^{-3}\) | A small constant to ensure numerical stability in the weight update equation. |
| \(T\) | Sampling Period | \(0.1 \, \text{s}\) | Balances update rate with computational load and sensor limitations. |
To validate the algorithm without the risks and costs of full-scale flight tests, a comprehensive HIL simulation platform was developed. This platform substitutes the actual aircraft dynamics and physical environment with real-time software simulations, while integrating real hardware for sensing, computation, and actuation. This approach provides a high-fidelity test bed where the anti-collision controller interacts with realistic sensor noise and communication delays.
The core of the wingman agent is a microcontroller board emulating the flight computer. The key hardware components and their functions are detailed below:
| Component | Model/Specification | Role in the System |
|---|---|---|
| Microcontroller | C8051F015 (8051-core) | Central processor. Runs the adaptive filter algorithm, handles sensor I/O, and generates motor control signals. |
| Ranging Sensor | Ultrasonic Module (HC-SR04 type) | Measures the distance to the leader agent using Time-of-Flight (ToF) of ultrasonic pulses. Provides the primary measurement \(z_k\). |
| Wireless Communication | Zigbee (XBEE Module) | Establishes a low-power, low-latency peer-to-peer data link between leader and wingman for sharing state information and commands. |
| Motor Driver | L298N H-Bridge | Converts low-power control signals from the microcontroller into high-current drives for the wheel/motor actuators on the ground vehicle. |
| Actuator (Surrogate) | Differential-Drive Wheeled Robot | Acts as a ground-based surrogate for the drone. Its 2D motion (forward/back, turn) effectively tests the lateral and longitudinal collision logic. |
The software architecture follows a cyclic executive model. In each cycle (period \(T = 0.1s\)): 1) The ultrasonic sensor is triggered, and the distance measurement \(z_k\) is acquired. 2) This measurement, along with any data received via Zigbee (e.g., leader’s intent), is passed to the Adaptive Kalman Filter. 3) The AKF executes its prediction and update steps, producing the estimated state \(\hat{\mathbf{X}}_{k|k}\). 4) The collision avoidance logic evaluates \(\hat{r}_k\) and \(\hat{u}_k\) against the thresholds. 5) Based on the logic output, target velocities for the left and right wheels are computed. 6) These commands are sent to the motor driver, and the cycle repeats. This seamless integration of hardware and software creates a realistic closed-loop test environment for the drone formation safety system.
The performance of the Adaptive Kalman Filter-based anti-collision system was evaluated through a series of HIL tests. The scenario involved a leader agent moving on a predefined path, with the wingman attempting to follow at a desired distance of 15 meters. A safety threshold \(r_{safe}\) was set at 10 meters, with the warning/activation threshold \(r_{warn}\) at 12 meters. The leader periodically performed maneuvers that would cause the separation to decrease dangerously if not corrected.
The primary metric is the inter-agent distance. The raw ultrasonic measurements are noisy, as expected. The AKF effectively smoothens this data, providing a stable and accurate estimate of the true distance. Critically, when the filtered distance \(\hat{r}_k\) approaches and crosses the \(r_{warn}\) threshold, the system reacts consistently and promptly. The following sequence is observed in the data: 1) The estimated distance drops below 12m. 2) The estimated velocity \(\hat{u}_k\) shows a significant negative value (closing speed). 3) The anti-collision logic triggers, commanding the wingman to turn away and decelerate. 4) The distance estimate begins to increase, and once it surpasses 12m with a positive relative velocity, normal formation keeping resumes. This demonstrates the system’s fundamental capability to prevent a breach of the minimum safe distance in a dynamic drone formation.
The superiority of the Adaptive Kalman Filter over a standard, fixed-parameter KF is evident in the state estimation quality. The adaptive mechanism prevents the filter from becoming “stale” during aggressive maneuvers. A quantitative analysis of estimation error supports this. Let us define the estimation error for distance and velocity at time \(k\) as:
$$ e_{r,k} = r_{true,k} – \hat{r}_{k|k}, \quad e_{u,k} = u_{true,k} – \hat{u}_{k|k} $$
We can then compute the Root Mean Square Error (RMSE) over a test run of \(N\) samples:
$$ RMSE_r = \sqrt{\frac{1}{N} \sum_{k=1}^{N} e_{r,k}^2}, \quad RMSE_u = \sqrt{\frac{1}{N} \sum_{k=1}^{N} e_{u,k}^2} $$
A comparison between the raw measurements, standard KF, and the proposed AKF reveals a clear performance gradient. The adaptive filter consistently achieves the lowest RMSE, particularly for velocity and acceleration estimates, which are crucial for predicting future separation.
| Estimation Method | Distance RMSE (m) | Velocity RMSE (m/s) | Acceleration RMSE (m/s²) | Note |
|---|---|---|---|---|
| Raw Ultrasonic Measurement | ~0.8 – 1.2 | N/A (Not directly measured) | N/A | High noise, unusable for derivative signals. |
| Standard Kalman Filter | ~0.25 | ~0.35 | ~1.5 | Good distance smoothing, but velocity/acceleration estimates lag and diverge during maneuvers. |
| Adaptive Kalman Filter (Proposed) | ~0.15 | ~0.12 | ~0.4 | Superior smoothing and tracking. Effectively suppresses divergence, providing stable and accurate estimates for all states. |
The real-time performance was also satisfactory. The complete control cycle—sensing, filtering, decision, actuation—was reliably executed within the 100ms window on the chosen microcontroller, confirming the computational feasibility of the algorithm for real-time drone formation control.
In conclusion, this research successfully demonstrates a practical and effective anti-collision system for a two-agent drone formation. By integrating an Adaptive Kalman Filter for robust state estimation with a threshold-based collision avoidance logic, the system can reliably maintain safe separation distances. The use of a Hardware-in-the-Loop simulation platform, featuring realistic ultrasonic ranging and Zigbee communication, provided a high-confidence validation environment that closely mirrors the challenges of actual flight. The results clearly show that the adaptive filter outperforms a standard Kalman filter, particularly in maintaining accurate velocity and acceleration estimates during dynamic maneuvers, which is essential for timely collision prediction and avoidance. The system exhibits strong robustness, real-time capability, and effective divergence suppression. This work provides a foundational framework that can be extended to larger drone formations, incorporating more complex conflict resolution strategies and integrating additional sensors like vision or lidar for a multi-modal safety solution.