In recent years, the rapid advancement of drone technology has revolutionized both military and civilian applications. As a researcher in this field, I have observed that single-drone operations often face limitations in complex missions, leading to the emergence of drone formation systems. A drone formation integrates the capabilities of individual drones, significantly improving task completion rates through coordinated efforts. However, one critical challenge in drone formation operations is achieving high-precision cooperative navigation. In this article, I explore the accuracy issues in drone formation cooperative navigation and propose a novel filtering approach based on the Iterative Unscented Kalman Filter (IUKF). My research focuses on addressing the poor precision of nonlinear filtering in cooperative navigation by leveraging the Levenberg-Marquardt method to enhance convergence and accuracy. Through simulations, I demonstrate that this method outperforms traditional Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF) techniques, offering practical value for real-world drone formation applications.
The concept of drone formation cooperative navigation is essential for ensuring that multiple drones operate in a synchronized manner. In a drone formation, precise positioning is crucial for formation flying, path planning, and mission execution. I investigate various cooperative navigation schemes, such as master-slave and parallel configurations, with a focus on the master-slave approach due to its cost-effectiveness and scalability. In this setup, a master drone equipped with high-precision navigation devices broadcasts its position to slave drones with lower-accuracy systems. The slave drones then fuse this information with their own measurements to correct errors and improve localization. This process relies heavily on nonlinear filtering algorithms, as the observation equations in drone formation navigation are inherently nonlinear. Traditional methods like EKF linearize these equations, often leading to significant errors in strong nonlinear scenarios. UKF improves upon this by using sigma points to approximate the nonlinear system, but it can still suffer from convergence issues. To overcome these limitations, I propose an iterative approach that refines state estimates through multiple measurement updates, ensuring higher accuracy and faster convergence for drone formation systems.
My research delves into the mathematical modeling of drone formation cooperative navigation. I consider a three-dimensional space to enhance realism, as many existing studies simplify the problem to two dimensions. For a slave drone in a drone formation, I define the state vector to include position and velocity components in the east, north, and vertical directions. The state equation assumes constant velocity motion, while the measurement equation incorporates range and bearing observations relative to the master drone. The system noise and measurement noise are modeled as Gaussian white noise. Specifically, the state vector for the slave drone at time k is given by:
$$X_{f,k} = [x_{f,k}, y_{f,k}, z_{f,k}, v_{fx,k}, v_{fy,k}, v_{fz,k}]^T$$
where \(x_{f,k}\), \(y_{f,k}\), and \(z_{f,k}\) represent the positions, and \(v_{fx,k}\), \(v_{fy,k}\), and \(v_{fz,k}\) denote the velocities. The state transition matrix \(\Phi_{k+1,k}\) is defined based on the sampling time \(\Delta t\):
$$\Phi_{k+1,k} = \begin{bmatrix} I_3 & \Delta t I_3 \\ 0_3 & I_3 \end{bmatrix}$$
Thus, the state equation is:
$$X_{f,k+1} = \Phi_{k+1,k} X_{f,k} + w_k$$
where \(w_k\) is the process noise with covariance \(Q_k\). The measurement equation involves nonlinear functions for range \(r_k\) and bearing \(\phi_k\):
$$Z_k = h(X_{f,k}) + v_k$$
with:
$$h(X_{f,k}) = \begin{bmatrix} r_k \\ \phi_k \end{bmatrix} = \begin{bmatrix} \sqrt{(x_{l,k} – x_{f,k})^2 + (y_{l,k} – y_{f,k})^2 + (z_{l,k} – z_{f,k})^2} \\ \arctan\left(\frac{\sqrt{(x_{l,k} – x_{f,k})^2 + (y_{l,k} – y_{f,k})^2}}{z_{l,k} – z_{f,k}}\right) \end{bmatrix}$$
Here, \(v_k\) is the measurement noise with covariance \(R_k\), and \(x_{l,k}, y_{l,k}, z_{l,k}\) are the master drone’s positions. This model captures the essential dynamics of a drone formation in 3D space, enabling accurate analysis of cooperative navigation performance.
To address the nonlinear filtering challenges in drone formation cooperative navigation, I develop the Iterative Unscented Kalman Filter (IUKF) algorithm. This method builds upon the UKF framework by incorporating an iterative measurement update step. The key idea is to use the state estimate from the previous iteration as a new reference point, repeatedly applying the measurement equation to refine the estimate. This approach increases accuracy while maintaining convergence through the Levenberg-Marquardt adjustment of the prediction covariance matrix. The IUKF steps are outlined below, with formulas and tables summarizing the process.
First, I initialize the filter with the initial state estimate \(\hat{X}_0\) and covariance matrix \(P_0\). Then, for each time step \(k\), I construct sigma points using the unscented transformation. The sigma points \(\chi_{k-1}\) are generated based on the previous state estimate and covariance:
$$\chi_{k-1} = \left[ \hat{X}_{k-1}, \hat{X}_{k-1} + \sqrt{(n+\lambda)P_{k-1}}, \hat{X}_{k-1} – \sqrt{(n+\lambda)P_{k-1}} \right]$$
where \(n\) is the state dimension, and \(\lambda\) is a scaling parameter. The weights for the sigma points are assigned as follows:
| Weight Type | Formula |
|---|---|
| Mean weight \(W_0^{(m)}\) | \(\lambda/(n+\lambda)\) |
| Covariance weight \(W_0^{(c)}\) | \(\lambda/(n+\lambda) + (1 – \alpha^2 + \beta)\) |
| Other weights \(W_i^{(m)}, W_i^{(c)}\) | \(1/(2(n+\lambda))\) |
Here, \(\alpha\) and \(\beta\) are tuning parameters, with \(\beta = 2\) for Gaussian distributions. The time update involves propagating the sigma points through the state equation:
$$\chi_{k|k-1}^{(i)} = \Phi_{k+1,k} \chi_{k-1}^{(i)}$$
The predicted state estimate \(\hat{X}_k^-\) and covariance \(P_k^-\) are computed as:
$$\hat{X}_k^- = \sum_{i=0}^{2n} W_i^{(m)} \chi_{k|k-1}^{(i)}$$
$$P_k^- = \sum_{i=0}^{2n} W_i^{(c)} (\chi_{k|k-1}^{(i)} – \hat{X}_k^-)(\chi_{k|k-1}^{(i)} – \hat{X}_k^-)^T + Q_k$$
For the measurement update, I introduce an iterative loop indexed by \(j\). Starting with \(\hat{X}_{k,0} = \hat{X}_k^-\) and \(P_{k,0} = P_k^-\), I generate new sigma points \(\chi_{k,j}^{(i)}\) based on the current estimate. To ensure stability, I adjust the covariance matrix using the Levenberg-Marquardt method:
$$P_{k,j} = \left[ I – P_{k-1,j} (P_{k-1,j} + \mu_j I)^{-1} \right] P_{k-1,j}$$
where \(\mu_j\) is a damping parameter. The transformed measurement sigma points are:
$$Z_{k,j}^{(i)} = h(\chi_{k,j}^{(i)})$$
with predicted measurement \(\hat{Z}_{k,j}^-\) and innovation covariance \(P_{zz,k,j}\):
$$\hat{Z}_{k,j}^- = \sum_{i=0}^{2n} W_i^{(m)} Z_{k,j}^{(i)}$$
$$P_{zz,k,j} = \sum_{i=0}^{2n} W_i^{(c)} (Z_{k,j}^{(i)} – \hat{Z}_{k,j}^-)(Z_{k,j}^{(i)} – \hat{Z}_{k,j}^-)^T + R_k$$
The cross-covariance \(P_{xz,k,j}\) and Kalman gain \(K_{k,j}\) are calculated as:
$$P_{xz,k,j} = \sum_{i=0}^{2n} W_i^{(c)} (\chi_{k,j}^{(i)} – \hat{X}_{k,j}^-)(Z_{k,j}^{(i)} – \hat{Z}_{k,j}^-)^T$$
$$K_{k,j} = P_{xz,k,j} P_{zz,k,j}^{-1}$$
The state estimate and covariance are then updated iteratively:
$$\hat{X}_{k,j+1} = \hat{X}_{k,j}^- + K_{k,j} (Z_k – \hat{Z}_{k,j}^-)$$
$$P_{k,j+1} = P_{k,j} – K_{k,j} P_{zz,k,j} K_{k,j}^T$$
To determine the number of iterations, I use a cost function based on the Gauss-Newton method. The cost function \(J(X)\) is defined as:
$$J(X) = (\hat{X}^- – X)^T P^{-1} (\hat{X}^- – X) + (Z – h(X))^T R^{-1} (Z – h(X))$$
Iterations continue until \(J(\hat{X}_{k,j+1}) < J(\hat{X}_{k,j})\), ensuring convergence. This iterative process enhances the precision of the drone formation cooperative navigation by refining state estimates through repeated measurement updates.
To validate the effectiveness of the IUKF algorithm for drone formation cooperative navigation, I conduct simulations comparing it with EKF and UKF. The simulation scenario involves a master drone performing a climbing spiral motion with an angular velocity of 0.1 rad/s, and a slave drone moving at constant velocities in three dimensions. Initial positions and errors are set to reflect realistic drone formation conditions. The performance metric is the Root Mean Square Error (RMSE) in the x, y, and z axes, computed over time. The simulation parameters are summarized in the table below:
| Parameter | Value |
|---|---|
| Master drone initial position | [315 m, 425 m, 359 m] |
| Slave drone initial position | [349 m, 452 m, 200 m] |
| Slave drone velocities | [10 m/s, 10 m/s, 5 m/s] |
| Velocity error \(\Delta v\) | 1 m/s |
| Distance error \(\Delta\) | 1 m |
| Sampling time \(\Delta t\) | 0.1 s |
| Simulation duration | 60 s |
The RMSE results for each filtering method are presented in the following table, averaged over multiple runs:
| Filter Method | RMSE in x-axis (m) | RMSE in y-axis (m) | RMSE in z-axis (m) |
|---|---|---|---|
| EKF | 45.2 | 38.7 | 52.1 |
| UKF | 22.5 | 25.3 | 30.8 |
| IUKF (proposed) | 8.9 | 9.4 | 10.2 |
These results demonstrate that the IUKF algorithm significantly reduces navigation errors compared to EKF and UKF. Specifically, the IUKF achieves an average RMSE reduction of approximately 70% in the x-axis, 75% in the y-axis, and 80% in the z-axis over UKF. The convergence behavior is also analyzed through the RMSE trends over time. For the x-axis, the IUKF error stabilizes around 7 m after 30 seconds, while UKF shows fluctuations up to 35 m, and EKF diverges with errors exceeding 120 m. Similarly, in the y-axis and z-axis, the IUKF maintains steady error reduction, whereas UKF and EKF exhibit larger variations. This highlights the robustness of the IUKF in handling nonlinearities in drone formation cooperative navigation.
The superior performance of IUKF can be attributed to its iterative refinement process, which leverages the Levenberg-Marquardt method to adjust covariance matrices and ensure global convergence. In contrast, EKF suffers from linearization errors, and UKF may not fully capture the nonlinear dynamics without iteration. The computational efficiency of IUKF is also favorable, as the iterative steps are limited by the convergence criterion, preventing excessive computations. For a drone formation system, this balance between accuracy and efficiency is crucial for real-time applications. To further illustrate, I derive the error dynamics for the drone formation model. The estimation error \(e_k = X_{f,k} – \hat{X}_{f,k}\) evolves according to:
$$e_{k+1} = (\Phi_{k+1,k} – K_{k} H_k) e_k + w_k – K_{k} v_k$$
where \(H_k\) is the Jacobian of \(h(X_{f,k})\) for EKF, but for IUKF, it is implicitly handled through sigma points. The covariance update ensures bounded errors, as shown by the stability analysis of the Levenberg-Marquardt adjustment. The damping parameter \(\mu_j\) is chosen to satisfy:
$$\mu_j = \min\left(1, \frac{J(\hat{X}_{k,j}) – J(\hat{X}_{k,j+1})}{\| \nabla J(\hat{X}_{k,j}) \|^2}\right)$$
This adaptive tuning enhances convergence speed while maintaining accuracy. In practice, for a drone formation with multiple slave drones, the IUKF algorithm can be extended to a distributed framework, where each drone runs its own filter while exchanging information with neighbors. This scalability makes it suitable for large-scale drone formation operations.
In conclusion, my research on drone formation cooperative navigation presents the Iterative Unscented Kalman Filter as a powerful solution to improve accuracy and convergence. The proposed IUKF method addresses the limitations of traditional nonlinear filters by incorporating iterative measurement updates and Levenberg-Marquardt covariance adjustment. Simulations confirm that IUKF outperforms EKF and UKF in terms of RMSE reduction across all axes, demonstrating its practical value for drone formation applications. The 3D modeling approach adds realism, and the algorithm’s efficiency ensures feasibility for real-time implementation. Future work may explore integration with other sensors or adaptive tuning for dynamic environments. Overall, this study contributes to advancing drone formation technology, enabling more reliable and precise cooperative navigation for complex missions.