Cooperative Navigation for Low-Cost UAV Swarm Based on Sigma Point Belief Propagation

Abstract

As navigation is a key to task execution of micro unmanned aerial vehicle (UAV) swarm, the cooperative navigation (CN) method that integrates relative measurements between UAVs has attracted widespread attention due to its performance advantages. In view of the precision and efficiency of cooperative navigation for low-cost micro UAV swarm, this paper proposes a sigma point belief propagation-based (SPBP) CN method that can integrate self-measurement data and inter-UAV ranging in a distributed manner so as to improve the absolute positioning performance of UAV swarm. The method divides the sigma point filter into two steps: the first is to integrate local measurement data; the second is to approximate the belief of position based on the mean and covariance of the state, and pass message by augmentation, resampling and cooperative measurement update of the state to realize a low-complexity approximation to traditional message passing method. The simulation results and outdoor flight test results show that with similar performance, the proposed CN method has a calculation load more than 20 times less than traditional BP algorithms.

1. Introduction

In recent years, UAVs have received extensive attention in military and civil fields such as battlefield reconnaissance, aerial photography, agricultural plant protection, express transportation, disaster rescue, and power inspection, and UAV cluster technology has become a research hotspot due to its high efficiency and damage resistance [1,2]. Navigation information is crucial for reliable flight of UAVs. Especially in an intensive UAV formation flight system, the UAV swarm requires high-precision position, velocity and other information to conduct task planning, precise control, and collision avoidance [3,4]. At present, the Global Navigation Satellite System (GNSS) is the most widely used means of navigation for UAVs; the GNSS based on Real Time Kinematic (RTK) technology can provide UAVs with centimeter-level positioning precision [5]. However, in certain restricted environments such as city and forest, the GNSS receiver may be unavailable to search sufficient satellites for navigation solution, and costs are high if RTK is applied to all UAVs in a swarm [6].

In order to make full use of the advantages of UAV swarm and obtain reliable and better navigation performance, researchers began to study the cooperative navigation method that integrates relative measurements between UAVs. Compared with single UAV navigation method that merely integrates the local measurement data of the UAV, the CN method can correct its navigation result based on the geometric relationship and observation of the swarm and thus greatly improve the absolute navigation performance [7,8,9]. Many researchers focused on the optimization algorithm based cooperative positioning technology, created constraint equation set for the geometric relationship between all users, and established corresponding objective functions to solve the positioning result [10,11,12,13,14]. Due to the existence of noise and observation fault such as Non-line-of-sight (NLOS) of wireless ranging signal [15,16], the cooperative positioning problem is often non-convex and cannot be solved globally and optimally. Thereby, convex optimization technologies such as Semi-definite Programming and Cone Programming are required to relax the cooperative estimation problem [14]. The disadvantage of optimization algorithm based cooperative positioning technology is that under worse observation conditions, its performance worsens rapidly, and on-board Inertial Measurement Unit (IMU) and other navigation sensors cannot be fully used for measurement.

A factor graph provides an effective framework for dealing with cooperative navigation problems and is used for describing the linkage between the navigation state and observation at each vehicle of the cooperative network. To pass messages between the nodes in the factor graph, many belief propagation (BP) methods have been proposed [17,18,19,20,21]. Ref. [17] proposed a sum-product algorithm for wireless network (SPAWN), which realized high-precision indoor positioning on the basis of factor graph and message passing. Ref. [21] put forward a factor graph and message passing based H-SPAWN algorithm that can integrate GNSS information and peer-to-peer information to realize cooperative positioning. Ref. [22] provided a distributed positioning method based on sequential particle-based sum-product algorithm (SPSPA), which has the ability to perform online inference in factor graph with continuous variables and nonlinear local functions. Even when obtaining a good effect, these methods require a large number of particles to describe cooperative messages and have a large calculation load, making them hard to guarantee the real-time navigation performance of UAV swarm in the time of formation flight. In order to reduce the computation load of BP algorithm, Ref. [23] offered a cooperative positioning framework combining relative position estimation and optimization, which transforms a multi-dimensional BP problem into an one-dimensional problem, reduces the computation load, and can integrate Ultra-wideband (UWB), GNSS and Inertial Navigation System (INS) information to obtain precise navigation results. However, this framework requires that each UAV in the swarm can conduct GNSS calculation. Ref. [24] presents a cooperative positioning method in which the posteriori distribution of marginalized circulation factor graph is approximated by using sigma point, but the movement of the user is described using a motion model, regardless of the observation information beyond the measurement range. Hence, this method is suitable for indoor application instead of UAV flight scene. It is also unavailable to provide continuous and comprehensive navigation parameters.

In this paper, a sigma point belief propagation-based (SPBP) new CN method is proposed. In this method, sigma point filter integrates local observations from INS, GNSS, barometers, and so on; then the filter’s state vector is augmented and the cooperative measurements are updated iteratively to realize the message passing between UAVs, and further improve the absolute navigation performance of UAV swarm. Compared with the traditional BP algorithm, this paper uses sigma points to replace the particles obtained by Monte Carlo method and constructs the state equation of the cooperative navigation system based on INS, so as to replace the state equation based on motion model. Most importantly, sigma point filter and BP algorithm are combined and expanded to 3D UAV flight urban scene; moreover, IMU, GNSS, and other local multi-source navigation information are incorporated into the CN framework, improving the performance and efficiency of the CN algorithm.

The structure of this paper is as follows: Section 2 provides the preliminaries of sigma point filter and describes the CN problem based on factor graph and BP; Section 3 proposes an SPBP based CN method; the simulations are described in Section 4; and flight tests are described in Section 5; Section 6 discusses the research implications and limitations of the proposed method; and the final conclusion is presented in Section 7.

2. Preliminaries and Description of Problem

2.1. Preliminary on Sigma Point Filter

The basic idea of sigma point filter (SP filter, also known as unscented Kalman filter) is to approximate to a nonlinear density function by using a certain number of sample points and transfer the state of a real system while capturing accurate mean and variance of the state [25]. Therefore, the selection of sigma points is crucial for sigma point filter and is realized by Unscented Transformation [26]. It is assumed that $N$-dimensional random vector $X$ is transformed into an M-dimensional random vector $Y$ after subjecting to $\mathrm{h}(\cdot )$ nonlinear transformation, namely:

$$Y=\mathrm{h}\left(X\right)$$

(1)

Given the mean $\overline{X}$ and variance $P_{XX}$ of $X$, the ( $2N+1$) sigma points of $X$ can be reproduced according to $\overline{X}$:

$$\begin{array}{c}{\chi }^{\left(0\right)}=\overline{\mathit{X}}\\ {\chi }^{\left(i\right)}=\overline{\mathit{X}}+{\left(\sqrt{(N+\lambda )P_{XX}}\right)}_{\left(i\right)} i=1,2,\dots ,N\\ {\chi }^{\left(i\right)}=\overline{X}-{\left(\sqrt{(N+\lambda )P_{XX}}\right)}_{(i-N)} i=N+1,N+2,\dots ,2N\end{array}$$

(2)

where ${\left(\sqrt{\left(N+\lambda \right)P_{XX}}\right)}_{\left(i\right)}$ represents the $i$-th column of the square root of the lower triangular decomposition of matrix $\sqrt{\left(N+\lambda \right)P_{XX}}$; $\lambda$ determines the distance between the sampling point and the mean. The sigma point produced by nonlinear transformation can be calculated as follows:

$${\mathit{Y}}^{\left(i\right)}=\mathrm{h}\left({\mathit{X}}^{\left(i\right)}\right) i=0,1,2,\dots ,2N$$

(3)

Hence, the mean $\overline{Y}$, variance $P_{YY}$, and the covariance $P_{XY}$ of $X$ and $Y$ can be approximated as:

$$\overline{Y}\approx {\displaystyle \sum _{i=0}^{2N}{W}_i^{\left(m\right)}}{Y}^{\left(i\right)}$$

(4)

$$P_{YY}={\displaystyle \sum _{i=0}^{2N}{W}_i^{\left(c\right)}}\left[Y^{\left(i\right)}-\overline{Y}\right]{\left[Y^{\left(i\right)}-\overline{Y}\right]}^{\mathrm{T}}$$

(5)

$$P_{XY}={\displaystyle \sum _{i=0}^{2N}{W}_i^{\left(c\right)}}\left[X^{\left(i\right)}-\overline{X}\right]{\left[Y^{\left(i\right)}-\overline{Y}\right]}^{\mathrm{T}}$$

(6)

where $W_i^{\left(m\right)}$ and $W_i^{\left(c\right)}$ are the corresponding weights, which can be calculated as:

$$\begin{array}{c}{W}_i^{\left(m\right)}=W_i^{\left(c\right)}=\frac{1}{2(N+\lambda )}, i=1,2,\dots ,2N\\ W_0^{\left(m\right)}=\frac{\lambda }{N+\lambda },W_0^{\left(c\right)}=\frac{\lambda }{N+\lambda }+\left(1-{\alpha }^2+\beta \right)\end{array}$$

(7)

where $\alpha$ and $\beta$ are constants and nonnegative.

Considering that the variable $X$ and observation $Z$ satisfy $Z=Y+\xi =\mathrm{h}\left(X\right)+\xi$ at time $k$, where $\xi$ is the additive measurement noise with variance of $R$, the posteriori probability density function of $X$ can be obtained based on $Z$:

$$p\left(X|Z\right)\propto p\left(Z|X\right)p\left(X\right)$$

(8)

The specific steps to realize the posterior probability density estimation of Equation (8) through SP filter are as follows [26]:

Step 1: Sampling sigma points as per Equation (2) at time $k-1$;

Step 2: Calculating the one-step predicted value at time $k$:

$${\chi }_{k\mid k-1}^{*\left(i\right)}=f\left({\chi }^{\left(i\right)},{\mathit{u}}_{k-1}\right) i=0,1,2,\dots 2N$$

(9)

$${\widehat{X}}_{k|k-1}\approx {\displaystyle \sum _{i=0}^{2N}{W}_i^{\left(m\right)}}{\chi }_{k|k-1}^{\ast \left(i\right)}$$

(10)

$$P_{X{X}_{k|k-1}}={\displaystyle \sum _{i=0}^{2N}{W}_i^{\left(c\right)}}\left[{\chi }_{k|k-1}^{\ast \left(i\right)}-{\widehat{X}}_{k|k-1}\right]{\left[{\chi }_{k|k-1}^{\ast \left(i\right)}-{\widehat{X}}_{k|k-1}\right]}^T$$

(11)

where f(∙) is the one-step prediction equation of the system, that is, the discrete form of the system state equation; u_k−1 is the input of the system, and ${\chi }_{k|k-1}^{\ast \left(i\right)}$ is the one-step predicted sigma point.

Step 3: Computing the observed value $Y_{k|k-1}^{\left(i\right)}$ and predicted value ${\overline{Y}}_{k|k-1}$ as per Equations (3) and (4), and the corresponding covariance matrices $P_{Y{Y}_{k|k-1}}$ and $P_{X{Y}_{k|k-1}}$ are calculated according to Equations (5) and (6);

Step 4: Calculating the posterior probability density estimation:

$$K_k=P_{X{Y}_{k|k-1}}{\left(P_{Y{Y}_{k|k-1}}+R\right)}^{-1}$$

(12)

$${\widehat{X}}_k={\widehat{X}}_{k|k-1}+K_k\left(Z_k-{\overline{Y}}_{k|k-1}\right)$$

(13)

$$P_{X{X}_k}=P_{X{X}_{k|k-1}}-K_k{P}_{Y{Y}_{k|k-1}}{K}_k^{-1}$$

(14)

As revealed in the above process of sigma point filter, the number of sigma points depends on the number of dimensions of $X$, namely the former increases with the increase in the latter.

2.2. Belief Propagation for UAV Swarm

Figure 1 shows a flight scene of low-cost micro UAV swarm. In urban areas, the navigation system of UAV may not receive enough number of satellite signals for positioning. For this reason, all UAVs are equipped with GNSS receiver as well as some conventional onboard navigation devices, such as INS and barometer, and are also mounted with UWB to provide the ranging information between UAVs. To meet the demand for CN, it is also essential to provide a wireless communication network for exchanging CN information.

Provided that all UAVs in the swarm belong to a set $U$, the navigation state of UAV $m$ at time $k$ is defined as $X_m^k$; the satellite observation and barometer output at time $k$ are defined as $Z_m^{k,gnss}$ and $Z_i^{k,baro}$, respectively; all the IMU measurements between $k-1$ and $k$ are defined as $Z_m^{k-1,imu}$; and all UWB observations at time $k$ are defined as:

$$Z_m^{k,uwb}=\left\{r_{m,n}^{k,uwb}|n\in U_m\right\}$$

(15)

where U_m is the set of all UAVs that have UWB ranging links with UAV m. The navigation state set and observation set of all UAVs in the swarm are defined as:

$${\mathbb{X}}^k=\left\{X_m^k|m\in U\right\}$$

(16)

$${\mathbb{Z}}^k=\left\{{\mathbb{Z}}_m^k|m\in U\right\}=\left\{Z_m^{k-1,imu},Z_m^{k,gnss},Z_m^{k,baro}{Z}_m^{k,uwb}|m\in U\right\}$$

(17)

Based on the above definitions, the joint posteriori probability distribution function of the navigation state of the UAV swarm is given by:

$$p\left({\mathbb{X}}^k|{\mathbb{Z}}^k,{\mathbb{X}}^{0:k-1}\right)$$

(18)

According to the flight characteristics and sensor characteristics of UAV swarm, the following two conditions can be assumed [23]: a. The navigation state of each UAV at time $k$ is only related to time $k-1$, and obeys the standard Markov assumption; b. The UWB, GNSS and barometric measurements obtained by UAV are independent and only related to the state at the present time. Based on these two assumptions, the joint posteriori probability distribution function can be factorized in terms of a priori information and individual process and measurement models. This factorization can be written as

$$\begin{array}{cc}\hfill p\left({\mathbb{X}}^k\mid {\mathbb{Z}}^{1:k},{\mathbb{X}}^{0:k-1}\right)\propto p\left({\mathbb{X}}^0\right)& \prod _{t=1}^k\left[p\left({\mathbb{X}}^t\mid {\mathbb{Z}}^{t-1,imu},{\mathbb{X}}^{t-1}\right)p\left({\mathbb{Z}}^{t,uwb}\mid {\mathbb{X}}^t\right)\right.\hfill \\ \hfill & \left.p\left({\mathbb{Z}}^{t,gnss}\mid {\mathbb{X}}^t\right)p\left({\mathbb{Z}}^{t,baro}\mid {\mathbb{X}}^t\right)\right]\hfill \\ \hfill =p\left({\mathbb{X}}^0\right)& \prod _{t=1}^k\left[\prod _{m\in U}p\left({\mathit{X}}_m^t\mid {\mathit{Z}}_m^{t-1,imu},{\mathit{X}}_m^{t-1}\right)\prod _{m\in U}p\left({\mathit{Z}}_m^{t,gnss}\mid {\mathit{X}}_m^t\right)\right.\hfill \\ \hfill & \left.\prod _{m\in U}p\left(Z_m^{t,baro}\mid {\mathit{X}}_m^t\right)\prod _{r_{m,n,t}\in {\mathbb{Z}}^t}p\left(r_{m,n}^{t,uwb}\mid {\mathit{X}}_m^t,{\mathit{X}}_n^t\right)\right]\hfill \end{array}$$

(19)

where $p\left({\mathbb{X}}^0\right)$ represents the priori factor of the navigation state. This factorization can be described by the factor graph composed of factor nodes and variable nodes as presented in Figure 2, where factor node $l_m^{k-1,imu}=p\left(X_m^k|Z_m^{k-1,imu},X_m^{k-1}\right)$ represents the motion of UAV obtained on the basis of IMU output; $l_m^{k,gnss}=p\left(Z_m^{k,gnss}|X_m^k\right)$ and $l_m^{k,baro}=p\left(Z_m^{k,baro}|X_m^k\right)$ are respectively the functions between UAV m and GNSS data, and between UAV m and barometric height;$l_{m,n}^{k,uwb}=p\left(r_{m,n}^{k,uwb}\mid {\mathit{X}}_m^k,{\mathit{X}}_n^k\right)$ represents the function of mutual ranging between UAV m and UAV n.

Based on Figure 2 and Equation (19), the approximated marginal posteriori probability density function $b\left(X_m^k\right)\approx p\left(X_m^k|{\mathbb{Z}}_m^k\right)$ of variable node $X_m^k$ is called as the belief, which can be calculated by iteration of BP message passing on the factor graph. At the $\tau$-th iteration at time $k$, the belief of $X_m^k$ can be obtained as follows:

$$b^{\left(\tau \right)}\left({\mathit{X}}_m^k\right)\propto {\mathfrak{q}}_{l_m^{k-1,imu}}\to {\mathit{X}}_m^k{\mathfrak{q}}_{l_m^{k,gsss}}\to {\mathit{X}}_m^k{\mathfrak{q}}_{l_m^{k,haro}}\to {\mathit{X}}_m^k\prod _{n\in U_m}{\mathfrak{q}}_{\begin{array}{c}{l}_{m,n}^{k,uwb}\to {\mathit{X}}_m^k\end{array}}^{\left(\tau \right)}$$

(20)

The forms of messages involved in the above equation are as follows:

① The prediction message passed from factor node $l_m^{k-1,imu}$ to variable node $X_m^k$

$${\mathfrak{q}}_{l_m^{k-1,imu}\to X_m^k}=l_m^{k-1,imu}b\left(X_m^{k-1}\right)$$

(21)

② The GNSS correction message passed from factor node $l_m^{k,gnss}$ to variable node $X_m^k$

$${\mathfrak{q}}_{l_m^{k,gnss}\to X_m^k}=l_m^{k,gnss}$$

(22)

③ The height correction message passed from barometer factor $l_m^{k,baro}$ to variable node $X_m^k$

$${\mathfrak{q}}_{l_m^{k,baro}\to X_m^k}=l_m^{k,baro}$$

(23)

④ The cooperative correction message passed from UWB factor $l_{m,n}^{k,uwb}$ to variable node $X_m^k$

$${\mathfrak{q}}_{l_{m,n}^{k,uwb}\to {\mathit{X}}_m^k}^{\left(\tau \right)}=\int l_{m,n}^{k,uwb}{b^{\prime }}^{(\tau -1)}\left({\mathit{X}}_n^k\right)d{\mathit{X}}_n^k$$

(24)

where ${b^{\prime }}^{\left(\tau \right)}\left(X_n^k\right)$ is the belief passed by UAV $n$ to factor node $l_{m,n}^{k,uwb}$ at the $\tau$-th iteration:

$${b^{\prime }}^{\left(\tau \right)}\left({\mathit{X}}_n^k\right)={\mathfrak{q}}_{l_n^{k-1,imu}}\to {\mathit{X}}_n^k{\mathfrak{q}}_{l_n^{k,gnss}}\to {\mathit{X}}_n^k{\mathfrak{q}}_{l_n^{k,baro}}\to {\mathit{X}}_n^k\prod _{m^{\prime }\in U_n\setminus \left\{m\right\}}{\mathbf{q}}_{l_{n,m^{\prime }}^{k,nwb}\to {\mathit{X}}_n^k}^{\left(\tau \right)}$$

(25)

where $m^{\prime }\in U_n\left\{m\right\}$ represents the set of all UAVs that have ranging links with UAV $n$, except UAV $m$. For the SPAWN message passing scheme, ${b^{\prime }}^{\left(\tau \right)}\left(X_n^k\right)$ can be directly replaced with $b^{\left(\tau \right)}\left(X_n^k\right)$ without recalculation [17].

3. Sigma Point Belief Propagation

Section 2.1 shows an SP filter based method for posteriori probability estimation of variables. According to a comparison among Equations (8), (19) and (20), the expression of belief is similar to Equation (8), and the belief of navigation state can also be calculated by SP. Using a mean vector and covariance matrix to approximate the belief of each UAV’s navigation state is conducive to reducing communication cost and calculation load, and low-complexity approximation to the traditional BP algorithm. This section will expound the SPBP method in detail.

3.1. Navigation State Model and Time Update

Taking Earth-fixed frame as the navigation coordinate frame, the motion of UAV from $k-1$ to $k$ is modeled. Based on the measurements of IMU, the time update of the motion state of UAV is obtained as follows [27]:

$$\left\{\begin{array}{c}\dot{\mathit{Q}}=0.5\mathit{Q}\circ \left(\tilde{\mathit{\omega }}-{\mathit{\omega }}_{ie}-{\mathit{\omega }}_{en}-{\mathit{\omega }}_r-{\mathit{\omega }}_{\epsilon }\right)\\ \dot{\mathit{v}}={\mathit{C}}_b^n\left(\tilde{\mathbf{a}}-{\nabla }_r-\left(2{\mathit{\omega }}_{ie}+{\mathit{\omega }}_{en}\right)\times \mathit{v}\right)+\mathit{g}\\ \dot{\mathit{p}}=\mathit{v}\end{array}\right.$$

(26)

where $Q$ is the attitude quaternion, including the one-dimensional scalar $Q_0$ and the three-dimensional vectors $Q_1$, $Q_2$ and $Q_3$, ∘ represents the quaternion multiplication, $v$ is the three-dimensional velocity, $p$ is the three-dimensional position, $\tilde{\mathit{\omega }}$ is the measurement of the gyroscope, ${\mathit{\omega }}_{ie}$ is the angular velocity of the Earth’s rotation, ${\mathit{\omega }}_{en}$ is the rotational angular velocity of the carrier relative to the Earth; ${\mathit{\omega }}_r$ is the random walk error of the gyroscope, ${\mathit{\omega }}_{\epsilon }$ is the measurement noise of the gyroscope, $C_b^n$ is the rotation matrix from body frame to navigation frame, $\tilde{a}$ is the accelerometer measurement, ${\nabla }_r$ is the random walk error of the accelerometer, $\times$ represents the cross multiplication of vectors, $G$ is the gravity vector, and $r$ is the position vector with respect to the Earth Centered Earth fixed frame. The random walk error update equation of the gyroscope and accelerometer is:

$$\{\begin{array}{c}{\dot{\mathit{\omega }}}_r={\mathit{\omega }}_n\\ {\dot{\nabla }}_r={\nabla }_n\end{array}$$

(27)

where ${\mathit{\omega }}_n$ and ${\nabla }_n$ are the random walk drive noises of the gyroscope and accelerometer, respectively, which are usually supposed to be zero mean white noises. The covariance of noise can be obtained by ALLAN variance method or other IMU error analysis methods.

Since the scalar part of the normalized quaternion can be obtained from the vector part, in order to reduce the dimension of the SP filter, the initial state vector is created on the basis of vectors $Q_1$, $Q_2$ and $Q_3$, as well as the position vector, velocity vector, gyro random walk error ${\mathit{\omega }}_r$ and accelerometer random walk error ${\nabla }_r$:

$$\mathit{x}={\left[Q_1 Q_2 Q_3 {\mathit{v}}^{\mathrm{T}}{\mathit{p}}^{\mathrm{T}}\right]}^{\mathrm{T}}{\left({\mathit{\omega }}_r\right)}^{\mathrm{T}}{\left({\nabla }_r\right)}^{\mathrm{T}}\mathrm{T}$$

(28)

The system noise is:

$$\mathit{W}={\left[{\left({\mathit{\omega }}_n\right)}^{\mathrm{T}}{\left({\mathit{\omega }}_{\epsilon }\right)}^{\mathrm{T}}{\left({\nabla }_n\right)}^{\mathrm{T}}\right]}^{\mathrm{T}}$$

(29)

When the sigma point filter is updating the time, the system noise should be augmented to the system state, and the augmented system state vector is $\mathit{X}={\left[\begin{array}{cc}{\mathit{x}}^{\mathrm{T}}\hfill & {\mathit{W}}^{\mathrm{T}}\hfill \end{array}\right]}^{\mathrm{T}}$. By combining and discretizing Equations (26) and (27), the one-step prediction equation of the navigation state can be obtained, that is, Equation (9). Then, the attitude, velocity and position represented by each sigma point can be updated in combination with the input of IMU as per Equations (9)–(11). It should be noted that for observations from satellites, barometers and UWB, if the observation error does not belong to additive white noise (e.g., error parameters such as the receiver clock error of equivalent pseudo range observation), the corresponding error vector should also be augmented to the system state vector. Apparently, the IMU measurement based time update of sigma point filter corresponds to the passing of message ${\mathbf{q}}_{l_m^{k-1,imu}}\to {\mathit{X}}_m^k$ in BP. At time $k-1$, the belief message passed by variable node ${\mathit{X}}_m^{k-1}$ to IMU factor $l_m^{k-1,imu}$ is approximated to the mean ${\widehat{\mathit{X}}}_m^{k\mid k-1}$ and variance ${\mathit{P}}_{X{X}_m}^{k\mid k-1}$ of the state.

3.2. Measurement Model and Measurement Update

3.2.1. Local Measurement Model and Measurement Update

In this paper, two measurements, GNSS information and barometric altitude, are considered in addition to IMU. Regarding whether the satellites searched by GNSS receiver can be positioned directly or not, the GNSS measurement can be divided into position measurement and pseudo-range measurement. The position measurement can be modeled as follows [28]:

$${\tilde{p}}_m^G=p_m^G+{\nu }_m^G$$

(30)

where ${\nu }_m^G$ is the GNSS position observation noise of UAV $m$ and can be modeled as a white noise with covariance of $R_m^G$. For position measurement, the SP filter does not need to augment the system state.

The pseudo range measurement can be modeled as [29]:

$${\tilde{\rho }}_m^{S_i}={\rho }_m^{S_i}+\delta t_m^{\rho }+{\zeta }_m^{S_i}+\Delta {\rho }_m^{S_i}+{\nu }_m^{S_i}$$

(31)

where ${\rho }_m^{S_i}$ is the true distance between UAV $m$ and satellite $S_i$; $\delta t_m^{\rho }$ is the distance offset corresponding to the equivalent clock error of the receiver; ${\zeta }_m^{S_i}$ is the error offset caused by the ionosphere and troposphere; $\Delta {\rho }_m^{S_i}$ is the error caused by multipath or NLOS; and ${\nu }_m^{S_i}$ is the receiver noise with covariance of receiver noise. The errors caused by ionosphere and troposphere can be eliminated by using the corresponding correction model or the information broadcasted by Satellite-Based Augmentation System (SBAS); the error caused by multipath or NLOS can be detected in some integrity methods [30,31,32]. When the detected error caused by multipath or NLOS is large, the pseudo range measurement can be deleted directly. For pseudo range measurement, the equivalent receiver clock error $\delta t_m$ should be augmented to system state. The time update equation of $\delta t_m$ is shown below:

$$\delta {\dot{t}}_m^{\rho }=\delta t_m^{\rho }+{\nu }_{t_m^{\rho }}$$

(32)

where ${\nu }_{t_m^{\rho }}$ is the equivalent clock error deviation noise.

The barometric height can be modeled as:

$${\tilde{h}}_m^{baro}=h_m+{\nu }_{}^{baro}$$

(33)

where $h_m$ is the true height of UAV $m$; and ${\nu }_{}^{baro}$ is the barometer measurement noise with variance of $R_{}^{baro}$.

After the local measurements are modeled, the system state should be augmented again as per the noise form when there is non-additive noise in the measurement model and the dimension of the augmented state is $N$. The error augmented to system state vector in local measurement also requires time update. In face of upcoming measurement, the measurement can be updated as per Equations (3)–(6) and (12)–(14). $b^{\left(0\right)}\left(X_m^k\right)\propto {\mathfrak{q}}_{l_m^{k-1,imu}\to X_m^k}{\mathfrak{q}}_{l_m^{k,gnss}\to X_m^k}{\mathfrak{q}}_{l_m^{k,baro}\to X_m^k}$ is the belief corresponding to the mean ${\widehat{X}}_m^{\left(0\right)k}$ and covariance $P_{X{X}_m}^{\left(0\right)k}$ of the local measurement state updated at time $k$.

3.2.2. Cooperative Measurement Model and Measurement Update

The UWB measurement within the visual range based on Time of Flight (TOF) can be modeled as [33,34]:

$${\tilde{d}}_{m,n}=\Vert p_m-p_n\Vert +{\nu }^{uwb}$$

(34)

where ${\tilde{d}}_{m,n}$ is the UWB ranging value between UAV $m$ and UAV $n$; $\Vert \cdot \Vert$ represents the Euclidean distance; and ${\nu }^{uwb}$ is the ranging noise with variance of $R^{uwb}$.

In order to effectively integrate the cooperative measurement of UWB, the position vector of adjacent UAV should be augmented to the system state. After the augmentation, the state vector of SP filter for UAV $m$ is:

$${\mathit{X}}_m^a={\left[{\left({\mathit{X}}_m\right)}^{\mathrm{T}}{\left({\mathit{p}}_{U_m}\right)}^{\mathrm{T}}\right]}^{\mathrm{T}}$$

(35)

where ${\mathit{p}}_{U_{{m}}}={\left[{\left({\mathit{p}}_{n_1}\right)}^{\mathrm{T}}{\left({\mathit{p}}_{n_2}\right)}^{\mathrm{T}}\dots {\left({\mathit{p}}_{n_{V_{{m}}}}\right)}^{\mathrm{T}}\right]}^{\mathrm{T}}$ is a vector composed of the positions of all UAVs that have ranging links with UAV m after the augmentation, the dimension of the state is $N^a$. Assuming that the mean and covariance of the state variable corresponding to the message passed by UAV $n$ to factor node $l_{m,n}^{k,uwb}$ after the $\tau$-th iteration at time $k$ are ${\widehat{X}}_n^{\left(\tau \right)k}$ and $P_{X{X}_n}^{\left(\tau \right)k}$ respectively, the positions ${\widehat{p}}_n^{\left(\tau \right)k}$ and $P_{p{p}_n}^{\left(\tau \right)k}$ can be obtained from ${\widehat{X}}_n^{\left(\tau \right)k}$ and $P_{X{X}_n}^{\left(\tau \right)k}$. Hence, the mean and covariance of the augmented state vector of UAV m are as presented below:

$$\begin{gathered} {\widehat{X}}_{X{X}_m}^{\left(\tau \right)k,a}\triangleq {[{\left({\widehat{X}}_{X{X}_m}^{\left(\tau \right)k}\right)}^{\mathrm{T}} {\widehat{p}}_{n_1}^{\left(\tau \right)k}\dots {\widehat{p}}_{n_{U_m}}^{\left(\tau \right)k}]}^{\mathrm{T}} \\ {P}_{X{X}_m}^{\left(\tau \right)k,a}\triangleq \mathrm{diag}\left(P_{X{X}_m}^{\left(\tau \right)k},P_{p{p}_{n_1}}^{\left(\tau \right)k},\dots P_{p{p}_{n_{U_m}}}^{\left(\tau \right)k}\right) \end{gathered}$$

(36)

where $\mathrm{diag}(\cdot )$ represents the block diagonal matrix whose diagonal blocks are the listed matrices. A sigma point can be regenerated as per the mean and covariance of Equation (36), followed by measurement update and completion of the $\left(\tau +1\right)$-th iteration, obtaining the belief $b^{\left(\tau +1\right)}\left(X_m^k\right)$. Noteworthy, the position vector is augmented without time update.

3.3. Algorithm Description

Taking the navigation state of UAV $m$ as an example, the proposed method is described in detail as shown in Algorithm 1 and Figure 3.

Algorithm 1 The Detailed Recursion Process of Navigation State Based on SPBP (Example of UAV m)

1: INPUT: Initial system state $X_m^0$, including the initial mean ${\widehat{X}}_m^0$ and the initial covariance $P_{X{X}_m}^0$ Measurements of IMU, GNSS, barometer and UWB

OUTPUT: Navigation results of UAV m, including position, velocity and attitude

CALCULATE:

2: for time step $t$ = 1 to T do

3:      for UAV $m\in U$ in parallel do

4:          Sample $2N+1$ sigma points according to Equation (2)

5:          Time update as per Equations (26), (27) and (9)–(11), calculate the message              passed by IMU factor $l_m^{t-1,imu}$ to variable node $X_m^{t-1}$ at time k, and obtain the              mean ${\hat{X}}_m^{t|t-1}$ and covariance $P_{X{X}_m}^{t|t-1}$ of the state

6:      end parallel

7:      for UAV $m\in U$ in parallel do

8:        Calculate the belief $b^{\left(0\right)}\left(X_m^t\right)=\propto {\mathfrak{q}}_{l_m^{t-1,imu}\to X_m^k}{\mathfrak{q}}_{l_m^{t,gnss}\to X_m^t}{\mathfrak{q}}_{l_m^{t,baro}\to X_m^t}$, use Equations            (3)–(6) and (12)–(14) to complete the measurement update of            local measurements, and obtain the mean ${\hat{X}}_m^{\left(0\right)t}$ and covariance $P_{X{X}_m}^{\left(0\right)t}$ of the state

9:      end parallel

10:    for UAV $m\in U$ in parallel do

11:        while ( $\Vert {\widehat{p}}_m^{\left(\tau +1\right)t}-{\widehat{p}}_m^{\left(\tau \right)t}\Vert /\Vert {\widehat{p}}_m^{\left(\tau +1\right)t}\Vert <\alpha$) do

12:            Propagate the mean ${\widehat{p}}_m^{\left(\tau \right)t}$ and covariance matrix $P_{p{p}_m}^{\left(\tau \right)t}$ of the position                  corresponding to message $b^{\left(\tau \right)}\left(X_m^t\right)$

13:            Receive the mean and covariance matrix of the position sent by adjacent UAV

14:             Augment the state as per Equation (35) and obtain the mean ${\widehat{X}}_{X{X}_m}^{\left(\tau \right)t,a}$ and                   covariance $P_{X{X}_m}^{\left(\tau \right)t,a}$ of the new state

15:            Resample $2N^a+1$ sigma points according to Equation (2)

16:            Complete the cooperative measurement update of UWB observation as per                  Equations (3)–(6) and (12)–(14), and obtain the mean ${\widehat{X}}_m^{\left(\tau +1\right)t,a}$ and                  covariance $P_{X{X}_m}^{\left(\tau +1\right)t,a}$ of the state quantity corresponding to belief                  $b^{\left(\pi +1\right)}\left(X_m^t\right)$

17:        end while

18:      end parallel

19:    for UAV $m\in U$ in parallel do

20:        Extract the navigation states from the final iteration results ${\widehat{X}}_m^{t,a}$ and $P_{X{X}_m}^{t,a}$, and              outputthe navigation results

21:    end parallel

22: end for

Step 1 (Algorithm 1: 1) is to initialize the state vector $X_m^0$ of SP filter, and set the initial mean ${\hat{X}}_m^0$ and initial covariance $P_{X{X}_m}^0$, which are the bases for the time recursion of the SP filter.

Step 2 (Algorithm 1: 3–6) is to sample sigma points as per the mean ${\hat{X}}_m^{t-1}$ and covariance $P_{X{X}_m}^{t-1}$ of the state at the previous time, and complete time update of the system based on the IMU measurements, obtain the mean ${\widehat{X}}_m^{t|t-1}$ and covariance $P_{X{X}_m}^{t|t-1}$ of the state quantity.

Step 3 (Algorithm 1: 7–9) is to complete local measurement update as per GNSS measurements, barometric height and other local measurements, obtain the mean ${\widehat{X}}_m^{\left(0\right)t}$ and covariance $P_{X{X}_m}^{\left(0\right)t}$ of the state. This mean and covariance correspond to belief $b^{\left(0\right)}\left(X_m^t\right)\propto {\mathfrak{q}}_{l_m^{t-1,imu}\to X_m^k}{\mathfrak{q}}_{l_m^{t,gnss}\to X_m^t}{\mathfrak{q}}_{l_m^{t,baro}\to X_m^t}$.

Step 4 (Algorithm 1: 10–17) is to iterate the cooperative messages. The iterative loop is divided into 4 stages:

① Propagate the mean ${\widehat{p}}_m^{\left(\tau \right)t}$ and covariance $P_{p{p}_m}^{\left(\tau \right)t}$ of position corresponding to the belief ${b^{\prime }}^{\left(\tau \right)}\left(X_m^t\right)$, receive the mean and covariance of the position of adjacent UAV, and then augment the state to obtain the mean ${\widehat{X}}_{X{X}_m}^{\left(\tau \right)t,a}$ and covariance $P_{X{X}_m}^{\left(\tau \right)t,a}$ of a new state;

② Resample $2N^a+1$ sigma points;

③ Update the cooperative measurement and obtaining the mean ${\widehat{X}}_m^{\left(\tau +1\right)t,a}$ and covariance $P_{X{X}_m}^{\left(\tau +1\right)t,a}$ of the state quantity corresponding to belief $b^{\left(\tau +1\right)}\left(X_m^t\right)$;

④ Judge whether the convergence condition $\Vert {\widehat{p}}_m^{\left(\tau +1\right)t}-{\widehat{p}}_m^{\left(\tau \right)t}\Vert /\Vert {\widehat{p}}_m^{\left(\tau +1\right)t}\Vert <\alpha$ is met or not; if yes, exiting the loop; it not, skipping to stage 1). $\alpha$ can be selected as per the empirical value.

Step 5 (Algorithm 1: 19–21) is to extract and output the navigation results ${\widehat{X}}_m^t$ and $P_{X{X}_m}^t$, and then go to Step 2 to start the next epoch.

3.4. Complexity Analysis of SPBP

For the SPBP method proposed in this paper, the main calculation load is to calculate the square root of the covariance matrix when selecting sigma points as per Equation (2). An effective calculation can be realized by Cholesky decomposition [35], with complexity being cube of the dimension of the state and also cube of particle number $N_{spbp}$. Within an epoch, the computation load of this method is $\mathcal{O}\left({\left(N\right)}^3+{\left(N^a+3M_{link}\right)}^3{I}_{sp}\right)$, where $M_{link}$ is the number of adjacent UAVs and $I_{sp}$ is the number of iterations. In contrast, for traditional NBP method, its computation complexity is linear in terms of $M_{link}$ and quadratic in terms of particle number. Hence, the computation complexity of traditional NBP method is $\mathcal{O}\left(I_{nbp}{\left(N_{nbp}\right)}^2{M}_{link}\right)$ regarding the state estimation requiring $I_{nbp}$ iterations and $N_{nbp}$ particles. However, the particle number for SPBP is much less than that for NBP in general. Thereby, SPBP method has far higher calculation efficiency than NBP method.

In terms of communication traffic, the SPBP method takes the mean ${\widehat{p}}_m^{\left(\tau \right)t}$ and covariance $P_{p{p}_m}^{\left(\tau \right)t}$ of the estimated position as the belief of the position. Compared with the NBP method that requires transferring sub-particles, the SPBP method obviously has a small communication load.

4. Simulations

4.1. Simulation Configuration

Based on a flight setting in urban scene, a UAV swarm containing 12 UAVs is simulated to verify the proposed SPBP method. Figure 4 and Figure 5 display the initial position and horizontal trajectories of the UAV swarm, respectively. It is assumed that every UAV can interact with the UAVs within the working distance of the UWB transceiver, 60 m. In order to fully evaluate the performance of this method, all the results shown in this section are based on 200 Monte Carlo simulation experiments where each simulation takes 1000 s.

The parameters used for sensor configuration and simulation are as listed in

Table 1. Sensor configuration and parameters.

Sensor	Parameter	Value
Gyro	Random walk drive noise	3°/h
	White noise error	3°/h
	Output frequency	50 Hz
Accelerometer	Random walk drive noise	10−4 g
	Output frequency	50 Hz
Barometer	Gaussian noise standard dev.	0.5 m
	Output frequency	5 Hz
GNSS	Position noise standard dev.	5 m
	Pseudo range noise standard dev.	5 m
	Initial equivalent clock error	60 m
	Equivalent clock drift noise	10 m
	Output frequency	5 Hz
UWB	Gaussian noise standard dev.	0.15 m
	Output frequency	5 Hz

[36]. Each UAV is equipped with low-cost IMU, barometer and GNSS receiver, and can conduct inter-UAV ranging via a UWB transceiver. It is set that all UAVs realize the transmission of cooperative information through broadcasting, and UAVs that receive cooperative information and are within the ranging range can use this information for cooperation. Two UAVs in the swarm can obtain the position observation of GNSS directly, the remaining UAVs at a flight height above 30 m can receive pseudo-range observations of three satellites, while that at a flight height below 30 m cannot receive the pseudo-range observations.

In the simulation, the positioning estimation error is calculated as per the following equation:

$$E=\sqrt{Er{r}_E^2+Er{r}_N^2+Er{r}_U^2}$$

(37)

where $Er{r}_E$, $Er{r}_N$ and $Er{r}_U$ are the estimation errors on the east, north and upward directions respectively.

4.2. Simulation Results for Cooperative Navigation

To compare the proposed SPBP with other methods, this section simulates several cooperative positioning algorithms based on the same dataset, including H-SPAWN [21], Cooperative Positioning based Extended Kalman Filter (CP-EKF) [37], Multidimensional Scaling (MDS) [38] and Non-linear Regression (NLR) [39] methods. The positioning results are illustrated in Figure 6 and Figure 7. The detail of each algorithm is presented as follows:

① H-SPAWN (particle number = 2500): It is short for hybrid sum-product algorithm for wireless network, a typical BP-based positioning algorithm proposed in [21]. Its message passing process is identical with Equations (20)–(24). In case 1, 2500 particles are used.

② H-SPAWN (particle number = 1000): This algorithm is the same as that stated in ①, but the number of particles used is 1000.

③ CP-EKF: It is a CN method, in which EKF is used for integrating local measurements and other UAV cooperative data. The error of navigation state is designed as the state of filter. The cooperative measurement obtained by inter-UAV ranging is also incorporated into the measurement model. It should be noted that the covariance of noise in the measurement model considers both the UWB ranging error and the collaborator’s position error.

④ NLR: In this method, a nonlinear regression equation set is established on the basis of all distance and position measurements, and the UAV’s position is solved in iterative least square method.

⑤ MDS: This method is based on multidimensional scaling and the maximum likelihood estimation. The approach is composed of three sub-steps: (a) construction of the distance matrix, (b) relative position estimation, (c) coordinates registration. If the node scale is large, the network can be divided into several local maps for relative positioning respectively.

⑥ SPBP: The proposed method in this paper.

Figure 6 presents the simulation results of several cases, where the cumulative distribution functions (CDF) of position errors for all UAVs are used as the performance metrics. Figure 7 provides the example performance of a single UAV without satellite position measurement. NLR is a cooperative positioning method merely depending on the geometric relationship of UAV swarm and its precision is obviously lower than other cooperative methods as shown in the figure. CP-EKF integrates the navigation information of IMU, barometer, GNSS, and adjacent UAVs. However, without iterative update, the indirect cooperative information of non-adjacent UAV is not fully used. Hence, the positioning precision of CP-EKF is higher than that of NLR, but lower than MDS method and other BP-based cooperative methods. The relative positioning process of the MDS filter only utilizes the geometric constraints of the UAV swarm, thus its performance is lower than BP-based methods. Compared with H-SPAWN, the SPBP proposed in this paper is a method for approximating the distribution of state based on finite sampling points (i.e., sigma points). The difference between H-SPAWN and SPBP is similar to the difference between particle filter and UKF. Therefore, when there is no strong nonlinearity in the measurement, such as NLOS, multipath, and other errors, the CN model is basically in line with the linear mixed problem model, and the SPBP can realize similar performance to H-SPAWN as well as lower calculation complexity. As for the SPBP method proposed in this paper, its positioning precision is close to that of H-SPAWN (particle number = 2500) and superior to that of H-SPAWN (particle number = 1500). For instance, the percentiles with positioning error not greater than 2 m are lower than 94.9% for SPBP, close to 92.3% for H-SPAWN (particle number = 2500), and 82.1%, 11.7%, 45.7%, and 33.4% respectively for H-SPAWN (particle number = 1500), NLR, MDS and CP-EKF methods.

To further analyze the complexity of the proposed method, the ‘tic’ and ‘toc’ functions of MATLAB are used to record the running time of the code, which can roughly represent the computation load. As can be seen from Figure 8, which presents the efficiency comparison among all the above methods, NLR and CP-EKF do not need iterative passing of message, hence the minimum computation load. The computational efficiency of the MDS filter is similar to SPBP but with large fluctuation, mainly because the iterative process of MDS filter is largely dependent on prior information, which is affected by measurement biases. For H-SPAWN (particle number = 2500), the processing time at each epoch is more than 30 times that of SPBP. This means that with similar performance, SPBP has much lower calculation complexity than H-SPAWN.

5. Flight Tests

5.1. System Configuration

Figure 9 and Figure 10 show the UAV platform for the test and the configuration and signal flow of the navigation equipment, respectively. Each UAV is equipped with Ublox M8 GPS module, Forsense IMU6132 module, BMP280 atmospheric sensor and LinkTrack UWB module. The sensor data are collected, packed and sent to Raspberry Pi4 by STM32H7 processor. Raspberry Pi4 is for storing and processing data and sending control commands. While ranging between UAVs, UWB as ranging and communication module may also be used by STM32H7 to send its position and other information to be passed to other UAVs. Each UWB module can range and communicate with the other four based on time-division mechanism.

5.2. Test Results

The effectiveness of the proposed method was verified by an outdoor automatic flight test on a UAV swarm on the drill ground. This swarm is composed of five quadrotor UAVs (Figure 11). To evaluate the navigation performance, each UAV was equipped with on-board RTK equipment to provide the true value of the reference position. Figure 12 displays the flight scene, and the 3D and 2D flight trajectories of UAV swarm. As shown, after taking off, the UAV swarm flew in an L-shaped loop for 6 cycles before landing.

The entire flight process is divided into three stages: take-off, flight in loop (cooperation stage), and landing. It is set as follows: in the first and third stages, the navigations of all UAVs are realized by the combination of INS/GPS; after entering the second stage, UAV 1 and UAV 2 can obtain the satellite position measurements continuously, while other UAVs reject the input of satellite measurements and position cooperatively. To prove the performance of the proposed method, H-SPAWN was selected to compare with the method proposed in this paper based on actual flight. In the second stage, the horizontal positioning error CDFs of UAV 3, 4 and 5 are presented in Figure 13, and the horizontal positioning error of each UAV is illustrated in Figure 14.

In order to compare the positioning performances of the mentioned algorithms more clearly,

Table 2. Positioning results for several methods.

Method Type	RMSE in the Positioning Error (Units: m)				Average Processing Time (Units: s)
	UAV3	UAV4	UAV5	Total
H-SPAWN(Particles = 3000)	0.581	0.696	0.324	0.556	0.3745
H-SPAWN(Particles = 1000)	0.764	0.875	0.478	0.725	0.0885
SPBP	0.612	0.487	0.459	0.524	0.0175

lists the positioning results of the several algorithms for comparison. As shown, the total positioning precision of SPBP is similar to that of H-SPAWN (particle number = 3000), while the positioning precision of SPBP used for each UAV is superior to that of H-SPAWN (particle number = 1000). In terms of the average processing time at each epoch, the computation load of the proposed SPBP algorithm is 4 times and 20 times lower than those of H-SPAWN (particle number = 1000) and H-SPAWN (particle number = 3000) respectively. The flight test results demonstrate that the SPBP algorithm can significantly improve the calculation efficiency while guaranteeing the performance.

6. Discussion

This paper proposes a new CN method based on SPBP. In this method, a limited number of sigma points are selected to approximate the posteriori distribution of the navigation state quantity. The calculation load of this method is greatly less than that of traditional nonparametric BP method. Based on onboard multisource navigation sensor, the SPBP method proposed in [24] is expanded to a 3D environment in this paper, the state vectors of the time and measurement updates of sigma point filter are separated, and the state vectors are augmented only when updating the measurement of the cooperative information. Thereby, the messages passed by different UAVs can be received in every time of iteration and the SPBP-based new CN method proposed in this paper is more suitable for the UAV flight scene.

Particularly, when the setting of convergence threshold is small, the covariance matrix of the sigma point filter may be overconfident over repeated iterations, resulting in over-concentrated particles in the next sampling. Although the SPBP method can be used to approximate state distribution on the basis of a few sigma points, it is still needed to satisfy the Gaussian assumption of the system. The estimation performance may be poor when the navigation system is not corrected for a long time or the observation is strongly nonlinear. For example, when NLOS exists in UWB measurement and is not removed, the navigation result may be biased; meanwhile, the state covariance cannot be adjusted accordingly so that the next sampling scope cannot cover the correct position.

In the future, the SPBP algorithm can combine with some performance evaluation methods, such as posteriori Cramer-Rao bound [40,41], to solve the problem of overconfidence in the covariance matrix.

In addition, for the cooperative navigation system, an excellent communication network is indispensable. For the case where the UAVs are closely distributed and the number is small, a wireless communication network can be established in a point-to-point manner, which can be realized through XBEE or UWB, and the cost and complexity are low. If the scale or the distribution range of UAV swarm is large, the situation becomes complex. It is best to realize cooperative information interaction through broadcasting, so that the UAVs that receive cooperative information and are within the UWB working distance can complete cooperative navigation.

7. Conclusions

Against the CN background of low-cost UAV swarm, this paper proposes a distributed CN method that integrates common onboard navigation sensors and cooperative measurement information. In this method, the filtering of sigma point filter is divided into two steps. First, local measurements are integrated. Then, the mean and covariance of state are used to approximate the belief of the navigation state, and the message passing in traditional BP method is implemented by augmentation, resampling and cooperative measurement update of the state, realizing a low-complexity approximation to the traditional message passing scheme.

The simulation results show that in the simulation scene, the positioning performance of the SPBP method proposed in this paper is significantly superior to that of traditional single-step cooperative positioning methods such as CP-EKF and non-Bayesian CN method such as NLR and MDS. With similar performance, the computation load of the proposed SPBP method is much lower than that of the H-SPAWN method. The flight test results also verify the effectiveness of the proposed SPBP method. The future research will continue to study the SPBP and performance evaluation combined CN method, as well as the method for monitoring the integrity of CN information.