A hybrid cooperative navigation method for UAV swarm based on factor graph and Kalman filter

Abstract

Navigation plays an important role in the task execution of the micro-unmanned aerial vehicle (UAV) swarm. The Cooperative Navigation (CN) method that fuses the observation of onboard sensors and relative information between UAVs is a research hotspot. Aiming at the efficiency and accuracy problems of previous studies, this article proposes a hybrid-CN method for UAV swarm based on Factor Graph and Kalman filter. A global Factor Graph is used to combine Global Navigation Satellite System (GNSS) and ranging information to provide position estimations for modifying the distributed Kalman filter; distributed Kalman filter is established on each UAV to fuse inertial information and optimized position estimation to modify the navigation states. In order to provide time-consistent GNSS position information for the Factor Graph, a time synchronization filter is designed. The proposed method is tested and verified using standard Monte Carlo simulations, simulation results show that it can provide a more precise and efficient CN solution than traditional CN methods.

Introduction

Unmanned aerial vehicle (UAV) swarm has received increased attention in military, Internet of things (IoT) due to its potential advantages, such as high efficiency and anti-damage capabilities.^1,2 The Inertial Navigation System (INS) and Global Navigation Satellite System (GNSS) are classic examples of integrated navigation systems and have been widely implemented as mainstream navigation solutions to UAV swarms currently, and they rely on satellites to correct the positioning bias. ³ The main defect of these solutions is that the positioning accuracy of the satellite navigation system will be degraded with the reduction in the number of visible satellites when working in some restricted scenarios. ⁴ Therefore, in some scenarios with high positioning accuracy requirements, ordinary GNSS navigation systems are difficult to meet the needs. By enhancing the positioning performance of UAV swarm through a certain positioning base station and external measurements such as Lidar, cameras and opportunity signals, the real-time kinematic (RTK) technique provides effective ways to solve the navigation problem with UAVs,^5–7 but such applications are limited by strict working scenarios and costs.

To obtain reliable and better navigation performance, cooperative navigation methods have been introduced into the navigation system of UAV swarm to improve the navigation accuracy with the assistance of relative measurements between UAVs. Such measurements include camera, Wi-Fi, Bluetooth, and ultra-wideband (UWB) transceiver.^8–13 Among them, UWB has received extensive attention due to its advantages of high accuracy, strong penetration, powerful anti-interference ability, and moderate layout complexity, compared with other technologies.

The current cooperative navigation methods can be divided into three types: based on Kalman filter, based on optimization algorithms, and based on graph theory. As the most commonly used fusion method, Kalman filter has been widely deployed in the field of cooperative navigation.^14,15 The research in Alam et al. ¹⁶ proposes a cooperative method based on distributed Extend Kalman Filter (EKF), in which any user in the cooperative network can achieve positioning solution by fusing GNSS observations and Dedicated Short Range Communication (DSRC) signals from other users. This method achieves cooperation by fusing the relative observations between the carrier itself and adjacent carriers, and the position and relative information of the non-adjacent carriers is not used in the filter. De Haag et al. ¹⁷ use a centralized Kalman filter to fuse the navigation information of the UAV swarm. Compared with the non-cooperative mode, the performance is significantly improved, but the increase in the number of UAVs will bring a heavy computational burden. To make full use of the absolute position and relative information of the whole swarm, some researchers focus on cooperative navigation technology based on optimization algorithms.^18,19 It builds a set of constraint equations based on the geometric relationship between all UAVs and establishes the corresponding objective function. For this method, the process of cooperative navigation can be regarded as a process where the objective function reaches a predetermined termination condition. However, these cooperative navigation algorithms have given little consideration to the inertial information and shown the defect of error accumulation.

Graph theory has gradually been applied in the navigation field over recent years. For example, the Simultaneous Localization and Mapping (SLAM) technology models the navigation states of the carrier through Factor Graphs, and then on this basis, estimates the trajectory of the carrier through the Maximum A Posterior (MAP) estimation. ²⁰ The Factor Graph is also applicable to the field of cooperative navigation. ²¹ Wymeersch et al. ²² propose a Sum-product Algorithm for Wireless Network (SPAWN), also known as Belief Propagation (BP), which delivers Cooperative Positioning (CP) based on Factor Graph and message passing schedules. The research in Li et al. ²³ presents a Gaussian Belief Propagation algorithm, which regards all the information in the Factor Graph as a Gaussian distribution, so that the message-passing process can be expressed as numerical or matrix multiplication. Although the BP-based CP methods have good performance, many of them are applicable only to static or simple motion scenarios,^22–25 not suitable for complex dynamic systems, while the computational loads and communication traffics are relatively heavy. To overcome these problems, the combination of INS with Factor Graph through auxiliary estimators is a computational efficient way, so as to meet the needs of UAV applications in complex dynamic scenarios.

In this study, a hybrid Cooperative Navigation (hybrid-CN) method in combination of global Factor Graph and distributed Kalman filter is proposed for UAV swarm. This method can integrate the navigation information collected from the onboard INS, GNSS receivers, and inter-UAV ranging equipment. In order to timely synchronize the GNSS information of each UAV entering the Factor Graph, a Time Synchronization (TS) filter based on the main Kalman filter and INS is presented.

The novelty of the article is to propose an algorithm which fuses all GNSS and UWB measurements of the swarm and processing them altogether to give position estimations for updating the distributed Kalman filters. Compared with the traditional factor graph, the calculation complexity is reduced, while the absolute position, relative information, and inertial observation of whole swarm are effectively used.

In addition, simulation results show that the proposed method in this study only needs a few high-precision UAV nodes (UAVs equipped with RTK equipment), thus cutting the cost of UAV swarms.

Problem definition

In this study, each UAV adopts a GNSS receiver and an inertial measurement unit (IMU) as the basic navigation sensor, while carrying a UWB transceiver to provide relative ranging measurements. Some of the UAVs carry RTK modules to obtain high precise navigation information (see Figure 1). All of them carry UWB transceivers to obtain the inter-UAV ranging data and communicate with a leader UAV via a wireless sensor network.

Figure 1.

Cooperative navigation for unmanned aerial vehicle swarm.

Cooperative navigation based on factor graph architecture

The cooperative navigation problem can be formulated as calculating the MAP estimation of the posterior probability of the navigation states over time, given all measurements. ²⁶ Let x denote the navigation states, which is composed of position, velocity, attitude, and calibration parameters (i.e. the biases of accelerometer and gyroscope of UAVs). The navigation variables of UAV swarm can be defined as

$${\mathit{X}}_{0:k}=\{{\mathit{X}}_{0:k}^1,···,{\mathit{X}}_{0:k}^n\}$$

(1)

where $X_{0:k}^i$ denotes the state set and calibration parameters of the i-th UAV up to the current time $t_k$ .

According to these definitions, the joint probability of distribution function (PDF) is given by

$$p(X_{0:k}|Z_{1:k})$$

(2)

where $Z_{1:k}$ represents all the measurements received up to the current time $t_k$ .

Let $p(V_0)$ represent all available prior information, and the joint PDF can be factorized as

$$\begin{array}{c}p(X_{0:k}|Z_{1:k})\propto p(V_0)\underset{t=1}{\overset{k}{\Pi }}\{\underset{i=1}{\overset{n}{\Pi }}[p({\mathit{X}}_t^i|{\mathit{X}}_{t-1}^i,{\mathit{Z}}_{t-1}^{i,\mathrm{IMU}})]\hfill \\ \times \underset{{\mathit{Z}}_j\in Z_t\backslash {\mathit{Z}}_t^{i,\mathrm{IMU}}}{\Pi }p({\mathit{Z}}_j|X_t^j)\}\hfill \end{array}$$

(3)

where $X_t^j\subseteq X_t$ is used to represent the variables involved in the measurement ${\mathit{Z}}_j$ . For example, if ${\mathit{Z}}_j$ represents a ranging measurement between UAV 1 and UAV 2, then $X_t^j$ would be $\{{\mathit{X}}_t^1,{\mathit{X}}_t^2\}$ , while in case of a GNSS measurement, $X_t^j$ is simply $\left\{{\mathit{X}}_t^i\right\}$ .

An all-state Factor Graph $G=(F,X,\mathbf{\epsilon })$ , as shown in Figure 2, can be used to represent the above factorization, ²⁷ which includes two types of nodes: factor nodes $f_k^i\in F$ and variable nodes ${\mathit{X}}_k^i\in X$ . The edge ${\mathit{e}}_k^{i,j}\in {\mathbf{\epsilon }}_k$ only exists between factor nodes and variable nodes. The set of variable nodes $X$ contains the navigation states and calibration parameters of the whole UAV swarm, with each factor representing an individual term in the factorization (3). So the PDF can be rewritten as

$$p(X_{0:k}|Z_{1:k})\propto \underset{i}{\Pi }\underset{k}{\Pi }{f}_k^i(X_t^i)$$

(4)

Figure 2.

All-state factor graph for a cooperative navigation system.

In the cooperative scenarios, the factor nodes of Factor Graph can be divided into four types: prior factor, IMU factor, UWB factor and GNSS factor. In a Gaussian noise distribution, each factor that represents a measurement model can be expressed in the following form

$$f_k^i(X_t^i)=\exp (-\frac{1}{2}\Vert h(X_t^i)-{\mathit{Z}}_t^i{\Vert }_{{\Sigma }_i}^2)$$

(5)

where $h(·)$ is a measurement function and $\Vert e{\Vert }_{\Sigma }^2=e^T{\Sigma }^{-1}e$ is defined as the squared Mahalanobis distance with covariance matrix $\Sigma$ .

The MAP estimation of navigation states is given by

$$\begin{array}{c}\multicolumn{1}{c}{X_{0:k}^{*}=\arg \underset{X_{0:k}}{max}p(X_{0:k}|Z_{1:k})}\\ \multicolumn{1}{c}{=\arg \underset{X_{0:k}}{min}(-\log (\underset{i}{\Pi }\underset{k}{\Pi }{f}_k^i(X_t^i)))}\\ \multicolumn{1}{c}{=\arg \underset{X_{0:k}}{min}\frac{1}{2}\sum _i\sum _k\Vert h(X_t^i)-{\mathit{Z}}_t^i{\Vert }_{{\Sigma }_i}^2}\end{array}$$

(6)

In order to produce the MAP estimation given in (6), each UAV sends IMU data, GNSS data and UWB information to a leader UAV for centralized processing. The INS solution of each UAV and the optimization of all navigation states will also be completed by the leader UAV. Therefore, the communication traffics and computation loads are quite huge.

Improved factor graph architecture

To ease the communication loads and computation complexities, this study simplifies the all-state Factor Graph shown in Figure 2, and uses the variable nodes only composed of position states at the current time to form the simplified Factor Graph together with GNSS factors and UWB factors, as shown in Figure 3. The recursive process from ${\mathit{X}}_{k-1}^i$ to ${\mathit{X}}_k^i$ , which represents the posterior probability $p({\mathit{X}}_t^i|{\mathit{X}}_{t-1}^i,{\mathit{Z}}_{t-1}^{i,\mathrm{IMU}})$ , is taken over by Kalman filter, so that the INS solution can also be completed by the onboard processor of each UAV. In this way, a centralized position Factor Graph (PFG) and distributed Kalman filters can be derived as shown in Figure 4, where $1{\alpha }_k^{n,\mathrm{FG}}$ is the position estimation provided by the simplified PFG for correcting the Kalman filter of the n-th UAV; $1X_k^{n,-}$ and $1X_k^{n,+}$ are the corresponding state vector after the time update and measurement update of Kalman filter, respectively. Because only the GNSS information and UWB ranging measurements participate in communication, and the recursion of variable nodes in the Factor Graph is assigned to each UAV for distributed processing, the communication traffics and computation loads are greatly reduced.

Figure 3.

Simplified factor graph for a cooperative navigation system.

Figure 4.

Hybrid cooperative navigation based on simplified factor graph and distributed Kalman filter.

However, a new problem arises after the all-state centralized Factor Graph is replaced with PFG and distributed Kalman filters. For the GNSS receiver of UAV swarm, the solution time of each receiver is not completely consistent in each positioning epoch. Different from the all-state Factor Graph which can realize the plug-and-play of GNSS factor, the PFG is established with the current position state as the variable node, so it will not be able to add the GNSS factor when the GNSS solution time is not synchronized. To solve this problem, a time synchronization filter implemented in each UAV is proposed, which can recur in the GNSS data in each epoch, until each UAV completes the GNSS solution in that epoch.

Here, the cooperative navigation problem of UAV swarm can be considered as a MAP estimation problem about navigation states, which can be solved with the Factor Graph. In order to solve the problem of heavy computation and communication, the all-state Factor Graph is decomposed into a centralized PFG, which will be processed by the leader UAV and distributed Kalman filters, and a time synchronization filter, which will be processed internally by each UAV.

Hybrid cooperative navigation

The architecture of the proposed hybrid-CN method in this study is illustrated in Figure 5, which mainly consists of two parts. The first part establishes the global PFG to centrally optimize the position states of UAV swarm, and this task is completed by the leader UAV. The second part is composed of the internal sensor information calculation by the UAV and two filters and performed internally by each UAV. The global PFG is used to fuse the GNSS position and UWB ranging observations of the UAV swarm to provide position estimates for correcting the distributed main Kalman filters, the Time Synchronization filter provides time-consistent GNSS position and covariance information for the PFG, the distributed main Kalman filter fuse the inertial information and the position estimation from the PFG to update the navigation states.

Figure 5.

Architecture of the hybrid cooperative navigation method.

Position factor graph

By combining the GNSS information of each UAV and the UWB ranging measurements, PFG can be established on the leader UAV. The position variables of UAV swarm is defined as

$$\begin{array}{c}\multicolumn{1}{c}{\mathit{\chi }={\left\{{\mathit{\alpha }}_i\right\}}_{i=1}^n}\\ \multicolumn{1}{c}{{\mathit{\alpha }}_i={\left[\begin{array}{ccc}{\alpha }_i^E\hfill & {\alpha }_i^N\hfill & {\alpha }_i^U\hfill \end{array}\right]}^T}\end{array}$$

(7)

where $\mathit{\chi }$ is the variable set of positions; ${\mathit{\alpha }}_i$ is the position variable of UAV i; ${\alpha }_i^E$ , ${\alpha }_i^N$ and ${\alpha }_i^U$ are the three-dimensional positions in the East-North-Up reference frame.

Then, the posterior probability distribution of UAV swarm can be expressed as follows

$$p(\mathit{\chi }|\mathit{\beta })$$

(8)

where $\mathit{\beta }$ is the observation set of GNSS position and UWB range of the whole UAV swarm.

Based on the definition above, the posterior probability distribution can be defined as ²⁸

$$p(\mathit{\chi }|\mathit{\beta })\propto \underset{m}{\Pi }{\mathrm{f}}^m({\mathit{\alpha }}_{i,j})$$

(9)

where ${\mathit{\alpha }}_{i,j}$ is the variable related to factor node $f^m$ in the variable set $\mathit{\chi }$ , m is the number of GNSS position or UWB ranging observation. As in equation (5), it can be assumed that each factor has the following form

$$f^m({\mathit{\alpha }}_{i,j})=\exp \{-\frac{1}{2}\Vert h^m({\mathit{\alpha }}_{i,j})-{\mathit{\beta }}^m{\Vert }_{{\Sigma }^m}^2\}$$

(10)

where $h^m(·)$ is the measurement function, and ${\mathit{\beta }}^m$ is the corresponding observation.

Each factor represents a cost function that should be minimized by adjusting the estimates of the variable $\mathit{\chi }$ . For the scenario of cooperative navigation in this study, the PFG mainly includes two kinds of factor nodes: GNSS factor and UWB ranging factor.

• 1. GNSS Factor: The GNSS measurements can be modeled as follows^29,30

$${\mathit{p}}_i^{\mathrm{GNSS}}=\mathit{p}+{\mathit{n}}_{\mathrm{GNSS}}$$

(11)

where $\mathit{p}$ is the actual position, and ${\mathit{n}}_{\mathrm{GNSS}}$ is the measurement noise. Therefore, GNSS factor can be expressed as

$$f_{\mathrm{GNSS}}({\mathit{\alpha }}_i)=\exp \{-\frac{1}{2}\Vert {\mathit{\alpha }}_i-{\mathit{p}}_i^{\mathrm{GNSS}}{\Vert }_{{\Sigma }^i}^2\}$$

(12)

• 2. UWB Ranging Factor: UWB equipment is widely used in communication and ranging systems, thanks to its high data transmission rate and high ranging accuracy.^31–33 The UWB ranging observation of UAV i and UAV j can be modeled as

$$d_{i,j}^{\mathrm{uwb}}=d_{i,j}+b_r+n_{\mathrm{uwb}}$$

(13)

where $d_{i,j}$ is the actual distance between two UAVs, $b_r$ is the bias caused by Non-Line-of-Sight (NLOS), and $n_{\mathrm{uwb}}$ is the ranging noise of UWB transceivers. In a normal case, the measurement errors used in the proposed method are assumed always to be normal distributed. For the faulty cases caused by NLOS or multipath, such faulty measurements can be removed by some integrity methods. ³⁴ The observation function is

$$h({\mathit{\alpha }}_{i,j})=\sqrt{{({\alpha }_i^E-{\alpha }_j^E)}^2+{({\alpha }_i^N-{\alpha }_j^N)}^2+{({\alpha }_i^U-{\alpha }_j^U)}^2}$$

(14)

Therefore, UWB ranging factor can be expressed as

$$f_{\mathrm{uwb}}({\mathit{\alpha }}_{i,j})=\exp \{-\frac{1}{2}\Vert h({\mathit{\alpha }}_{i,j})-d_{i,j}^{\mathrm{uwb}}{\Vert }_{{\Sigma }_{\mathrm{uwb}}}^2\}$$

(15)

By taking the logarithm of equation (9), the MAP estimate ${\mathit{\chi }}^{\mathrm{MAP}}$ can be obtained by

$$\begin{array}{c}\multicolumn{1}{c}{{\mathit{\chi }}^{\mathrm{MAP}}=\arg maxp(\mathit{\chi }|\mathit{\beta })}\\ \multicolumn{1}{c}{=\arg min-\log (\underset{m}{\Pi }{f}^m({\mathit{\alpha }}_{i,j}))}\\ \multicolumn{1}{c}{=\arg \underset{\mathit{\chi }}{min}\sum _m\Vert h^m({\mathit{\alpha }}_{i,j})-{\mathit{\beta }}^m{\Vert }_{{\Sigma }^m}^2}\end{array}$$

(16)

In practice, the linearized version (equation (16)) of the problem is generally considered. For the measurement function in equation (10), Gauss–Newton or Levenberg–Marquardt algorithm can be used to solve its linear approximation of equation (16), ²⁶ so as to approach the minimum. The linearized measurement function of factor node is

$$\begin{array}{c}{h}^m({\mathit{\alpha }}_{i,j})=h^m({\mathit{\alpha }}_{i,j}^0+\mathrm{\Delta }{\mathit{\alpha }}_{i,j}^m)\hfill \\ \approx h^m({\mathit{\alpha }}_{i,j}^0)+{\mathit{H}}^m\mathrm{\Delta }{\mathit{\alpha }}_{i,j}^m\hfill \end{array}$$

(17)

where ${\mathit{\alpha }}_{i,j}^0$ is the linearization point, ${\mathit{H}}^m$ is the observation Jacobian matrix at a given linearization point, and $\mathrm{\Delta }{\mathit{\alpha }}_{i,j}^m$ is the state update vector.

By substituting equation (17) into equation (16), the state update vector of the PFG is

$$\begin{array}{c}\multicolumn{1}{c}{\mathrm{\Delta }{\mathit{\alpha }}^{*}=\arg \underset{\mathrm{\Delta }}{min}\sum _i\Vert h^m({\mathit{\alpha }}_{i,j}^0)+{\mathit{H}}^m\mathrm{\Delta }{\mathit{\alpha }}_{i,j}^m-{\mathit{\beta }}^m{\Vert }_{{\Sigma }^m}^2}\\ \multicolumn{1}{c}{=\arg \underset{\mathrm{\Delta }}{min}\sum _i\Vert {\mathit{H}}^m\mathrm{\Delta }{\mathit{\alpha }}_{i,j}^m-\{{\mathit{\beta }}^m-h^m({\mathit{\alpha }}_{i,j}^0)\}{\Vert }_{{\Sigma }^m}^2}\end{array}$$

(18)

where ${\mathit{\beta }}^m-h^m({\mathit{\alpha }}_{i,j}^0)$ is the prediction error at linearization point, and $\mathrm{\Delta }{\mathit{\alpha }}^{*}$ is the solution of the local linear problem. In order to eliminate the covariance matrix ${\Sigma }^m$ , the Jacobian matrix and the prediction error in the above equation need to be whitened, and the whitening method is as follows

$$\begin{array}{c}\multicolumn{1}{c}{A^{1}m=({\Sigma }^{1}m{)}^{-1/2}{H}^{1}m}\\ \multicolumn{1}{c}{b^{1}m=({\Sigma }^{1}m{)}^{-1/2}({\mathit{\beta }}^{1}m-{\mathrm{h}}^{1}m({\mathit{\alpha }}_{i,j}^0))}\end{array}$$

(19)

The process of whitening eliminates the unit of observation, so the cost functions represented by different factor nodes can be combined into a single cost function. After the Jacobian matrix and the prediction error are whitened, the following standard least squares problem can be obtained

$$\begin{array}{c}\multicolumn{1}{c}{\mathrm{\Delta }{\mathit{\alpha }}^{*}=\arg \underset{\mathrm{\Delta }}{min}\sum _{1}m{\Vert A^{1}m\mathrm{\Delta }{\mathit{\alpha }}_{1i,j}^{1}m-b^{1}m\Vert }^2}\\ \multicolumn{1}{c}{=\arg \underset{\mathrm{\Delta }}{min}\Vert A\mathrm{\Delta }\mathit{\alpha }-b{\Vert }^2}\end{array}$$

(20)

where $A$ and $b$ are the large Jacobian matrix and prediction error formed by combining the Jacobian matrix $A^{1}m$ and prediction error $b^{1}m$ accordingly.

Once $\mathrm{\Delta }{\mathit{\alpha }}^{*}$ is obtained, the new estimate is given by $\mathrm{\Delta }{\mathit{\alpha }}_{1i,j}^{*}+{\mathit{\alpha }}_{1i,j}^{1}0$ , which is used as the linearization point in the next Gauss–Newton or Levenberg–Marquardt iteration. The probability density on $\mathrm{\Delta }\mathit{\alpha }$ defined by the PFG is

$$p(\mathrm{\Delta }\mathit{\alpha })\propto e^{-\log \left(\mathrm{\Delta }\mathit{\alpha }\right)}=\exp \{-\frac{1}{2}{\Vert \mathit{A}\mathrm{\Delta }\mathit{\alpha }-\mathit{b}\Vert }^2\}$$

(21)

QR matrix factorization can be applied to the measurement Jacobian to solve the normal equation ${\mathit{A}}^T\mathit{A}\mathrm{\Delta }\mathit{\alpha }={\mathit{A}}^T\mathit{b}$ , thus avoiding calculating the information matrix ${\mathit{A}}^{T}A$ with the associated squaring of matrix condition number.^35–37 The specific process can be seen in Kaess et al. ³⁵

After the optimization of the PFG, the position error covariance can be obtained from the following equation:

$$\mathrm{cov}(\mathrm{\Delta }\mathit{\alpha })=(({\mathit{R}}^T\mathit{R}{)}^{-1}{\mathit{R}}^T{\mathit{Q}}^T)\mathrm{cov}(\mathit{b})(({\mathit{R}}^T\mathit{R}{)}^{-1}{\mathit{R}}^T{\mathit{Q}}^T{)}^T$$

(22)

where Q and R are the orthogonal matrix and the upper triangular matrix after QR decomposition for matrix A, respectively.

The position error covariance ${\mathit{P}}_i^{\mathrm{PFG}}$ of each UAV can be obtained by splitting the corresponding part of the total covariance matrix.

Main Kalman filter

The distributed main Kalman filter is used to fuse the inertial information of UAV and the position optimized by PFG. According to the analysis in Xiong et al. ⁴ and Kamilov et al., ³⁸ when the noise of input satisfies the zero mean Gaussian distribution, the estimation of Factor Graph will also follow the Gaussian distribution, and their mean will converge to the true value after a certain iteration. In other words, the estimated position of the PFG follows the Gaussian distribution, which forms the basis for Kalman filter fusion.

The INS error equation is used as the state equation. In this study, the east-north-up geographic coordinate frame is selected as the navigation coordinate frame. Therefore, the state of the main KF is defined as ³⁹

$$\begin{array}{c}\multicolumn{1}{c}{\mathit{X}(k)=[{\phi }_E,{\phi }_N,{\phi }_U,\delta v_E,\delta v_N,\delta v_U,\delta L,\delta \lambda ,\delta h,}\\ \multicolumn{1}{c}{{\epsilon }_{\mathrm{bx}},{\epsilon }_{\mathrm{by}},{\epsilon }_{\mathrm{bz}},{\epsilon }_{\mathrm{rx}},{\epsilon }_{\mathrm{ry}},{\epsilon }_{\mathrm{rz}},{\nabla }_x,{\nabla }_y{,{\nabla }_z]}^T}\end{array}$$

(23)

where ${\phi }_E$ , ${\phi }_N$ , and ${\phi }_U$ are the misalignment angle errors; $\delta v_E$ , $\delta v_N$ , and $\delta v_U$ are the east, north, and up velocity errors, respectively; $\delta L$ , $\delta \lambda$ , and $\delta h$ are the latitude, longitude, and altitude errors, respectively; ${\epsilon }_{\mathrm{bx}}$ , ${\epsilon }_{\mathrm{by}}$ , ${\epsilon }_{\mathrm{bz}}$ , ${\epsilon }_{\mathrm{rx}}$ , ${\epsilon }_{\mathrm{ry}}$ , and ${\epsilon }_{\mathrm{rz}}$ are gyro constant drift errors and first-order Markov drift errors, respectively; and ${\nabla }_x$ , ${\nabla }_y$ , and ${\nabla }_z$ are the accelerometer biases.

The state equation of the system is ²⁹

$$\stackrel{·}{\mathbf{X}}=\mathit{FX}+\mathit{GW}$$

(24)

where $\mathit{F}$ is the linearized INS error states matrix, $\mathit{G}$ is the noise transfer matrix, and $\mathit{W}$ is the system process noise, which is determined by the parameters of the accelerometer and gyro and has zero mean multivariate normal distribution with variance $\mathit{Q}$ .

The following observation is the difference between the position information optimized by PFG and the corresponding position provided by the INS

$$\begin{array}{c}\multicolumn{1}{c}{{\mathit{Y}}_{\mathrm{FG}}={\mathit{H}}_{\mathrm{kalman}}\mathit{X}+{\mathit{V}}_{\mathrm{FG}}}\\ \multicolumn{1}{c}{=[0_{3\times 6}\mathrm{diag}[R_M{R}_N\cos L1]0_{3\times 9}]\mathit{X}+{\mathit{V}}_{\mathrm{FG}}}\end{array}$$

(25)

where $L$ is the latitude of the UAV; $R_M$ and $R_N$ are curvature radii of the reference ellipsoid in longitude and latitude circles of the earth, respectively; and ${\mathit{V}}_{\mathrm{FG}}$ is the noise of the position error provided by PFG with variance matrix ${\mathit{P}}_{\mathrm{FG}}^i$ . The observation vector is

$${\mathit{Y}}_{\mathrm{FG}}={\mathit{\alpha }}_{\mathrm{FG}}-{\mathit{\alpha }}_{\mathrm{ins}}$$

(26)

where ${\mathit{\alpha }}_{\mathrm{FG}}$ is the position estimated by the PFG, and ${\mathit{\alpha }}_{\mathrm{ins}}$ is the output position of INS.

The procedure of the main Kalman filter can be divided into two steps.

• 1. Prediction: With the recursive process of INS, the prediction of the main Kalman filter is implemented before the PFG information comes. The relevant equations are

$${\hat{\mathit{X}}}_i(t|t-1)={\mathit{F}}_i(t,t-1){\hat{\mathit{X}}}_i(t-1|t-1)$$

(27)

$$\begin{array}{c}{\mathit{P}}_i(t|t-1)={\mathit{F}}_i(t,t-1){\mathit{P}}_i(t-1|t-1){\mathit{F}}_i(t,t-1{)}^T\hfill \\ +{\mathit{G}}_i(t-1){\mathit{Q}}_i(t-1){\mathit{G}}_i(t-1{)}^T\hfill \end{array}$$

(28)

where ${\hat{\mathit{X}}}_i(t|t-1)$ and ${\mathit{P}}_i(t|t-1)$ are the predicted system state and covariance, respectively; ${\hat{\mathit{X}}}_i(t-1|t-1)$ and ${\mathit{P}}_i(t-1|t-1)$ are the system state and covariance estimated at the last epoch, respectively; and ${\mathit{G}}_i(t-1)$ is the white noise error transfer matrix of the INS system.

• 2. Measurement update: The relevant equations include

$$\begin{array}{c}{\mathit{K}}_i(t)={\mathit{P}}_i(t|t-1){\mathit{H}}_{\mathrm{kalman}}(t{)}^T\hfill \\ ({\mathit{H}}_{\mathrm{kalman}}(t){\mathit{P}}_i(t|t-1){\mathit{H}}_{\mathrm{kalman}}(t{)}^T+{\mathit{P}}_i^{\mathrm{PFG}}{)}^{-1}\hfill \end{array}$$

(29)

$$\begin{array}{c}{\hat{\mathit{X}}}_i(t|t)={\hat{\mathit{X}}}_i(t|t-1)+\hfill \\ {\mathit{K}}_i(t)({\mathit{Y}}_i(t)-{\mathit{H}}_{\mathrm{kalman}}(t){\hat{\mathit{X}}}_i(t|t-1))\hfill \end{array}$$

(30)

$${\mathit{P}}_i(t|t)=(\mathit{I}-{\mathit{K}}_i(t){\mathit{H}}_{\mathrm{kalman}}(t)){\mathit{P}}_i(t|t-1)$$

(31)

where ${\hat{\mathit{X}}}_i(t|t)$ and ${\mathit{P}}_i(t|t)$ are the estimated system state and covariance at the current epoch, respectively; ${\mathit{K}}_i(t)$ is the filter gain; and ${\mathit{Y}}_i(t)$ is the position observation vector of the corresponding UAV as calculated by equation (26). ${\hat{\mathit{X}}}_i(t|t)$ can be used to modify the navigation information of the INS.

Time synchronization filter

Given that the output time of each set of GNSS equipment is not necessarily synchronized, the information on GNSS positions cannot be directly sent to the leader UAV, especially in a dynamic scene, because such a method will lead to position bias. With the advantage of short-time accuracy, the INS can be used to realize the recurrence of positions and position uncertainty of GNSS, so as to synchronize the position information sent to the leader UAV. This recursive process is completed by the prediction of Kalman filter. The state equation of the TS filter can also be defined by equation (23).

Figure 6 shows the synchronization procedure after acquisition of the GNSS position. First, the attitude and velocity values are taken out from the INS and then put into the sub-INS with the GNSS position ${\mathit{\alpha }}_i^{\mathrm{GNSS}-}$ . At each epoch, the sub-INS takes ${\mathit{\alpha }}_i^{\mathrm{GNSS}-}$ as the initial value and combines the IMU data to recursively update the GNSS position, thereby realizing the time synchronization of the position; furthermore, the states and covariance, except the position, are taken from the main KF and are combined with the GNSS position error covariance ${\mathit{R}}_{\mathrm{GNSS}}$ to initialize the TS filter. In other words, the errors in attitude, velocity, and IMU in the initial state of the TS filter are set to the corresponding state of the main KF, and the initial position error state is set to 0. In the same way, the covariance matrix of the TS filter is initialized by the covariance matrix of the main KF, but the position covariance should be replaced by ${\mathit{R}}_{\mathrm{GNSS}}$ . After the TS filter is initialized, the prediction process will be completed according to equations (27) and (28) to predict the position error. After all UAVs receive the GNSS position information, the leader UAV will send out instructions to allow the UAV swarm to communicate and range; afterward, the synchronized GNSS position ${\mathit{\alpha }}_i^{\mathrm{GNSS}+}$ , the position covariance ${\mathit{P}}_i^{\mathrm{GNSS}+}$ of each UAV, and all UWB measurements will be sent to the leader UAV to establish the global PFG.

Figure 6.

Synchronization procedure.

Algorithm description

Figure 7 shows the procedure of the hybrid cooperative navigation method, which can be divided into the following steps:

• Step 1. Initialize the INS and main Kalman filter of each UAV in the UAV swarm, and set up the initial position ${\mathit{\alpha }}_i^0$ , velocity ${\mathit{v}}_i^0$ , attitude ${\mathit{a}}_i^0$ , state ${\hat{\mathit{X}}}_i(0)$ , and covariance matrix ${\mathit{P}}_i^0$ of the main Kalman filter.

• Step 2. Use the IMU information for INS recursion, update the state ${\hat{\mathit{X}}}_i(t|t-1)$ and covariance ${\mathit{P}}_i(t|t-1)$ of the main Kalman filter with equations (27) and (28), and wait for the arrival of GNSS information.

• Step 3. When the navigation system receives GNSS position at t₁, the attitude and velocity of INS are taken out and combined with GNSS position to establish a sub-INS; meanwhile, the initial state of the time synchronization filter is set as follows

$$\begin{array}{c}{\hat{\mathit{X}}}_{\mathrm{ts},i}(0)=[{\phi }_{E,i}^{t_1},{\phi }_{N,i}^{t_1},{\phi }_{U,i}^{t_1},\delta v_{E,i}^{t_1},\delta v_{N,i}^{t_1},\delta v_{U,i}^{t_1},0,0,0,\hfill \\ {\epsilon }_{\mathrm{bx},i}^{t_1},{\epsilon }_{\mathrm{by},i}^{t_1},{\epsilon }_{\mathrm{bz},i}^{t_1},{\epsilon }_{\mathrm{rx},i}^{t_1},{\epsilon }_{\mathrm{ry},i}^{t_1},{\epsilon }_{\mathrm{rz},i}^{t_1},{\nabla }_{x,i}^{t_1},{\nabla }_{y,i}^{t_1}{,{\nabla }_{z,i}^{t_1}]}^T\hfill \end{array}$$

(32)

where the superscript t₁ denotes the state of the main Kalman filter at time t_1. It can be seen from equation (32) that the initial position error state of the time synchronization filter is set to 0. Similarly, the initial covariance matrix ${\mathit{P}}_{\mathrm{ts},i}(0)$ of the time synchronization filter is formed by replacing the position error covariance part in the covariance matrix ${\mathit{P}}_i(t|t-1)$ of the main KF with ${\mathit{R}}_{\mathrm{GNSS}}$ .

• Step 4. Carry out the recurrence of INS and Sub-INS and the prediction of the two filters simultaneously, until all UAVs receive GNSS information.

• Step 5. After the UAV swarm receives the GNSS position information, each UAV will send the time-synchronized position and position covariance to the leader UAV, which will then build up a global PFG.

• Step 6. Use the PFG to optimize the position of the UAV swarm, the optimized position ${\mathit{\alpha }}_i^{\mathrm{PFG}}$ , and the position error covariance ${\mathit{P}}_i^{\mathrm{PFG}}$ , all of which can be sent back to each UAV.

• Step 7. Use ${\mathit{\alpha }}_i^{\mathrm{PFG}}$ and ${\mathit{P}}_i^{\mathrm{PFG}}$ to achieve measurement updates of the main Kalman filter, and modify the navigation parameters of the INS by ${\hat{\mathit{X}}}_i(t|t)$ . And then go back to Step 2 for the next epoch.

Figure 7.

Procedure for hybrid navigation method.

Analysis of computation complexity and communication load

For the proposed hybrid-CN method, the computation load of the leader UAV is derived from the Factor Graph. This load is mainly related to the time spent in iteratively solving the optimization problem converted from the Factor Graph. In this way, the computation load is $O(I_G(M_G+N_G))$ for $I_G$ iterations, where $M_G$ is the set of position measurements, and $N_G$ is the set of ranging measurements for UAV swarm. For the traditional CN method based on sum-product algorithm over a Factor Graph, such as SPAWN, the computation burden is dominated by the message-passing process between nodes for each particle. The SPAWN computation load for UAV j which requires $I_U$ iterations and N particles is $O(I_{U}N|M_j{|}^2)$ , where M_j is the set of measurements related to UAV j. It can be seen that the proposed method can deliver lower computation complexity than the SPAWN algorithm.

In terms of communication traffics, for the proposed method, the UAVs only need to communicate the position estimates and the corresponding covariance matrix with the leader UAV, so the communication load is $12N_S$ , where $N_S$ is the number of UAVs in the swarm. For SPWAN, the communication overhead as calculated in Georges et al. ²⁴ is $3N{I}_U$ . Therefore, the communication overhead of the method proposed in this study is lower than the SPWAN method, which requires iterative interactions with a large number of particles.

Simulation results

Simulation configuration

Based on the background of cooperative scenarios, an UAV swarm system consisting of 12 UAVs is simulated to verify the proposed hybrid-CN method. To fully assess the performance of this method, all the results in this section are based on 200 Monte Carlo simulation trials.

Each of the UAVs carries an INS. Nine of them are equipped with ordinary GNSS receivers, and the other three are with RTK equipment. UWB is used as a relative measurement solution for UAV swarm, and all UAVs can communicate the navigation data to the leader UAV, so that UWB can measure the distance with peers and find out whether such distance is lower than a certain value. The initial position and simulated trajectories in the horizontal direction of all UAVs are shown in Figures 8 and 9. All comparisons in this section are based on the navigation results of UAVs equipped with ordinary GNSS.

Figure 8.

The initial position of unmanned aerial vehicle swarm.

Figure 9.

Trajectories of unmanned aerial vehicle swarm in horizontal direction.

The output frequency of the INS is 100 Hz, and the parameters used in the simulation are shown in Table 1. ⁴ The measurement errors of the GNSS and UWB are considered as Gaussian noise, with the parameters shown in Table 2.^39,40 The UWB link of UAV 1 is shown in Table 3. The output frequencies of these sensors are both 1 Hz. The solution time difference of UAV swarm in each GNSS epoch is randomly set to 0–0.2 s.

Table 1.

Inertial measurement unit parameters.

Sensor	Parameter	Value
Gyro	Constant drift	10°/h
	White noise error	10°/h
	First-order Markov drift error	10°/h
	First-order Markov time	3600s
Accelerometer	First-order Markov drift error	0.001 g
	First-order Markov time	1800 s

Table 2.

GNSS and UWB parameters.

Sensor	Parameter	Noise density
Ordinary GNSS	East position error	10 m
	North position error	10 m
	Height error	20 m
RTK	East position error	0.2 m
	North position error	0.2 m
	Height error	0.5 m
UWB	Distance error	0.3 m

GNSS: Global Navigation Satellite System; RTK: real-time kinematic; UWB: ultra-wideband.

Table 3.

The UWB link of UAV 1.

Number	The number of the UAV connectedto UAV 1 via UWB link
1	2, 3, 4, 8, 11, 12

UWB: ultra-wideband; UAV: unmanned aerial vehicle.

The positioning estimation errors are calculated by

$$\mathrm{Err}=\sqrt{{\mathrm{Err}}_E^2+{\mathrm{Err}}_N^2+{\mathrm{Err}}_U^2}$$

(33)

where $\mathrm{Er}{r}_E$ , $\mathrm{Er}{r}_N$ , and $\mathrm{Er}{r}_U$ are the estimated errors of positions in three directions, that is, east, north, and up, respectively.

Comparison of different cooperative navigation methods

To compare the performance of the proposed hybrid-CN method with other methods, several cooperative navigation algorithms are simulated in this section. The positioning error cumulative distribution functions (CDF) is used as the performance metric.

Figure 10 demonstrates the comparison results among CP-EKF, ¹⁶ MDS Filter, ⁴¹ SPAWN, ²² and Non-CN, where Non-CN is adopted as the performance baseline. The details of each CN algorithm are described as follows:

1. Hybrid-CN: The proposed CN method in this study.

2. Non-CN: INS/GNSS integrity system without cooperation. ⁴² The loosely coupled INS/GNSS system in the non-CN case is based on the Kalman filter, which only uses INS and GNSS data, without considering UWB measurement. The estimated state, system model and observation model of the INS/GNSS integrity system can also be defined as equations (23)–(25). Different from the proposed method of this study, the observation of the INS/GNSS integrated navigation in non-CN case is the difference between GNSS position and INS position.

3. CP-EKF: Unlike the Non-CN method, CP-EKF not only uses local data, but also fuses measurements of other collaborators based on EKF. ¹⁶ The estimated states and system model are the same as equations (1) and (2), but take the cooperative measurements from inter-UAV ranging into consideration in the observation model. It should be noted that the noise covariance in the measurement model considers not only the UWB ranging error, but also the position error of the collaborator.

4. MDS Filter: This as a cooperative positioning method based on the multidimensional scaling (MDS) and the maximum likelihood estimation. ⁴¹ The procedure of MDS filter can be divided into three steps:

   Step 1: Obtain the non-cooperative priori position estimation and position estimation covariance from each UAV;

   Step 2: Carry out MDS computation based on the stress function in each iteration, and use the inter-UAV UWB measurements, until the terminal threshold is met;

   Step 3: Estimate the covariance of cooperative positioning results and apply it to a ML filter to make further estimation.

5. SPAWN: This is a sum-product algorithm for wireless network. ²² It is a typical positioning algorithm based on belief propagation, which uses the marginal distribution to estimate the navigation states. This method is widely considered to have superior performance. The core principle of this algorithm is message passing, and the positioning accuracy depends on the number of samples. In the simulations of this section, the importance sampling method in Sudderth et al. ⁴³ is adopted to obtain the marginal distribution. The number of samples is set to 400.

Figure 10.

CDF comparison between several methods (positioning errors collected from 200 trials).

It can be seen from Figure 10 that hybrid-CN delivers the similar positioning performance to SPAWN, outperforming other methods. Quantitative analyses show that the probability of positioning error less than 1.5 m (P (error < 1.5 m)) is 0.971, 0.964, 0.314, and 0.231 for hybrid-CN, SPAWN, MDS, and CP-EKF, respectively. And it can be found that the positioning accuracy of the CP-EKF method is just higher than that of Non-CN, because only the constraint information of adjacent UAV nodes is used.

To compare the navigation performance under the several cases more clearly, the position estimation root mean square error (RMSE) of UAV 1 in the three directions are shown in Table 4. As shown in Table 4, the positioning accuracy of UAV 1 in the hybrid-CN case is improved by more than 10 times compared with the Non-CN case.

Table 4.

Comparison of position errors between several CN algorithms for UAV 1.

Method	Direction	RMSE(m)
Hybrid-CN	East	0.35
	North	0.45
	Up	0.56
Non-CN	East	5.88
	North	5.70
	Up	9.56
CP-EKF	East	3.58
	North	2.69
	Up	3.95
MDS Filter	East	1.87
	North	1.76
	Up	2.04
SPAWN	East	0.36
	North	0.43
	Up	0.48

CN: Cooperative Navigation; UAV: unmanned aerial vehicle; CP: Cooperative Positioning; EKF: Extend Kalman Filter; MDS: multidimensional scaling; SPAWN: Sum-Product Algorithm for Wireless Network.

In order to compare the computation efficiency of several methods, Figure 11 shows the computation loads of several methods in a simulation experiment, and the processing time for each epoch obtained with Matlab “tic” and “toc” functions is used as the performance metric.

Figure 11.

Comparison of processing time between the methods of CP-EKF, MDS, SPAWN, and hybrid-CN.

As shown, the computation load of SPAWN is much higher than that of other methods, which require a large number of particles to approximate the posterior probability. For MDS, the priori positioning error of each UAV has an impact on the computation efficiency, indicating that a large priori positioning error requires more iterations. CP-EKF has the lowest computational complexity and the shortest processing time, because it only uses EKF to fuse the relative measurements of adjacent UAVs. For hybrid-CN, the computation load mainly lies in the calculation of PFG and the two Kalman Filter. It can be seen from Figure 11 that its processing time is less than that of SPAWN and MDS.

Comparison of different scales of UAV swarms for the hybrid cooperative navigation method

The navigation accuracy for a single UAV in the UAV swarm is also related to the number of neighboring UAV nodes, so the performance of the hybrid-CN method should be discussed for different scales of UAV swarms.

Given that no UAV is equipped with RTK, Figure 12 shows the performance of the proposed method in different scales of UAV swarms. It can be found that even without the assistance of RTK, the positioning accuracy of the UAV swarm can be improved through the cooperative solution, and the hybrid-CN method with 15 UAVs would deliver the best navigation performance. Also, it is identified that as the scale of UAV swarm grows, the cooperative navigation performance gain is decreasing.

Figure 12.

Positioning error CDF comparison in different unmanned aerial vehicle (UAV) swarm scales: 6, 9, 12, and 15 UAVs.

The number of UAV nodes equipped with high-precision RTK in the swarm also affects the navigation performance. Figure 13 presents a navigation performance comparison of a swarm of 12 UAVs in different UAV nodes equipped with RTK equipment. Obviously, the positioning accuracy is improved significantly with the increased number of high-precision UAVs when the number is below three. On the other hand, it can be seen that the cooperative performance gain is dramatically decreased with the growing number of high-precision UAVs when the number is above three. The degrading of cooperative performance gain is caused by the decay of cooperative position error bound.^44,45

Figure 13.

Comparison of positioning error CDF under different numbers of unmanned aerial vehicles (UAVs) equipped with real-time kinematic devices in a UAV swarm.

Conclusion

This study proposes a hybrid navigation method for UAV swarm based on Factor Graph and Kalman filter. This method can fuse the INS and GNSS information of each UAV with the relative ranging measurements between UAVs cooperatively. By taking only positions as the optimized state of Factor Graph and utilizing Kalman filtering to replace the time recursion process of the variable nodes, the decomposition of the all-state Factor Graph is delivered, thus improving the navigation performance of UAV swarms and reducing the computation burdens and communication loads.

Simulation results demonstrate that the hybrid-CN method can bring an obvious improvement in the navigation accuracy compared with non-cooperative cases, regardless of whether the UAVs are equipped with RTK devices in the UAV swarm. In the simulation scenarios, the positioning performance of hybrid-CN is better than that of many existing CN algorithms, such as MDS, CP-EKF, and so on. The proposed method in this study can achieve a performance comparable to that of SPAWN, but with lower computation loads.

Anyway, the method proposed in this article is based on the availability of GNSS, the situations of multipath, and GNSS denial are not discussed. During the flight of the UAV swarm, more complex mission scenarios may be encountered, and the cooperative navigation method for the satellite-constrained environment will be developed in the near future.