Collaborative Integrated Navigation for Unmanned Aerial Vehicle Swarms Under Multiple Uncertainties

Abstract

UAV swarms possess unique advantages in performing various tasks and have been successfully applied across multiple scenarios. Accurate navigation serves as the foundation and prerequisite for executing these tasks. Unlike single UAV localization, swarms enable the sharing and propagation of precise positioning information, which enhances overall swarm localization accuracy but also introduces the issue of uncertainty propagation. To address this challenge, this paper proposes an integrated navigation and positioning method that models, propagates, and mitigates uncertainties. To tackle the issue of uncertainty in information quality caused by outliers in external correction data, a robust integrated navigation method for nonlinear systems is derived based on a normal gamma distribution model. Considering uncertainty propagation, a statistical linearization model for nonlinear systems is developed. Building upon this model, an augmented measurement nonlinear least squares positioning method is applied, achieving further improvements in localization accuracy. Simulation experiments demonstrate that the proposed method, which thoroughly accounts for the effects of multiple uncertainties, can achieve robust tracking and provide relatively accurate positioning results.

1. Introduction

Unmanned aerial vehicle (UAV) swarms have emerged as a transformative approach in various applications, such as disaster relief, environmental monitoring, precision agriculture, and military operations [1,2]. Unlike single UAV systems, swarms offer significant advantages in terms of scalability, redundancy, and the ability to perform complex tasks collaboratively. For a UAV swarm to function effectively, precise and reliable navigation is paramount. Navigation enables each UAV to determine its position, maintain formations, and execute coordinated tasks efficiently.

Collaborative navigation is particularly critical in UAV swarms, as it allows individual units to share information, collectively compensate for uncertainties, and enhance the overall reliability of the system. This cooperative framework minimizes the reliance on external infrastructure, making UAV swarms more robust and adaptable to dynamic and challenging environments. At the same time, this cooperative approach enables the entire UAV swarm to achieve absolute positioning as long as a subset of the UAVs in the swarm can receive external correction information. Despite its potential, UAV swarm navigation faces significant challenges, primarily due to various sources of uncertainty that can degrade positioning accuracy and system reliability.

For UAVs capable of receiving external correction information, called the leader UAVs, the onboard inertial navigation system (INS) is corrected using this information. Common types of external correction information include global navigation satellite system (GNSS) signals or visual matching with pre-mapped environments. Ref. [3] emphasizes the importance of utilizing multiple sensors for integrated navigation. Ref. [4] implements a loose coupling of GNSS signals, an INS, and LiDAR data, achieving an INS and LiDAR simultaneous localization and mapping (SLAM) integration. When the GNSS is denied, visual matching methods serve as a crucial means of obtaining external information. Ref. [5] presents an integrated navigation system for UAVs in GNSS-denied environments based on a radar odometry and an enhanced visual odometry. Ref. [6] assists visual–inertial odometry in scale estimation and correction by identifying artificial landmarks. However, these external sources are higly uncertain and prone to large errors due to factors like multipath effects [7,8], signal occlusion, or mismatches in visual features [9], and measurements with large errors are called outliers. Outliers can significantly distort the positioning results if not robustly handled. To address the issues of non-line-of-sight (NLOS) and multipath effects, Ref. [7] proposes a novel hybrid federated fusion framework. Ref. [8], in combination with Monte Carlo simulation, introduces a Kalman filter tunning method. Aiming at the problem of mismatching of single-point feature matching, Ref. [9] proposes a new method utilizing conjugate line segments, feature curve elements, and texture color region segmentation to enhance robustness. Ref. [10] proposes a method for modeling and processing data corrupted with outliers based on a normal gamma distribution; however, it is designed for linear systems.

For UAVs that cannot directly receive external correction information, referred to as follower UAVs, the external correction information can be propagated to them based on measurements made by leader UAVs. However, the self-localization uncertainty of a UAV propagates into the relative positioning estimates, compounding errors across the swarm. To address the 3D positioning problem of UAV swarms, Ref. [11] treats the UAV swarm system as a rigid body and determines the final position of each node by calculating the global minimum, thereby achieving relative positioning. Ref. [12] also proposes a range-only EKF navigation system that integrates IMU data with inter-node distance measurements obtained from onboard sensors to mitigate the growth of position, navigation, and timing errors during GNSS outages. Refs. [13,14] introduce the sigma point belief propagation method to enable information exchange between UAVs. However, the positioning error of leader UAVs propagates during the information transfer, leading to increased uncertainty in positioning, a problem highlighted in [15]. Ref. [3] considers the dependence of positioning accuracy on the distribution and accuracy of leader UAVs in the swarm.

The measurement and localization among UAVs primarily rely on range estimation through communication time, such as ultra wide band (UWB) ranging. Using multiple measurements, the position of the target UAV can typically be estimated using methods like least squares, as exemplified by the classical two-step weighted least squares method in [16]. However, the relationship between ranging measurements and positions is highly nonlinear [17,18], while most existing least squares methods are linear in structure [19]. To apply linear least squares, the nonlinear system is often linearized. This approach has two issues. Firstly, when linearizing the ranging model, the positioning accuracy error of the leader UAV is not considered. Secondly, the final estimate remains a linear function of the measurements, failing to fully utilize the measurement information and mismatching the inherent nonlinear characteristics of the problem. These two issues have not been thoroughly investigated in the existing literature, with only preliminary discussions found in [20].

Three main types of uncertainties faced in cooperative localization and navigation of UAV swarms are summarized as follows:

• Uncertainty in external correction information for the swarm: This information often contains outliers, significantly affecting the localization accuracy of the leader UAVs;
• Propagation of leader UAV localization uncertainties: When the leader drones localize the follower drones, their own uncertainties are transmitted;
• Challenges in handling uncertainties due to high nonlinearity in inter-UAV distance measurements.

To address these uncertainties, this paper considers them comprehensively in UAV swarm cooperative localization and navigation, proposing an integrated modeling and processing method, which extends the filter and model in [3,20], while making the following contributions:

• To address the issue of outliers in external information, a normal gamma distribution is employed to model measurement noise. A robust filter for integrated navigation based on the normal gamma distribution (RINNG) under tight coupling is derived to model and handle uncertainties in external information, which is presented in Section 3.
• A statistical linearization model is proposed in Section 4 to further account for the uncertainty in the leader UAV’s positioning. The positioning result of the leader UAV is given by the RINNG in Section 3. This contribution focuses on achieving statistical linearization of nonlinear models for the least squares positioning in contribution 3, while considering the uncertainty of the positioning in contribution 1.
• To address the issue of inadequate information utilization due to the high nonlinearity of the model, an uncorrelated conversion method is applied to achieve least squares localization with nonlinear structures, which is given in Section 5.

The paper is organized as follows. Section 2 formulates the problem and addresses the motivations of the paper. Section 3 proposes the robust filter for integrated navigation based on the normal gamma distribution. Section 4 proposes the statistical linearization for nonlinear systems considering multiple uncertainties. Section 5 derives the augmented nonlinear least square estimation for collaborative positioning among UAVs. Section 6 shows the simulation results, and Section 7 concludes the paper.

2. Problem Formulation

2.1. Modeling

Consider a swarm system composed of N UAVs, shown in Figure 1, where $n_1$ UAVs are equipped with high-precision INSs and/or can receive external correction information, while the remaining $n_2$ (i.e., $n_2=N-n_1)$ UAVs are equipped with low-cost, low-precision INS. The available navigation information for the entire swarm includes each UAV’s inertial navigation data, external correction information, and inter-UAV relative positioning data. Our goal is to utilize this information to achieve accurate estimation of each UAV’s dynamic state, with the state defined as

$$x_k^i={\left[{\mathsf{x}}_k^i,{\mathsf{y}}_k^i,{\mathsf{z}}_k^i\right]}^{\mathrm{T}}$$

(1)

where k is the time index and i the label of the UAV, and ${[{\mathsf{x}}_k^i,{\mathsf{y}}_k^i,{\mathsf{z}}_k^i]}^{\mathrm{T}}$ denotes the position of the ith UAV.

Figure 1. The problem considered in the paper.

2.1.1. Inertial Navigation System

For the INS, a dynamic system of the system error can be established [1]:

$$\begin{array}{cc}\hfill \varphi =& \varphi \times {\omega }_{in}^n+\delta {\omega }_{in}^n-C_b^n{\epsilon }^b\hfill \end{array}$$

(2)

$$\begin{array}{cc}\hfill \delta {\dot{v}}^n=& -\varphi \times C_b^n{f}^b+\delta v^n\times (2{\omega }_{ie}^n+{\omega }_{en}^n)\hfill \end{array}$$

(3)

$$\begin{array}{cc}\hfill & +v^n\times (\delta {\omega }_{ie}^n+\delta {\omega }_{en}^n)+C_b^n{▽}^b\hfill \end{array}$$

(4)

$$\begin{array}{cc}\hfill \delta \dot{L}=& {\displaystyle \frac{\delta v_{\mathrm{N}}}{R_{\mathrm{M}}+h}}-\delta h{\displaystyle \frac{v_{\mathrm{N}}}{R_M+h}}\hfill \end{array}$$

(5)

$$\begin{array}{cc}\hfill \delta \dot{\lambda }=& {\displaystyle \frac{v_{\mathrm{E}}secL}{R_{\mathrm{M}}+h}}+\delta L{\displaystyle \frac{v_{\mathrm{E}}tanLsecL}{R_{\mathrm{N}}+h}}-\delta h{\displaystyle \frac{v_{\mathrm{E}}secL}{{(R_{\mathrm{N}}+h)}^2}}\hfill \end{array}$$

(6)

$$\begin{array}{cc}\hfill \delta \dot{h}=& \delta v_U\hfill \end{array}$$

(7)

$$\begin{array}{cc}\hfill {\dot{\epsilon }}_B^b=& 0\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill {\dot{▽}}_B^b=& 0\hfill \end{array}$$

(9)

Let the inertial navigation velocity $v^n={\left[\begin{array}{ccc}{v}_E& v_N& v_U\end{array}\right]}^{\mathrm{T}}$ and the velocity error $\delta v^n={\left[\begin{array}{ccc}\delta v_E& \delta v_N& \delta v_U\end{array}\right]}^{\mathrm{T}}$, where $v_E,v_N$, and $v_U$ represent the components in the east, north, and up directions, respectively. $\delta L,\delta \lambda ,$ and $\delta h$ represent latitude error, longitude error, and altitude error, respectively. $C_b^n$ denotes the ideal error-free strapdown inertial navigation attitude matrix from the inertial navigation frame to the body coordinate frame. $\delta {\omega }_{in}^n$ is the navigation frame error, with $\delta {\omega }_{ie}^n$ and $\delta {\omega }_{en}^n$ representing the calculation errors of the earth’s rotational angular velocity and the navigation frame’s rotational angular velocity, respectively. ${▽}^b$ is the accelerometer measurement bias. $R_N$ is the radius of curvature in the prime vertical, and $R_M$ is the radius of curvature in the meridian. Let ${\Theta }_k={[{\varphi }^{\mathrm{T}},{\left(\delta v^n\right)}^{\mathrm{T}},\delta p,{\left({\epsilon }_B^b\right)}^{\mathrm{T}},{\left({▽}_B^b\right)}^{\mathrm{T}}]}^{\mathrm{T}}$ with variables ${\varphi }^{\mathrm{T}},{\left(\delta v^n\right)}^{\mathrm{T}},\delta p,{\left({\epsilon }_B^b\right)}^{\mathrm{T}},{\left({▽}_B^b\right)}^{\mathrm{T}}$ being the attitude error, velocity error, position error, gyroscope random drift error, and accelerometer random bias error of the INS, respectively. The dynamic system can be rewritten as

$${\Theta }_{k+1}=F_k{\Theta }_k+G_k{w}_k^b$$

(10)

where $F_k$ is a function of ${\Theta }_k$, $w^b={[{\left({\epsilon }_w^b\right)}^{\mathrm{T}},{\left({▽}_B^b\right)}^{\mathrm{T}}]}^{\mathrm{T}}$ is the process noise with ${\epsilon }_w^b$ and ${▽}_B^b$ being the process noise of the gyroscope random drift and accelerometer components, respectively, and $w_k^b\sim \mathcal{N}(0,Q_k)$.

2.1.2. External Correction Information

External correction information refers to navigation data obtained from outside the UAV swarm that can be used to correct the errors in the INS. The most commonly used external correction data is from the GNSS, which provides relatively accurate positioning information to compensate for the cumulative drift of the INS. When the GNSS is unavailable, visual matching becomes another common source of correction information. This method uses onboard cameras to capture images, identify key features, and extract positional data.

Although external correction data are generally assumed to be accurate, outliers often occur in practical applications. The GNSS can experience significant positioning errors under interference, while feature-matching errors in complex environments can also lead to outliers. If these outliers are not properly handled, the corrected positioning error can be substantial, thereby compromising the overall positioning accuracy of the swarm.

Detecting and eliminating these outliers is challenging. On one hand, it is difficult to set a threshold that avoids both missing true outliers and mistakenly discarding valid data. On the other hand, simply discarding potential outliers results in information wasting.

To address the outliers in external correction information, we propose the following measurement modeling method. Assume the position output by the INS is ${\tilde{p}}_i$, and the position provided by the external correction information is ${\tilde{p}}_e$. Then, the position matching measurement is defined as

$$z={\tilde{p}}_i-{\tilde{p}}_e$$

(11)

and the measurement equation is

$$z_k=H_k{\Theta }_k+v_k$$

(12)

with ${\Theta }_k$ being the state defined by (10), and $H_k$ the measurement equation defined by

$$H_k=\left[0_{3\times 6}{I}_{3\times 6}{0}_{3\times 6}\right]$$

(13)

The measurement noise is assumed conditional Gaussian distributed with a probability density function (PDF) given by [10,21]

$$p\left(v_k\right|{\tau }_k)=\mathcal{N}(v_k;0,{\displaystyle \frac{R_k}{{\tau }_k}})$$

(14)

where ${\tau }_k$ is a gamma variable representing the uncertainty in the quality of external correction information with

$$\begin{array}{c}\hfill p\left({\tau }_k\right)=\mathcal{G}({\tau }_k;a_k,b_k)={\displaystyle \frac{b_k^{a_k}}{\Gamma \left(b_k\right)}}{\tau }_k^{a_k-1}{e}^{-b_k{\tau }_k}\end{array}$$

(15)

The shape parameter of ${\tau }_k$ is denoted by $a_k$ and the rate parameter by $b_k$. Marginally, the measurement noise $v_k$ is Student’s t-distributed, which is a heavy-tailed distribution suitable for modeling measurements with outliers.

2.1.3. Time Difference of Arrival Localization Information

A time difference of arrival (TDOA) measurement is the time difference between a source and a pair of sensors with known positions. To estimate the source position, multiple TDOA measurements are needed. In the considered problem, $n_1$ UAVs equipped with high-precision INSs are regarded as sensors to perform TDOA positioning for the remaining $n_2$ UAVs with low-precision INSs. The TDOA measurements for the ith UAV are

$$Z_k^i=h(x_k^i,X_k^j)+{\check{V}}_k^i$$

(16)

where $Z_k^i\in {\mathbb{R}}^{n_1-1}$, measurement noise ${\check{V}}_k^i\in {\mathbb{R}}^{n_1-1}$, the set of locations of sensors $X_k^j={\left\{x_k^j\right\}}_{j=1}^{n_1}$, and for $j=1,\dots ,n_1,i=n_1+1,\dots ,N$

$$Z_k^i=\left[\begin{array}{c}{z}_{21}^i\\ ⋮\\ z_{n_{1}1}^i\end{array}\right]=\left[\begin{array}{c}{d}_2^i-d_1^i\\ ⋮\\ d_{n_1}^i-d_1^i\end{array}\right],{\check{V}}_k^i=\left[\begin{array}{c}{v}_{21}^i\\ ⋮\\ v_{n_{1}1}^i\end{array}\right]$$

(17)

$$h(x_k^i,X_k^j)=\left[\begin{array}{c}∥x_k^i-x_k^2∥-∥x_k^i-x_k^1∥\\ ⋮\\ ∥x_k^i-x_k^{n_1}∥-∥x_k^i-x_k^1∥\end{array}\right]$$

(18)

$$\begin{array}{c}\hfill d_j^i=∥x_k^i-x_k^j∥=\sqrt{{({\mathsf{x}}_k^i-{\mathsf{x}}_k^i)}^2+{({\mathsf{y}}_k^i-{\mathsf{y}}_k^j)}^2+{({\mathsf{z}}_k^i-{\mathsf{z}}_k^i)}^2}\end{array}$$

(19)

Without lose of generality, we select UAV 1 as the reference. The arrival times at all other UAVs are substracted from the arrival time at UAV 1 $x_k^1$ to obtain the TDOA measurement in Equation (18). Note that unlike most TDOA problems, in Equation (18), $h(·)$ is not only the function of $x_k^i$, but also of $x_k^j,j=1,\dots ,n_1$, and they are also random. The uncertainty of $x_k^j$ further influences the estimation of $x_k^i$. Additionally, $h(·)$ is highly nonlinear. All these factors make it extremely challenging to use UAVs equipped with high-precision INSs to locate other UAVs.

2.2. Motivations

Collaborative integrated navigation in UAV swarms enables the effective fusion of various sensor information and the transfer of high-precision navigation data from a set of UAVs to the entire swarm, thereby enhancing overall positioning accuracy. However, this approach faces significant challenges due to various uncertainties.

First, there is the uncertainty in external correction information. Common sources of external correction include the GNSS and visual matching. While the GNSS provides high accuracy, it is susceptible to interference and may be denied in highly contested environments, rendering it unable to deliver correction data. Moreover, adversaries could intercept and retransmit fake GNSS signals to deceive the system. Visual matching, on the other hand, is less affected by electromagnetic interference. If terrain features or key landmarks [6] are accurately matched, it can offer precise correction information. However, matching errors and offsets frequently occur, leading to significant errors in correction data at certain times. Both the GNSS and visual matching share a common issue: the correction information they provide often contains numerous outliers. If these outliers are not properly handled, they can cause integrated navigation to fail, which would further compromise the positioning accuracy of the entire swarm. Simply identifying and removing outliers carries the risk of false positives or missed detections, resulting in information loss. Therefore, a more effective solution is to accurately model measurements with outliers for better integrated navigation.

The most popular approach to handling measurement outliers is to model measurement noise using heavy-tailed distributions. The core idea is to assign higher probabilities to large noise values. Common heavy-tailed distributions include the t-distribution and its variants whose marginal distributions are also t-distributed [21,22,23,24]. In this paper, a conditional Gaussian distribution is proposed, whose marginal distribution is also a t-distribution, effectively capturing measurements with outliers. Compared to existing modeling methods, this approach is simpler and facilitates the development of a closed-form filter within the Bayesian framework.

Secondly, mutual positioning among UAVs enables the transfer of high-precision location information but also propagates the uncertainties in positioning results. With the support of a high-precision INS and external correction information, some UAVs in the swarm achieve relatively accurate self-positioning, while others maintain lower self-positioning accuracy. UAVs with accurate self-positioning can measure the positions of other UAVs, facilitating mutual positioning within the swarm and spreading high-precision information throughout. However, since the self-positioning of high-precision UAVs is still based on the estimate that is a function of the external correction data, it inherently contains uncertainty. Additionally, outliers in the external correction information further exacerbate this uncertainty. If the impact of these uncertainties is ignored and a conventional TDOA positioning method is directly applied, it can result in significant positioning errors.

Thirdly, the TDOA measurements are highly nonlinear with respect to the estimand, making TDOA-based positioning essentially a nonlinear estimation problem. Traditional TDOA algorithms primarily rely on the least squares method. However, the existing least squares methods provide a linear estimator of the measurements, meaning they are linear functions of the measurements, which is mismatched with the inherently nonlinear nature of the estimation problem. The limitation imposed by the linear structure of the estimator makes it difficult to improve the accuracy of the positioning results. Under conditions of strong nonlinearity, modeling, describing, and processing the uncertainties in both external correction information and self-localization become even more challenging. This has prompted us to propose new, more effective methods for extracting positioning information from nonlinear systems.

In summary, in collaborative positioning for UAV swarms, the first step is to achieve high-precision self-localization for some UAVs through integrated navigation using external correction information and INSs, with a focus on addressing outliers in the correction data. Secondly, based on the self-localization results and considering their uncertainties, TDOA positioning is performed for the remaining UAVs, emphasizing the propagation of self-localization uncertainties and the development of a nonlinear TDOA method. The motivations of this paper are summarized by Figure 2.

Figure 2. Motivations of this paper.

3. Robust Filter for Integrated Navigation Based on the Normal Gamma Distribution

According to Figure 2, external correction information and INSs are first used for integrated navigation to determine the positions of a set of UAVs. For the GNSS, interference may cause only a subset of UAVs to receive satellite signals. For visual matching, limitations in observation angles may result in only some UAVs successfully matching to positioning features. Both types of information may contain outliers, necessitating a robust integrated navigation method to handle the uncertainties in the correction data while also accounting for the uncertainties in the self-localization results. The following proposes such a robust filter to address the uncertainties, which is the filter mentioned by contribution 1, and is named the robust filter for integrated navigation based on the normal gamma distribution (RINNG). The models of integrated navigation are defined by (10) and (12), and summarized as follows.

$$\left\{\begin{array}{c}{\Theta }_{k+1}=F_k{\Theta }_k+G_k{w}_k^b\\ z_k=H_k{\Theta }_k+v_k\end{array}\right.$$

(20)

with $w_k^b$ and $v_k$ defined by (10) and (14), respectively.

Assume that at time $k-1$, the state ${\Theta }_{k-1}$ and ${\tau }_{k-1}$ are jointly distributed with PDF $p({\Theta }_{k-1},{\tau }_{k-1}|Z^{k-1})=\mathcal{NG}({\Theta }_{k-1},{\tau }_{k-1};{\widehat{x}}_{k-1|k-1},P_{k-1|k-1},{\widehat{a}}_{k-1|k-1},{\widehat{b}}_{k-1|k-1})$, where ${\tau }_{k-1}$ is the uncertainty variable.

3.1. Time Update

The predicted filtering density at time k can be obtained by the Chapman–Kolmogorov equation based on the prior density,

$$\begin{array}{cc}\hfill & p({\Theta }_k,{\tau }_k|z^{k-1})\hfill \\ \hfill =& \int \int p\left({\Theta }_k\right|{\Theta }_{k-1})p\left({\tau }_k\right|{\Theta }_{k-1},{\tau }_{k-1})p({\Theta }_{k-1},{\tau }_{k-1}|Z^{k-1})\mathrm{d}{\tau }_{k-1}\mathrm{d}{\Theta }_{k-1}\hfill \\ \hfill =& \int p\left({\Theta }_k\right|{\Theta }_{k-1})p({\Theta }_{k-1},{\tau }_k|z^{k-1})\mathrm{d}{\Theta }_{k-1}\hfill \end{array}$$

(21)

where

$$\begin{array}{c}\hfill p({\Theta }_{k-1},{\tau }_k|z^{k-1})=\int p\left({\tau }_k\right|{\Theta }_{k-1},{\tau }_{k-1})p({\Theta }_{k-1},{\tau }_{k-1}|z^{k-1})\mathrm{d}{\tau }_{k-1}.\end{array}$$

(22)

Assume that the evolution of ${\Theta }_k$ is independent of ${\tau }_k$ and ${\tau }_{k-1}$, and then we have

$$p\left({\Theta }_k\right|{\Theta }_{k-1})=\mathcal{N}(x_k;F_k{\Theta }_{k-1},Q_k)$$

(23)

By (22), the density $p\left({\tau }_k\right|{\Theta }_{k-1},{\tau }_{k-1})$ is needed to get $p({\Theta }_{k-1},{\tau }_k|z^{k-1})$. However, it is not straightforward to have a $p\left({\tau }_k\right|{\Theta }_{k-1},{\tau }_{k-1})$ such that (22) is a normal gamma distribution. We follow the idea of [21,22,23,24] to obtain the predicted density $p({\Theta }_{k-1},{\tau }_k|z^{k-1})$ by replacing ${\widehat{a}}_{k-1|k-1}$ and ${\widehat{b}}_{k-1|k-1}$ in $p({\Theta }_{k-1},{\tau }_{k-1}|z^{k-1})$ with ${\widehat{a}}_{k|k-1}$ and ${\widehat{b}}_{k|k-1}$:

$$\begin{array}{cc}\hfill & p({\Theta }_{k-1},{\tau }_k|z^{k-1})\hfill \\ \hfill =& \mathcal{N}({\Theta }_{k-1};{\widehat{\Theta }}_{k-1|k-1},P_{k-1|k-1}/{\tau }_k)\mathcal{G}({\tau }_k;{\widehat{a}}_{k|k-1},{\widehat{b}}_{k|k-1})\hfill \\ \hfill =& \mathcal{NG}({\Theta }_{k-1},{\tau }_k;{\widehat{\Theta }}_{k-1|k-1},P_{k-1|k-1},{\widehat{a}}_{k|k-1},{\widehat{b}}_{k|k-1})\hfill \end{array}$$

(24)

where

$$\begin{array}{cc}\hfill {\widehat{a}}_{k|k-1}& =\rho {\widehat{a}}_{k-1|k-1}\hfill \end{array}$$

(25)

$$\begin{array}{cc}\hfill {\widehat{b}}_{k|k-1}& =\rho {\widehat{b}}_{k-1|k-1}\hfill \end{array}$$

(26)

with $\rho \in (0,1]$. By (25) and (26), the predicted density of ${\tau }_k$ is

$$p\left({\tau }_k\right|Z^{k-1})=\mathcal{G}({\tau }_k;{\widehat{a}}_{k|k-1},{\widehat{b}}_{k|k-1})$$

(27)

One advantage of this technique is that it keeps ${\widehat{\tau }}_{k|k-1}$ identical to ${\widehat{\tau }}_{k-1|k-1}$, and just scales its variance up by ${\rho }^{-1}$ since the variance of ${\tau }_k$ is $a_k/b_k^2$, which means that without new information, the estimate becomes more uncertain. This is natural when we indeed have no knowledge of the evolution. The value $\rho =1$ corresponds to stationary noise, and a lower value $(0<\rho <1)$ increases the time fluctuation. More discussions on this technique can be found in [21,22,23,24].

Substituting (23) and (27) into (21) yields

$$\begin{array}{c}\hfill p({\Theta }_k,{\tau }_k|z^{k-1})=\mathcal{N}({\Theta }_k;{\overline{\Theta }}_{k|k-1},{\overline{P}}_{k|k-1}/{\tau }_k)\mathcal{G}({\tau }_k;{\widehat{a}}_{k|k-1},{\widehat{b}}_{k|k-1})\end{array}$$

(28)

The following derivation shows how to get Equation (28), including ${\overline{\Theta }}_{k|k-1}$ and ${\overline{P}}_{k|k-1}$.

Rewrite $p\left({\Theta }_k\right|{\Theta }_{k-1})$ as

$$\begin{array}{cc}\hfill p\left({\Theta }_k\right|{\Theta }_{k-1})\propto & exp[-{\displaystyle \frac{1}{2}}{({\Theta }_k-F_k{\Theta }_{k-1})}^{\mathrm{T}}{Q}_k^{-1}(·)]\hfill \end{array}$$

(29)

$$\begin{array}{cc}\hfill \propto & exp[-{\displaystyle \frac{{\tau }_k}{2}}{({\Theta }_k-F_k{\Theta }_{k-1})}^{\mathrm{T}}{\left({\tau }_k{Q}_k\right)}^{-1}(·)]\hfill \end{array}$$

(30)

By (30), Equation (28) becomes

$$\begin{array}{cc}\hfill & {\int }_{R^{n_x}}exp[-{\displaystyle \frac{{\tau }_k}{2}}{({\Theta }_k-F_k{\Theta }_{k-1})}^{\mathrm{T}}{({\tau }_k{Q}_k)}^{-1}(·)]\hfill \\ \hfill & \times {\tau }_k^{n_x/2+{\widehat{a}}_{k|k-1}-1}exp\{-{\displaystyle \frac{{\tau }_k}{2}}[2{\widehat{b}}_{k|k-1}\hfill \\ \hfill & +{({\Theta }_{k-1}-{\widehat{\Theta }}_{k-1|k-1})}^{\mathrm{T}}{P}_{k-1|k-1}^{-1}(·)]\}\mathrm{d}{x}_{k-1}\hfill \\ \hfill =& {\tau }_k^{n_x/2+{\widehat{a}}_{k|k-1}-1}exp(-{\tau }_k{\widehat{b}}_{k|k-1})\hfill \\ \hfill & \times {\int }_{R^{n_x}}exp\{-{\displaystyle \frac{{\tau }_k}{2}}[{({\Theta }_k-F_k{\Theta }_{k-1})}^{\mathrm{T}}{({\tau }_k{Q}_k)}^{-1}(·)\hfill \\ \hfill & +{({\Theta }_{k-1}-{\widehat{\Theta }}_{k-1|k-1})}^{\mathrm{T}}{P}_{k-1|k-1}^{-1}(·)]\}\mathrm{d}{\Theta }_{k-1}\hfill \end{array}$$

(31)

Note that $F_k$ is a function of ${\Theta }_{k-1}$, making Equation (31) an integral of a nonlinear function multiplied by a Gaussian distribution. This form is well suited for approximating state prediction using Gauss–Hermite quadrature [25], which is a fixed-point sampling method. The basic steps are as follows:

• The integration points ${\chi }_m,m=1,\dots ,M$ are generated, where M is a predetermined number, usually chosen as 3 or 5. Construct a diagonal symmetric matrix D with elements on the diagonal being 0 and $D_{m,m+1}=\sqrt{m/2},m=1,\dots ,M-1$. Calculate the eigenvalues of the matrix D, and let the integration points ${\chi }_m=\sqrt{2}{\lambda }_m$ with ${\lambda }_m$ being the mth eigenvalue. Let the normalized eigenvector corresponding to ${\lambda }_m$ be denoted as ${\nu }_m$, then the weight ${\omega }_m={\left({\nu }_m^1\right)}^2$, where ${\nu }_m^1$ is the first element of ${\nu }_m$.
• Based on the estimate ${\widehat{\Theta }}_{k-1|k-1}$ and its MSE $P_{k-1|k-1}$, sigma points are generated, with the mth sigma point being

  $${\xi }_m=\sqrt{P_{k-1|k-1}}{\chi }_m+{\widehat{\Theta }}_{k-1|k-1}$$

  (32)

  where $\sqrt{P_{k-1|k-1}}$ is the result of the Cholesky decomposition of $P_{k-1|k-1}$, i.e., $P_{k-1|k-1}=$$\sqrt{P_{k-1|k-1}}{\sqrt{P_{k-1|k-1}}}^{\mathrm{T}}$.
• Based on the sigma points and their weights, the conditional mean is

  $${\overline{\Theta }}_{k|k-1}=\mathrm{E}\left[{\Theta }_k\right|{\tau }_k,z^{k-1}]=\sum _{m=1}^M{\omega }_m{F}_k\left({\xi }_m\right){\xi }_m$$

  (33)

  and the conditional MSE matrix is

  $$\begin{array}{c}\hfill C_{k|k-1}=\mathrm{E}\left[({\Theta }_k-{\overline{\Theta }}_{k|k-1}){({\Theta }_k-{\overline{\Theta }}_{k|k-1})}^{\mathrm{T}}\right|{\tau }_k,z^{k-1}]={\overline{P}}_{k|k-1}/{\tau }_k\end{array}$$

  (34)

  where the conditional scale matrix ${\overline{P}}_{k|k-1}$ is

  $${\overline{P}}_{k|k-1}=\sum _{m=1}^M{\omega }_m(F_k\left({\xi }_m\right){\xi }_m-{\overline{\Theta }}_{k|k-1}){(·)}^{\mathrm{T}}+{\tau }_k{Q}_k={\check{P}}_{k|k-1}+{\tau }_k{Q}_k$$

  (35)

Therefore, the predicted density is

$$\mathcal{N}({\Theta }_k;{\overline{\Theta }}_{k|k-1},{\overline{P}}_{k|k-1}/{\tau }_k)\mathcal{G}({\tau }_k;{\widehat{a}}_{k|k-1},{\widehat{b}}_{k|k-1})$$

(36)

We desire the predicted density $p({\Theta }_k,{\tau }_k|Z^{k-1})$ to have a normal gamma form, which requires that its mean and scale matrix do not depend on ${\tau }_k$. Since Equations (34) and (35) depend on ${\tau }_k$, the current density (36) is not a desirable predicted density. To get such a $p({\Theta }_k,{\tau }_k|z^{k-1}),$ we choose a normal gamma distribution that is closest to the predicted density (36). Denote the density (36) as $q({\Theta }_k,{\tau }_k|z^{k-1})$. We would like to find a $p({\Theta }_k,{\tau }_k|z^{k-1})$ whose dissimilarity with $q({\Theta }_k,{\tau }_k|z^{k-1})$ is minimized. Assume that such a normal gamma density has the following form,

$$p({\Theta }_k,{\tau }_k|z^{k-1})=\mathcal{NG}({\Theta }_k,{\tau }_k;{\widehat{\Theta }}_{k|k-1},P_{k|k-1}^m,{\widehat{a}}_{k|k-1},{\widehat{b}}_{k|k-1})$$

(37)

where

$$P_{k|k-1}^m={\check{P}}_{k|k-1}+m_k{Q}_k$$

(38)

with a positive constant $m_k>0$ to be determined. Note that $p({\Theta }_k,{\tau }_k|Z^{k-1})$ differs from $q({\Theta }_k,{\tau }_k|Z^{k-1})$ only in the scale matrix $P_{k|k-1}^m$.

We adopt the K-L divergence

$$D_{\mathrm{KL}}[q({\Theta }_k,{\tau }_k|z^{k-1})\left|\right|p({\Theta }_k,{\tau }_k|z^{k-1})]$$

(39)

as the optimality criterion that measures information loss by using $p({\Theta }_k,{\tau }_k|Z^{k-1})$ to approximate $q({\Theta }_k,{\tau }_k|z^{k-1})$. That is, we find a density in the form of (37) that is closest to $q({\Theta }_k,{\tau }_k|Z^{k-1})$ (36) in the K-L sense:

$$p({\Theta }_k,{\tau }_k|z^{k-1})=\underset{p}{argmin}{D}_{\mathrm{KL}}\left(q\right|\left|p\right)$$

(40)

with the parameter $m_k$ of $p({\Theta }_k,{\tau }_k|Z^{k-1})$ to be determined. The minimum point of $D_{\mathrm{KL}}\left(q\right|\left|p\right)$ in (40) is $m_k=$ E$\left[{\tau }_k\right|Z^{k-1}]={\widehat{\tau }}_{k|k-1}.$ Note that ${\widehat{\tau }}_{k|k-1}={\widehat{\tau }}_{k-1|k-1}$. This ensures that the order of predicting ${\Theta }_k$ and ${\tau }_k$ does not change the results.

3.2. Measurement Update

The posterior density can be obtained as follows, since the measurement model is linear:

$$p({\Theta }_k,{\tau }_k|z^k)={\displaystyle \frac{p\left(z_k\right|{\Theta }_k,{\tau }_k)p({\Theta }_k,{\tau }_k|z^{k-1})}{p\left(z_k\right|z^{k-1})}}$$

(41)

The likelihood function is

$$\begin{array}{c}\hfill p\left(z_k\right|{\Theta }_k,{\tau }_k)=\mathcal{N}(z_k;H_k{\Theta }_k,{\displaystyle \frac{R_k}{{\tau }_k}})\propto {\tau }_k^{n_x/2}exp[-{\displaystyle \frac{{\tau }_k}{2}}{(z_k-H_k{x}_k)}^{\mathrm{T}}{R}_k^{-1}(·)]\end{array}$$

(42)

Since the normal gamma distribution is a conjugate prior of the normal distribution, the posterior density is still a normal gamma distribution, given by

$$\begin{array}{cc}\hfill & p({\Theta }_k,{\tau }_k|z^k)\hfill \\ \hfill =& \mathcal{NG}({\Theta }_k,{\tau }_k;{\widehat{\Theta }}_{k|k},P_{k|k},{\widehat{a}}_{k|k},{\widehat{b}}_{k|k})\hfill \\ \hfill \propto & {\tau }_k^{n_z/2+{\widehat{a}}_{k|k}-1}exp\{-{\displaystyle \frac{{\tau }_k}{2}}[2{\widehat{b}}_{k|k}+{({\Theta }_k-{\widehat{\Theta }}_{k|k})}^{\mathrm{T}}{P}_{k|k}^{-1}(·)]\}\hfill \end{array}$$

(43)

with

$$\begin{array}{cc}\hfill {\widehat{\Theta }}_{k|k}& ={\widehat{\Theta }}_{k|k-1}+K_k(z_k-H_k{\widehat{\Theta }}_{k|k-1})\hfill \end{array}$$

(44)

$$\begin{array}{cc}\hfill P_{k|k}& =P_{k|k-1}-K_k{S}_k{K}_k^{\mathrm{T}}\hfill \end{array}$$

(45)

$$\begin{array}{cc}\hfill {\widehat{a}}_{k|k}& ={\widehat{a}}_{k|k-1}+{\displaystyle \frac{n_z}{2}}\hfill \end{array}$$

(46)

$$\begin{array}{cc}\hfill {\widehat{b}}_{k|k}& ={\widehat{b}}_{k|k-1}+[{(z_k-H_k{\widehat{\Theta }}_{k|k})}^{\mathrm{T}}{R}_k^{-1}(·)+{({\widehat{\Theta }}_{k|k-2}-{\widehat{\Theta }}_{k|k})}^{\mathrm{T}}{P}_{k|k-1}^{-1}(·)]/2\hfill \end{array}$$

(47)

where

$$\begin{array}{cc}\hfill S_k& =R_k+H_k{P}_{k|k-1}{H}_k^{\mathrm{T}}\hfill \end{array}$$

(48)

$$\begin{array}{cc}\hfill K_k& =P_{k|k-1}{H}_k{S}_k^{-1}\hfill \end{array}$$

(49)

The posterior density of ${\Theta }_k$ conditioned on ${\tau }_k$ is Gaussian,

$$p({\Theta }_k|{\tau }_k,z^k)=\mathcal{N}({\Theta }_k;{\widehat{\Theta }}_{k|k},{\displaystyle \frac{P_{k|k}}{{\tau }_k}})$$

(50)

and, marginally, ${\Theta }_k$ is t-distributed

$$p({\Theta }_k|z^k)=\mathcal{T}({\Theta }_k;{\widehat{\Theta }}_{k|k},{\displaystyle \frac{{\widehat{b}}_{k|k}}{{\widehat{a}}_{k|k}}}{P}_{k|k},2{\widehat{a}}_{k|k})$$

(51)

Equations (50) and (51) indicate that the posterior distribution of ${\Theta }_k$ accounts for the influence of the variable ${\tau }_k$, which represents the uncertainty in the external correction information.

Based on the INS solution ${\widehat{x}}_k^{j,\mathrm{INS}}$ at time k and the estimate of ${\Theta }_k$, the location of $x_k^j$ is given by

$$\begin{array}{cc}\hfill {\widehat{x}}_{k|k}^j=& {\widehat{x}}_k^{j,\mathrm{INS}}+{\widehat{\Theta }}_{k|k}^{7:9}\hfill \\ \hfill P_{k|k}^j=& P_{k|k}^{7:9}\hfill \end{array}$$

where ${\widehat{\Theta }}_{k|k}^{7:9}$ refers to the 7th to 9th dimensions of ${\widehat{\Theta }}_{k|k}$, and $P_{k|k}^{7:9}$ the 7th to 9th rows and columns of the matrix $P_{k|k}$.

4. Statistical Linearization for Nonlinear Systems Considering Multiple Uncertainties

After completing the self-localization of some UAVs to get their location estimates ${\widehat{X}}_{k|k}={\left\{{\widehat{x}}_{k|k}^j\right\}}_{j=1}^{n_1}$, these UAVs are treated as base stations. Through methods such as UWB, inter-UAV ranging is performed to obtain TDOA measurements. In Equation (16), $h(·)$ is a nonlinear function, $x_k^i$ represents the position of the UAV to be estimated, $x_k^j$, $j=1,\dots ,n_1$ denote the positions of the self-localized UAVs (base stations), and ${\check{V}}_k^i$ is the measurement error with a mean of E$\left[{\check{V}}_k^i\right]$ and covariance of $C_{\check{V}}$. Generally, E$\left[{\check{V}}_k^i\right]=0.$ For the nonlinear estimation problem described by Equation (16), to estimate the position $x_k^i$ using the least squares method, the system must first be linearized, resulting in the following approximate system,

$$h(x_k^i,X_k,{\check{V}}_k^i)\approx L_h(x_k^i,X_k,V_k^i)=b_k+H_k(x_k^i-{\overline{x}}_k^i)+V_k^i$$

(52)

where $b_k$ and $H_k$ are the parameters to be determined. Note that $V_k$ and ${\check{V}}_k$ are different: $V_k$ represents the measurement noise after accounting for multiple uncertainties. These uncertainties include the uncertainty in external correction information, the resulting uncertainty in UAV self-localization $X_k$, and the uncertainty in TDOA measurements, i.e., ${\check{V}}_k^i$.

To determine the above parameters, the following two assumptions are considered [20]:

• The function $h(·)$ is continuously differentiable and can be sufficiently approximated by a first-order Taylor series expansion in the neighborhood of a given point.
• The first two moments of $L_h(·)$ and $h(·)$ are equal.

Based on the above two assumptions, the statistical linearized system can be obtained,

$$L_h(x_k^i,X_k,V_k)=h({\overline{x}}_k^i,{\widehat{X}}_{k|k},0)+H_k(x_k-{\overline{x}}_k^i)+V_k$$

(53)

where $H_k={\left.{\displaystyle \frac{\partial h(x_k^i,X_k,{\check{V}}_k^i)}{\partial x_k}}\right|}_{x_k^i={\overline{x}}_k^i,X_k={\widehat{X}}_{k|k},{\check{V}}_k^i=0}$ with ${\widehat{X}}_{k|k}={\left\{{\widehat{x}}_{k|k}^j\right\}}_{j=1}^{n_1}$. The first two moments of $V_k$ can be obtained by deterministic sampling [25,26] based on

$$\begin{array}{cc}\hfill \mathrm{E}\left[V_k\right]=& \mathrm{E}[h({\overline{x}}_k^i,X_k,{\check{V}}_k^i)-h({\overline{x}}_k^i,{\widehat{X}}_{k|k},0)]\hfill \end{array}$$

(54)

$$\begin{array}{cc}\hfill \mathrm{Cov}\left[V_k\right]=& \mathrm{E}[\{h({\overline{x}}_k^i,X_k,{\check{V}}_k^i)-h({\overline{x}}_k^i,{\widehat{X}}_{k|k},0)\}{\{·\}}^{\mathrm{T}}]\hfill \end{array}$$

(55)

and be calculated by

$$\begin{array}{cc}\hfill \mathrm{E}\left[V_k\right]\approx & \sum _{m=1}^M{\varpi }_m[h({\overline{x}}_k^i,X_k^m,{\check{V}}_k^{i,m})-h({\overline{x}}_k^i,{\widehat{X}}_{k|k},0)]\hfill \end{array}$$

(56)

$$\begin{array}{cc}\hfill \mathrm{Cov}\left[V_k\right]\approx & \sum _{m=1}^M{\varpi }_m[h({\overline{x}}_k^i,X_k^m,{\check{V}}_k^{i,m})-h({\overline{x}}_k^i,{\widehat{X}}_{k|k},0){(·)}^{\mathrm{T}}]\hfill \end{array}$$

(57)

where $X_k^m$ and ${\check{V}}_k^{i,m}$ are the samples of $X_k$ and ${\check{V}}_k^i$, and ${\varpi }_m$ is the corresponding weight.

Remark 1.  (a) By performing deterministic sampling on $X_k$ and ${\check{V}}_k^i$, both types of uncertainties are considered and equivalently represented as noise $V_k^i$, as illustrated by Figure 3.

(b) Since the elements of ${\check{V}}_k^i$ follow a Gaussian distribution, unscented transformation for Gaussian distributions can be used for sampling. When sampling $X_k$, its posterior distribution is a conditional Gaussian distribution. To leverage the extensive results of fixed-point sampling for Gaussian distributions, the following approximation can be made for $x_k^j$:

$$p(x_k^j|{\tau }_k^j)=\mathcal{N}(x_k^j;{\widehat{x}}_{k|k}^j,{\displaystyle \frac{P_{k|k}^j}{{\tau }_k^j}})\approx \mathcal{N}(x_k^j;{\widehat{x}}_{k|k}^j,{\displaystyle \frac{P_{k|k}^j}{{\widehat{\tau }}_{k|k}^j}})$$

(58)

Figure 3. Illustration of the linearization method considering multiple uncertainties.

Figure 3. Illustration of the linearization method considering multiple uncertainties.

5. Augmented Nonlinear Least Squares Estimation for Collaborative Positioning Among UAVs

For linear systems, the linear least squares method minimizes the fitting error,

$$J={(z-Hx)}^{\mathrm{T}}W(z-Hx)$$

(59)

and we set the gradient of J to zero to get

$$\widehat{x}={\left(H^{\mathrm{T}}WH\right)}^{-1}{H}^{\mathrm{T}}Wz$$

(60)

However, for nonlinear systems, using the above linear-structured estimator leads to an inherent mismatch. When the system is linear, the estimator is optimal. However, when the system is nonlinear, the linear structure of the estimator limits its potential for further performance improvement.

Recently proposed uncorrelated conversion methods [20,27] aim to find nonlinear functions of measurements and obtain filters with nonlinear structures. Their basic idea is to seek nonlinear and uncorrelated conversions $g\left(z\right)$ of the original measurement z and then augment $y=g\left(z\right)$ with z to get

$$Z^a=\left[\begin{array}{c}Z\\ Y\end{array}\right]=\left[\begin{array}{c}h(x_k^i,X_k^j,{\check{V}}^i)\\ g(h(x_k^i,X_k^j,{\check{V}}^i))\end{array}\right]$$

(61)

The estimate is performed based on the augmented measurement $Z^a$. Using the statistical linearization method proposed in Section 4,

$$Z^a\approx L_{h,\phi }(x_k^i,X_k,V^a)=b^a+H^a(x_k^i-{\overline{x}}_k^i)+V^a$$

(62)

where

$$b^a=\left[\begin{array}{c}h({\overline{x}}_k^i,{\widehat{X}}_{k|k},0)\\ g\left(h({\overline{x}}_k^i,{\widehat{X}}_{k|k},0)\right)\end{array}\right],H^a=\left[\begin{array}{c}H\\ H_y\end{array}\right],V^a=\left[\begin{array}{c}{V}^i\\ V^Y\end{array}\right]$$

(63)

with

$$\begin{array}{ccc}& & H_y\hfill \\ & =& {\left.{\displaystyle \frac{\partial g(h(x_k^i,X_k^j,{\check{V}}^i))}{\partial h}}{\displaystyle \frac{\partial h(x_k^i,X_k^j,{\check{V}}^i)}{\partial x_k^i}}\right|}_{x_k^i={\overline{x}}_k^i,X_k={\widehat{X}}_{k|k},{\check{V}}_k^i=0}\hfill \end{array}$$

(64)

and

$$\begin{array}{cc}\hfill \mathrm{E}\left[V^Y\right]=& \mathrm{E}[g\left(h({\overline{x}}_k^i,X_k,{\check{V}}_k^i)\right)-g\left(h({\overline{x}}_k^i,{\widehat{X}}_{k|k},0)\right)]\hfill \end{array}$$

(65)

$$\begin{array}{cc}\hfill \mathrm{Cov}\left[V^Y\right]=& \mathrm{E}[g\left(h({\overline{x}}_k^i,X_k,{\check{V}}_k^i)\right)-g\left(h({\overline{x}}_k^i,{\widehat{X}}_{k|k},0)\right){(·)}^{\mathrm{T}}]\hfill \end{array}$$

(66)

The calculation of these moments can be performed using the deterministic sampling method described in Equations (56) and (57), and also, the approximation in Equation (58) accounts for the uncertainty in sensor position estimation.

The location estimation of the remaining UAVs can be obtained by replacing the parameters in Equation (60) with those defined by (61)–(66). The following augmented nonlinear least squares (ANLS) estimation method is proposed. It is proven that the nonlinear estimate ${\widehat{x}}^{\mathrm{ANLS}}$ based on the augmented measurement $z^a$ is more accurate than the original one ${\widehat{x}}^{\mathrm{LLS}}$, that is, the mean square error of the augmented one is smaller than the original one,

$$P_{{\widehat{x}}^{\mathrm{ANLS}}}={\left[{\left(H^a\right)}^{\mathrm{T}}{W}^a{H}^a\right]}^{-1}=P_{{\widehat{x}}^{\mathrm{LLS}}}-P_x^a$$

(67)

where $P_x^a$ is a positive semi-definite matrix and, therefore, $P_{{\widehat{x}}^{\mathrm{ANLS}}}$ is smaller than $P_{{\widehat{x}}^{\mathrm{LLS}}}$.

Following [20], the uncorrelated conversion function is selected as

$$Y=g\left(Z\right)=\left[\begin{array}{c}{(z_{21}+r_1)}^2\\ ⋮\\ {(z_{n_1}+r_1)}^2\end{array}\right]$$

(68)

Using the the augmented measurement (61) and based on the TWLS method, the collaborative positioning among UAVs is achieved. Detailed steps are summarized as follows.

5.1. First Step

Based on the statistical linearization model (62), the parameters defined by (63)–(66) can be calculated by

$$Y=\left[\begin{array}{c}{\displaystyle \frac{1}{2}}{r}_{21}^2\\ ⋮\\ {\displaystyle \frac{1}{2}}{r}_{n_{1}1}^2\end{array}\right],$$

(69)

$$b_Y=\left[\begin{array}{c}{\displaystyle \frac{1}{2}}({\left({\widehat{x}}_{k|k}^2\right)}^{\mathrm{T}}{\widehat{x}}_{k|k}^2-{\left({\widehat{x}}_{k|k}^1\right)}^{\mathrm{T}}{\widehat{x}}_{k|k}^1)\\ ⋮\\ {\displaystyle \frac{1}{2}}({\left({\widehat{x}}_{k|k}^{n_1}\right)}^{\mathrm{T}}{\widehat{x}}_{k|k}^{n_1}-{\left({\widehat{x}}_{k|k}^1\right)}^{\mathrm{T}}{\widehat{x}}_{k|k}^1)\end{array}\right]$$

(70)

$$\begin{array}{cc}\hfill H_Y=& \left[\begin{array}{cc}{({\widehat{x}}_{k|k}^2-{\widehat{x}}_{k|k}^1)}^{\mathrm{T}}& r_{21}\\ ⋮& ⋮\\ {({\widehat{x}}_{k|k}^{n_1}-{\widehat{x}}_{k|k}^1)}^{\mathrm{T}}& r_{n_{1}1}\end{array}\right],\hfill \end{array}$$

(71)

$$\begin{array}{cc}\hfill V^Y=& \left[\begin{array}{c}{r}_{21}{v}_{21}-{\displaystyle \frac{1}{2}}{v}_{21}^2\\ ⋮\\ r_{n_{1}1}{v}_{n_{1}1}-{\displaystyle \frac{1}{2}}{v}_{n_{1}1}^2\end{array}\right]\hfill \end{array}$$

(72)

The moments ${\overline{V}}^a$ and $C_{V^a}$ of $V^Y$ can be obtained by deterministic sampling. The estimate is given by

$$\begin{array}{cc}\hfill \widehat{u}=& {\left[{\left(H^a\right)}^{\mathrm{T}}{W}^a{H}^a\right]}^{-1}{\left(H^a\right)}^{\mathrm{T}}{W}^a(Z^a-b^a-{\overline{V}}^a)\hfill \end{array}$$

(73)

$$\begin{array}{cc}\hfill P_{\widehat{u}}=& {\left[{\left(H^a\right)}^{\mathrm{T}}{W}^a{H}^a\right]}^{-1}\hfill \end{array}$$

(74)

with $\widehat{u}=\left[\begin{array}{c}{\check{x}}_{k|k}^i\\ {\widehat{r}}_1\end{array}\right]$, which also estimates the range $r_1$.

5.2. Second Step

According to TWLS, based on $\widehat{u}$ and the relationship between $r_1$ and $x^i$, the estimate of $x^i$ can be further improved. The statistical linearization model is constructed as follows,

$$z^{\prime }=H^{\prime }{u}^{\prime }+v^{\prime }$$

(75)

where

$$\begin{array}{cc}\hfill u^{\prime }=& \left[(x_k^i-{\widehat{x}}_{k|k}^j)\odot (x_k^i-{\widehat{x}}_{k|k}^j)\right],H^{\prime }=\left[\begin{array}{c}{I}_{3\times 3}\\ 1_{1\times 3}\end{array}\right]\hfill \end{array}$$

(76)

$$\begin{array}{cc}\hfill z^{\prime }=& \left[\begin{array}{c}({\check{x}}_{k|k}^i-{\widehat{x}}_{k|k}^j)\odot ({\check{x}}_{k|k}^i-{\widehat{x}}_{k|k}^j)\\ {\widehat{r}}_1\end{array}\right]\hfill \end{array}$$

(77)

$$\begin{array}{cc}\hfill v^{\prime }=& \left[\begin{array}{c}2({\check{x}}_{k|k}^i-x_k^j)\odot {\tilde{x}}_{k|k}^i+{\tilde{x}}_{k|k}^i\odot {\tilde{x}}_{k|k}^i\\ 2r_1{\tilde{r}}_1+{\tilde{r}}_1{\tilde{r}}_1\end{array}\right]\hfill \end{array}$$

(78)

with ${\tilde{x}}_{k|k}^i=x_k^i-{\check{x}}_{k|k}^i$, ${\tilde{r}}_1=r_1-{\widehat{r}}_1$, and ${\left[a_1{a}_2{a}_3\right]}^{\mathrm{T}}\odot {\left[b_1{b}_2{b}_3\right]}^{\mathrm{T}}={\left[a_1{b}_1{a}_2{b}_2{a}_3{b}_3\right]}^{\mathrm{T}}$. The estimate of $\tilde{u}$ is given by

$${\widehat{u}}_{k|k}^{\prime }={\left[{\left(\tilde{H}\right)}^{\mathrm{T}}\tilde{W}\tilde{H}\right]}^{-1}{\left(\tilde{H}\right)}^{\mathrm{T}}\tilde{W}(\tilde{z}-\mathrm{E}\left[\tilde{v}\right])$$

(79)

with E$\left[\tilde{v}\right]$ obtained by deterministic sampling.

Finally, the location of the ith UAV is

$${\widehat{x}}_{k|k}^i=U{\left({\widehat{u}}_{k|k}^{\prime }\right)}^{1/2}+{\widehat{x}}_{k|k}^j$$

(80)

where $U=$ diag(sign(${\widehat{u}}_{k|k}^{\prime }-{\widehat{x}}_{k|k}^j$)) is a sign function.

The steps for cooperative localization in a swarm of UAVs can be summarized as follows. Firstly, the robust filter for integrated navigation based on the normal gamma distribution (RINNG) method described in Section 3 is utilized to estimate the error states, primarily addressing the uncertainty in measurement quality caused by outliers in external correction information. Based on this external correction information and the INS resolution, integrated navigation is performed to determine the positions of a subset of UAVs. Using these position estimates and estimates of their uncertainties, the statistical linearization of the distance measurement model is then carried out using the method outlined in Section 4. Finally, based on the linearized model, the augmented nonlinear least squares (ANLS) method is applied to estimate the positions of the other UAVs, ultimately yielding the localization results for the entire UAV swarm.

6. Simulation

The simulation considers three scenarios: The first scenario involves positioning using external correction information corrupted by outliers, primarily for evaluating the proposed robust filter for integrated navigation based on the normal gamma distribution (RINNG) method. The second scenario involves TDOA positioning among UAVs, primarily for evaluating the proposed augmented nonlinear least squares (ANLS) method. The third scenario is collaborative positioning of a swarm of UAVs, utilizing a unified scenario to comprehensively evaluate the methods proposed in this paper.

6.1. Scenario 1: Positioning Using Outlier Corrupted External Correction Measurements

Assume that the target has a constant-turn motion,

$$x_{k+1}=\left[\begin{array}{ccccc}1& {\displaystyle \frac{sin\omega T}{\omega }}& 0& {\displaystyle \frac{cos\omega T}{\omega }}& 0\\ 0& cos\omega T& 0& -sin\omega T& 0\\ 0& {\displaystyle \frac{1-cos\omega T}{\omega }}& 1& {\displaystyle \frac{sin\omega T}{\omega }}& 0\\ 0& sin\omega T& 0& cos\omega T& 0\\ 0& 0& 0& 0& 1\end{array}\right]x_k+w_k$$

(81)

where $x_k={\left[{\mathsf{x}}_k,{\dot{\mathsf{x}}}_k,{\mathsf{y}}_k,{\dot{\mathsf{y}}}_k,\omega \right]}^{\mathrm{T}}$ is the state vector that consists of position, velocity, and the turn rate, the sampling interval $T=1$s, the process noise $w_k\sim \mathcal{N}(0,Q_k)$ with $Q_k=$ diag$(q_1^{2}M,q_1^{2}M,q_2^{2}T)$, $q_1=0.1$ m/s², $q_2=1.74\times {10}^{-4}$, and

$$M=\left[\begin{array}{cc}{T}^3/3& T^2/2\\ T^2/2& T\end{array}\right]$$

(82)

The model of range and bearing measurements is

$$z_k=\left[\begin{array}{c}\sqrt{{\mathsf{x}}_k^2+{\mathsf{y}}_k^2}\\ arctan{\displaystyle \frac{{\mathsf{y}}_k}{{\mathsf{x}}_x}}\end{array}\right]+v_k$$

(83)

where $p\left(v_k\right)=0.9\mathcal{N}(0,R)+0.1\mathcal{N}(0,100R)$ with covariance $R=$diag$({\sigma }_r^2,{\sigma }_{\theta }^2)$ (${\sigma }_r=10$ m, ${\sigma }_{\theta }=\sqrt{10}$ mrad). The probability of outlier occurrence is 0.1. When an outlier appears, the measurement noise covariance becomes 100 times that under normal conditions.

Figure 4 shows the RMSEs of position and of velocity over 5000 Monte Carlo runs. As can be seen from the figure, the proposed RINNG method outperforms the UKF [26] in both position and velocity estimation, effectively addressing the challenge of outliers present in external correction information.

Figure 4. Simulation scenario 1: positioning using outlier corrupted external correction measurements. (a) RMSE of position estimation; (b) RMSE of velocity estimation.

6.2. Scenario 2: Mutual Positioning Using TDOA Measurements

Consider the TDOA localization scenario depicted in Figure 5a, where the positions of UAVs numbered 1 to 7 are known (as annotated in the figure), serving as sensors to locate an object with an unknown position. Compare the localization accuracy of the proposed ANLS and the classical TWLS [16] methods under different numbers of sensors and noise levels. Figure 5b, c, and d, respectively, present the localization errors from 1000 Monte Carlo simulations using $5,6,$ and 7 UAVs for positioning. In each figure, the horizontal axis represents the standard variance of measurement noise, and the vertical axis represents the positioning RMSE. It can be observed from the figures that with more sensors, the localization errors of both TWLS and ANLS decrease. When using the same number of sensors, larger noise results in poorer localization accuracy. In all scenarios, the proposed ANLS method significantly improves the localization accuracy compared to TWLS, with a reduction in localization error by more than 40%, validating the effectiveness of the proposed ANLS method.

Figure 5. Simulation scenario 2: mutual positioning using TDOA measurements. (a) The scenario; (b) positioning RMSE based on 5 sensors; (c) positioning RMSE based on 6 sensors; (d) positioning RMSE based on 7 sensors.

6.3. Scenario 3: UAV Swarm Collaborative Positioning

Consider the UAV swarm collaborative positioning scenario shown in Figure 6. The figure illustrates the true trajectories of the UAVs, with the labeled points indicating the starting positions of their flights. In the UAV swarm, UAVs labeled 1–5 are equipped with high-precision INSs and are assumed to receive external correction information. UAVs labeled 6–10 are equipped with low-precision INSs and rely on TDOA positioning with the assistance of UAVs 1–5 to improve their navigation accuracy. The error parameters of the INS for each UAV are summarized in Table 1. As shown in the table, the INS parameters of UAVs 1–5 are identical, while the INS performance of UAVs 6–10 is inferior to that of UAVs 1–5, with varying values among them.

Figure 6. Simulation scenario 3: 10 UAVs are considered for collaborative integrated navigation, where 5 of them are equipped with high precision INSs and capable of receiving external correction information.

Table 1. Parameter settings of INSs of UAVs.

No.	Accelerometer ($\mathsf{\mu }$g)		Gyroscope (∘/h)
	Const. Drift	Rand. Drift	Const. Drift	Rand. Drift
1–5	10	100	1	1
6	100	10,000	10	10
7	10	10,000	10	10
8	10	1000	1	10
9	10	1000	10	15
10	100	1000	10	15

Figure 7 illustrates the self-localization results of integrated navigation for UAVs 1–5 when external correction information is corrupted by outliers. It is assumed that, in the absence of interference, the external correction information is highly accurate, with a measurement noise covariance of $R_k=$ diag$({10}^{-10},{10}^{-10},{10}^{-4})$. However, each UAV has a 0.05 probability of receiving outliers in the correction information, in which case the measurement noise covariance increases to ${10}^6{R}_k$. As shown in the figure, the presence of outliers significantly impacts positioning accuracy. The UKF method produces fused localization results with substantial errors. In contrast, the proposed RINNG method, which accounts for the presence of outliers and adaptively adjusts the covariance of measurement noise, effectively mitigates the impact of outliers and provides robust self-localization results.

Figure 7. Comparison of integrated navigation results.

Table 2 compares the RMSE of the positioning for UAVs 1–5 as provided by the UKF and RINNG methods. The results show that RINNG achieves significantly higher positioning accuracy than UKF, demonstrating its robustness in handling outliers in external correction data.

Table 2. Time-averaged RMSE of UAVs 1–5.

No.	1	2	3	4	5
UKF	$38.0593$	$47.2109$	$47.0874$	$44.5690$	$58.7933$
RINNG	$3.5439$	$3.9141$	$3.9060$	$3.8053$	$4.3767$

Figure 8 shows the results of mutual positioning for UAVs 6–10, based on the self-localization results of UAVs 1–5, which serve as sensors. Due to the influence of outliers in external correction data, the UKF self-localization results exhibit significant errors. Building mutual positioning on this basis further propagates these errors, causing the positioning deviations of UAVs 6–10 to increase over time, eventually leading to divergence. The proposed statistical linearization model accounts for the uncertainties in self-localization, ensuring that the positioning results of UAVs 6–10 do not diverge over time.

Figure 8. Comparison of collaborative positioning results.

Figure 9 shows the RMSE of the positioning for each of the UAVs 6–10. As observed, due to insufficient consideration of various uncertainties, the positioning results provided by the UKF method [26] diverge in the later stages. In contrast, the proposed method adequately accounts for these uncertainties, suppressing their impact on positioning accuracy and keeping navigation errors within an acceptable range. The figure also presents results obtained using only RINNG without ANLS. It is evident that incorporating ANLS further improves the least squares positioning accuracy.

Figure 9. RMSEs of collaborative positioning.

Table 3 summarizes the performance of UAVs 6–10 and presents an ablation study of the proposed method. The localization of UAVs 6–10 is conducted in two steps: first, locating UAVs 1–5, and second, locating UAVs 6–10 based on the localization results of UAVs 1–5. In the table, UKF+UKF indicates that both steps are based on the UKF. RINNG+UKF indicates that the proposed RINNG is used for locating UAVs 1–5 to handle outliers, while UKF is employed in the second step. RINNG+ANLS indicates that both steps utilize the proposed methods. As shown in the table, the RINNG method demonstrates remarkable performance in handling outliers, while the ANLS method further improves localization accuracy in mutual localization.

Table 3. Time-averaged RMSE of UAVs 6–10 and an ablation study.

No.	6	7	8	9	10
UKF+UKF	$6.2566$	$24.0521$	$7.6684$	$6.3815$	$4.7194$
RINNG+UKF	$2.5273$	$10.3385$	$2.7722$	$3.8936$	$2.7534$
RINNG+ANLS	$2.2128$	$9.5732$	$2.6567$	$3.3472$	$2.3087$

The above results validate the effectiveness of the proposed method. The proposed RINNG method can effectively handle outliers in external correction data, while the statistical linearization approach accounts for the uncertainties in self-localization results. Additionally, the ANLS method further enhances positioning accuracy for nonlinear localization problems.

7. Conclusions

This paper investigates the multiple uncertainties present in cooperative navigation and positioning for UAV swarms. It primarily addresses challenges such as the uncertainty arising from outliers in external correction information, the uncertainty in the self-position estimation of leaders engaged in mutual localization within the swarm, and incomplete information processing due to the strong nonlinearity of mutual localization measurements. The basic idea of the proposed method is to quantify the primary source of uncertainty, namely the uncertainty in external correction information, using gamma variables. A statistical linearization method for nonlinear models incorporating gamma variables is then proposed. Based on the linearized model, the paper further applies an uncorrelated conversion method to realize nonlinear least squares positioning. When dealing with uncertainties as a whole, the modeling and evolution of uncertainties are considered in an integrated manner. Based on the proposed method, robust navigation and positioning for the entire UAV swarm can be achieved.

Simulation verifications were conducted in three scenarios. The proposed robust integrated navigation method effectively suppressed large positioning deviations caused by outliers. For mutual localization within the swarm, the proposed statistical linearization method and the least squares method based on augmented data effectively considered the uncertainty in leader positioning, enabling further utilization of nonlinear measurement information and achieving higher positioning accuracy. The proposed methods can be applied not only to cooperative navigation of UAV swarms but also to navigation and positioning in other multi-UAV system collaborations.