Collaborative Localization Method Based on Hybrid Network for Aerial Swarm

Abstract

In light of the satellite rejection environment and how aircraft can obtain high-precision positioning, this paper proposes a collaborative correction algorithm for aircraft based on the rank-defect network. Aiming at the problem of insufficient anchor points, which result in insufficient observations and the divergence of aircraft inertial navigation errors, this algorithm can effectively improve the navigation performance of cluster aircraft. On the basis of the observation information provided by the anchor aircraft, the observation information between aircraft is fully utilized to improve the observability of the aircraft cluster positioning method. At the same time, the pseudo-observation equation of heterogeneous aircraft cluster positioning is introduced, and the divergence of inertial navigation positioning errors caused by insufficient observations is suppressed by the pseudo-observation solution. On the basis of introducing the pseudo-observation equation, the inertial navigation error is solved and corrected by the Newton iterative method and the divergence of the inertial navigation position error is restrained. Compared with an aircraft cluster positioning method that does not use the inertial navigation error co-correction based on the pseudo-observation solution, this paper can achieve better overall cluster positioning accuracy when the available observations are insufficient, which is suitable for practical applications.

1. Introduction

With the development of the Information Age, the application of UAVs is more and more extensive. At the same time, the cluster network composed of multiple UAVs also has very broad application prospects [1].

The initial research on the cluster collaboration algorithm mainly focused on maritime navigation [2], and subsequent research expanded to the ground [3] and air [4]. In an environment with weak satellite signals, it is difficult to obtain accurate GNSS positioning, so the overall performance of navigation is improved through the collaboration algorithm. Xie [5], Zhang [6], and Nicholson [7] used underwater sonar to transmit information, obtain information between multiple unmanned ships, and fuse it with the information obtained by the sensors of the unmanned ships themselves, estimate the position of the unmanned ships, and effectively improve the navigation accuracy. Zhang [8] and Santiago [9] considered the impact of ocean currents on the unmanned ship cooperative positioning algorithm and improved the algorithm using the ocean current information received by the main unmanned ship. Feng [10] and Qi [11] considered the delay of underwater communication and effectively solved the problem of communication delay by improving the algorithm. Hu [12] studied the use of high-precision unmanned ship information and the relative distance information between unmanned ships for collaborative positioning, which effectively improved the dynamic positioning performance of unmanned ships. In the “two master and one slave” unmanned ship system, the distance between the master and slave unmanned ships is used to assist the position estimation of the slave unmanned ship [13], and all information are weighted and fused. Fortmann [14] and Sun [15] used factor graph theory to design an underwater unmanned ship cooperative navigation algorithm and verified that the positioning accuracy of the algorithm was higher than the extended Kalman. Chen [16], Liu [17], and Zhang [18] studied the indoor pedestrian positioning method, which is based on the premise that only one pedestrian can obtain GNSS positioning. Chen [16] used the distance information between pedestrians to suppress the divergence of other pedestrian inertial navigation positioning. Liu [17] and Zhang [18] used the particle filter algorithm and Kalman filter algorithm, respectively, to fuse the UWB ranging information and IMU information, which can effectively compensate for IMU errors. Zhao [19] aimed at the satellite rejection environment, added a pseudo satellite on the ground to assist with the vehicle positioning, and verified the navigation performance under static and dynamic conditions. When some GNSS nodes are missing, the system model is built based on the particle filter and track estimation principle, and the location of GNSS missing nodes is estimated based on the relative distance information between ground nodes [20].

At present, aircraft cluster networking mostly uses GNSS and inertial navigation for integrated navigation to determine the position information of each aircraft. There are still some defects that cannot be ignored when cooperating in air operations, that is, when GNSS signals are subject to strong electromagnetic interference or are even completely rejected [21,22], the accuracy of the positioning and navigation cannot meet the requirements. When the cluster aircraft is an unmanned aircraft, the above problems will be more acute. Liu [23] constructed the observation equation through the data link ranging information, estimated the missile position information using the weighted least squares, and effectively suppressed the divergence of inertial navigation. Liu [24] used the relative distance and relative motion between nodes (speed difference and path difference between nodes) to conduct missile formation and studied the impact of relative motion between nodes on the navigation performance of the missile system. Causa [25] used the parent UAV to assist the child UAV rejected by GNSS in positioning, and used the accuracy factor to predict the positioning accuracy of the child UAV. Wang [26] reconstructed the measurement equation using the ranging information of anchor and label aircraft, deduced the pseudo-observation equation, and estimated the positioning error using the least square method. Wang [27] proposed a collaborative navigation method based on partnership optimization for clustered aircraft and analyzed the impact of the equivalent ranging error on the optimization effect of a geometric configuration.

In this paper, based on the analysis of the rank-defect observation in the cluster network when the satellite is rejected, a distributed inertial navigation error cooperative correction scheme for heterogeneous multi-aircraft is proposed. Secondly, based on the cluster network positioning technology of aircraft mutual ranging and angle measurement, a distributed inertial navigation error co-correction model of heterogeneous multi-aircraft based on pseudo-observation was established, and a distributed inertial navigation error co-correction algorithm of heterogeneous multi-aircraft based on pseudo-observation was designed according to the characteristics of the model. Finally, the proposed algorithm was simulated and analyzed. The simulation results show that the distributed inertial navigation error correction algorithm based on pseudo-observation for heterogeneous multi-aircraft can effectively improve the overall navigation accuracy of the rank-defect network, and that the algorithm also significantly improves the navigation accuracy when the benchmark aircraft participates in the cooperation.

2. Construction of Hybrid Measurement Network for Aerial Swarm

2.1. Network by Relative Measurement

The cooperative observations among the aircraft can make full use of the relative observation advantages of the cluster to form a cluster observation geometric network. In some complex environments, due to the lack of external reference information and the inability of GNSS signals to cover all aircraft, the observation geometric network composed of relative observations between multi-aircraft cannot be aligned to the real position. The relationship between the observation geometric network and the real geometric network space is shown in Figure 1. At this time, the observability of positioning is obviously insufficient. With the help of the pseudo-observation method and the characteristics of the inertial navigation geometric network composed of mathematical analysis and multi-aircraft airborne inertial navigation, the observability of positioning can be improved, and therefore, the positioning accuracy of collaborative navigation can be improved. The schematic diagram of a geometric network corrected by the pseudo-observation method is shown in Figure 1.

As shown in Figure 1, the relative observation information can only determine the space relative position of multi-aircraft. To align to the real space position, three rotational degrees of freedom and three translational degrees of freedom are also required. From a mathematical point of view, there is a rank defect in the multi-aircraft cooperative observation equation.

2.2. Observation Geometry with Anchors

When there is an anchor aircraft used to provide reliable information in the cluster network, the schematic diagram of space transformation based on distance observation network with anchor aircraft is as follows:

It can be seen from Figure 2a that by adding an anchor aircraft to measure the relative distance of each label aircraft, the geometric network of multi-aircraft that originally lacked the limit of six degrees of freedom increased the limit of three degrees of freedom. It reduced the observation rank defect as well as the uncertainty of cluster collaborative positioning. It can be seen from Figure 2b that by adding an anchor aircraft to measure the relative distance and the relative line-of-sight angle of each label aircraft, the multi-aircraft geometric network that originally lacked the restriction of six degrees of freedom will no longer lack the restriction of degrees of freedom. At this time, the observation of the measuring geometric network is not rank deficient. It can be seen that compared with the ranging method, the multi-aircraft geometric network of angle and ranging measurement cooperation has lower requirements on the number of anchor aircraft.

3. Collaborative Localization with Hybrid Measurement Network

3.1. Airborne Navigation Scheme with Measurement Network

In order to solve the problem of the rank defect of heterogeneous multi-aircraft cooperative observations caused by the lack of external reference information, based on the above analysis of the deviation between the multi-aircraft observation geometric network and the real geometric network, this paper designed a general scheme of heterogeneous multi-aircraft cooperative navigation error correction based on pseudo-observation, as shown in Figure 3.

As shown in Figure 3, there are two types of heterogeneous multi-aircraft: anchor aircraft and label aircraft. The anchor aircraft needs to support its high-precision positioning with the help of satellite signals. However, in the harsh environment of GNSS discussed in this paper, there are few or even no anchor aircraft in the cluster, so it is very easy to have a collaborative observation rank defect. The overall scheme of error correction based on pseudo-observation mainly includes the inter-aircraft transmission of observation and position information, the establishment of the observation model and the cooperative model, and the error calculation and correction of the airborne inertial navigation system. Firstly, each label aircraft synchronously obtains the relative observation information of other aircraft in the cluster and the output position of the airborne inertial navigation system through the cooperative measurement sensor and the aircraft communication equipment. Secondly, the heterogeneous multi-aircraft cooperative measurement modeling is carried out by using the distance and line-of-sight angle measurement information, the anchor aircraft position, and the inertial navigation position of the label aircraft. That is, the multi-aircraft inertial navigation error model based on the relative distance and the multi-aircraft inertial navigation error model based on the phase angle are established by the range linearization and the angle linearization. The pseudo-observation model is established according to the rank defect of the observation equation. Finally, the inertial navigation error of each label aircraft is obtained and the error compensation is carried out by solving the error of the cooperative observation equation fused with the pseudo-observation equation.

3.2. Modeling of Measurement Network for Aerial Swarm

Assume that the position coordinates of each label aircraft in the cluster on the geocentric earth fixed connection coordinate system are $\left(x_i,y_i,z_i\right)$, $i=1,2,\cdots ,n$, $n$ is the total number of label aircraft in the cluster, and the coordinate position of the anchor aircraft is $\left(x,y,z\right)$. This paper studied the adverse combat environment of satellite signal rejection, so it was assumed that there was at most one anchor aircraft in the cluster. The inertial navigation position estimated by the airborne inertial navigation system of each label aircraft is $\left(x_i^I,y_i^I,z_i^I\right)$, so the estimation of its positioning error by the airborne inertial navigation system of the label aircraft can be expressed by the following formula:

$$\left\{\delta x_i,\delta y_i,\delta z_i\right\}=\left\{x_i^I,y_i^I,z_i^I\right\}-\left\{x_i,y_i,z_i\right\}$$

(1)

The measured distance between aircraft is obtained through the distance sensor, which can be expressed as:

$$r_{ij}^m=\sqrt{{\left(x_i-x_j\right)}^2+{\left(y_i-y_j\right)}^2+{\left(z_i-z_j\right)}^2}+{\epsilon }_{ij}^r$$

(2)

where ${\epsilon }_{ij}^r$ is the measurement noise of the distance between Aircraft $i$ and Aircraft $j$.

Accordingly, the distance estimation value obtained from the inertial navigation position can be expressed as:

$$r_{ij}^I=\sqrt{{\left(x_i^I-x_j^I\right)}^2+{\left(y_i^I-y_j^I\right)}^2+{\left(z_i^I-z_j^I\right)}^2}$$

(3)

The above formula is linearized, that is, it is expanded into the Taylor series at the true value, and the first two terms are approximately taken as

$$\begin{array}{l}{r}_{ij}^I\approx \sqrt{{\left(x_j-x_i\right)}^2+{\left(y_j-y_i\right)}^2+{\left(z_j-z_i\right)}^2}\\ +\left(-\frac{x_j^I-x_i^I}{r_{ij}^I}\right)\left(x_i-x_i^I\right)+\left(-\frac{y_j^I-y_i^I}{r_{ij}^I}\right)\left(y_i-y_i^I\right)+\left(-\frac{z_j^I-z_i^I}{r_{ij}^I}\right)\left(z_i-z_i^I\right)\\ +\left(\frac{x_j^I-x_i^I}{r_{ij}^I}\right)\left(x_j-x_j^I\right)+\left(\frac{y_j^I-y_i^I}{r_{ij}^I}\right)\left(y_j-y_j^I\right)+\left(\frac{z_j^I-z_i^I}{r_{ij}^I}\right)\left(z_j-z_j^I\right)\end{array}$$

(4)

Subtract Formula (4) from Formula (2) to construct the cooperative measurement equation of cluster aircraft based on distance measurement:

$$r_{ij}^m-r_{ij}^I=-\frac{x_j^I-x_i^I}{r_{ij}^I}\delta x_i^I-\frac{y_j^I-y_i^I}{r_{ij}^I}\delta y_i^I-\frac{z_j^I-z_i^I}{r_{ij}^I}\delta z_i^I+\frac{x_j^I-x_i^I}{r_{ij}^I}\delta x_j^I+\frac{y_j^I-y_i^I}{r_{ij}^I}\delta y_j^I+\frac{z_j^I-z_i^I}{r_{ij}^I}\delta z_i^I$$

(5)

In the same way, the distance measurement value between the aircraft and the anchor aircraft is $r_i^m$ through the range sensor equipped by the aircraft, and the measurement equation based on the distance measurement between the reference and the label aircraft can be constructed:

$$r_i^m-r_i^I=-\frac{x^I-x_i^I}{r_{ij}^I}\delta x_i^I-\frac{y^I-y_i^I}{r_{ij}^I}\delta y_i^I-\frac{z^I-z_i^I}{r_{ij}^I}\delta z_i^I$$

(6)

Combine all $n\left(n-1\right)/2$ cooperative distance measurement equations and $n*m$ relative distance measurement equations between the label aircraft and the anchor aircraft into a set of equations, expressed as:

$$\begin{array}{l}\left[\begin{array}{l} ⋮\\ r_{ij}^m-r_{ij}^I\\ ⋮\\ r_{ik}^m-r_{ik}^I\\ ⋮\\ r_{jk}^m-r_{jk}^I\\ ⋮\end{array}\right]=\left[\begin{array}{l} ⋮ ⋮ ⋮ ⋮ ⋮ ⋮\\ -\frac{x_i^I-x_j^I}{r_{ij}^I} -\frac{y_i^I-y_j^I}{r_{ij}^I} -\frac{z_i^I-z_j^I}{r_{ij}^I} \frac{x_i^I-x_j^I}{r_{ij}^I} \frac{y_i^I-y_j^I}{r_{ij}^I} \frac{z_i^I-z_j^I}{r_{ij}^I}\\ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮\\ \\ -\frac{x_i^I-x_k}{r_{ik}^I} -\frac{y_i^I-y_k}{r_{ik}^I} -\frac{z_i^I-z_k}{r_{ik}^I} 0 0 0\\ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮\\ 0 0 0 \frac{x_k^I-x_j^I}{r_{kj}^I} \frac{y_k^I-y_j^I}{r_{kj}^I} \frac{z_k^I-z_j^I}{r_{kj}^I}\\ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮\end{array}\right]\left[\begin{array}{l}⋮\\ \Delta x_i^I\\ \Delta y_i^I\\ \Delta z_i^I\\ \Delta x_j^I\\ \Delta y_j^I\\ \Delta z_j^I\\ ⋮\end{array}\right]+\left[\begin{array}{l}⋮\\ {\epsilon }_{ij}^r\\ ⋮\\ {\epsilon }_{ik}^r\\ ⋮\\ {\epsilon }_{jk}^r\\ ⋮\end{array}\right]\\ \triangleq {\mathit{Y}}_{net}^r={\mathit{H}}_{net}^r\mathit{X}+{\mathit{V}}_{net}^r\end{array}$$

(7)

where $\left[\Delta x_i^I,\Delta y_i^I,\Delta z_i^I\right]=\left[x_i^I,y_i^I,z_i^I\right]-\left[x_i,y_i,z_i\right],\left(i=1,2,\cdots ,n\right)$ is the inertial navigation position error of the aircraft. $k=1,2,\cdots ,m$, $m$ is the total number of label aircraft in the cluster, and the distance measured noise ${\epsilon }^r$ obeys a normal distribution.

Based on the configuration connection of the aircraft cluster, as shown in Figure 2b, we can also obtain the corresponding azimuth angle ${\theta }_{ik}^m$ and height angle ${\phi }_{ik}^m$. Similar to the relative distance measurement in Formula (7), we can establish a relative angle measurement equation as follows:

$$\begin{array}{l}\left[\begin{array}{l} ⋮\\ {\theta }_{ij}^I-{\theta }_{ij}^m\\ {\phi }_{ij}^I-{\phi }_{ij}^m\\ ⋮\\ {\theta }_{ik}^I-{\theta }_{ik}^m\\ {\phi }_{ik}^I-{\phi }_{ik}^m\\ ⋮\\ {\theta }_{jk}^I-{\theta }_{jk}^m\\ {\phi }_{jk}^I-{\phi }_{jk}^m\\ ⋮\end{array}\right]=\left[\begin{array}{l} ⋮ ⋮ ⋮ ⋮ ⋮ ⋮\\ \\ \frac{y_j^I-y_i^I}{{\left(d_{ij}^I\right)}^2} -\frac{x_j^I-x_i^I}{{\left(d_{ij}^I\right)}^2} 0 -\frac{y_j^I-y_i^I}{{\left(d_{ij}^I\right)}^2} \frac{x_j^I-x_i^I}{{\left(d_{ij}^I\right)}^2} 0\\ \frac{\left(x_j^I-x_i^I\right)\left(z_j^I-z_i^I\right)}{d_{ij}^I{\left(r_{ij}^I\right)}^2} \frac{\left(y_j^I-y_i^I\right)\left(z_j^I-z_i^I\right)}{d_{ij}^I{\left(r_{ij}^I\right)}^2} -\frac{d_{ij}^I}{{\left(r_{ij}^I\right)}^2}-\frac{\left(x_j^I-x_i^I\right)\left(z_j^I-z_i^I\right)}{d_{ij}^I{\left(r_{ij}^I\right)}^2} -\frac{\left(y_j^I-y_i^I\right)\left(z_j^I-z_i^I\right)}{d_{ij}^I{\left(r_{ij}^I\right)}^2} \frac{d_{ij}^I}{{\left(r_{ij}^I\right)}^2}\\ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮\\ \\ \frac{y_k^I-y_i^I}{{\left(d_{ik}^I\right)}^2} -\frac{x_k^I-x_i^I}{{\left(d_{ik}^I\right)}^2} 0 0 0 0\\ \frac{\left(x_k^I-x_i^I\right)\left(z_k^I-z_i^I\right)}{d_{ik}^I{\left(r_{ik}^I\right)}^2} \frac{\left(y_k^I-y_i^I\right)\left(z_k^I-z_i^I\right)}{d_{ik}^I{\left(r_{ik}^I\right)}^2} -\frac{d_{ik}^I}{{\left(r_{ik}^I\right)}^2} 0 0 0\\ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮\\ 0 0 0 -\frac{y_k^I-y_j^I}{{\left(d_{jk}^I\right)}^2} \frac{x_k^I-x_j^I}{{\left(d_{jk}^I\right)}^2} 0\\ 0 0 0 -\frac{\left(x_k^I-x_j^I\right)\left(z_k^I-z_j^I\right)}{d_{jk}^I{\left(r_{jk}^I\right)}^2} -\frac{\left(y_k^I-y_j^I\right)\left(z_k^I-z_j^I\right)}{d_{jk}^I{\left(r_{jk}^I\right)}^2} \frac{d_{jk}^I}{{\left(r_{jk}^I\right)}^2}\\ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮\end{array}\right]\left[\begin{array}{l}⋮\\ \Delta x_i^I\\ \Delta y_i^I\\ \Delta z_i^I\\ \Delta x_j^I\\ \Delta y_j^I\\ \Delta z_j^I\\ ⋮\end{array}\right]+\left[\begin{array}{l}⋮\\ {\epsilon }_{ij}^{\theta }\\ {\epsilon }_{ij}^{\phi }\\ ⋮\\ {\epsilon }_{ik}^{\theta }\\ {\epsilon }_{ik}^{\phi }\\ ⋮\\ {\epsilon }_{jk}^{\theta }\\ {\epsilon }_{jk}^{\phi }\\ ⋮\end{array}\right]\\ \triangleq {\mathit{Y}}_{net}^a={\mathit{H}}_{net}^a\mathit{X}+{\mathit{V}}_{net}^a\end{array}$$

(8)

where $d_{ij}^I=\sqrt{{\left(x_i^I-x_j^I\right)}^2+{\left(y_i^I-y_j^I\right)}^2}$ and the angle measured noise ${\epsilon }^{\theta }$, ${\epsilon }^{\phi }$ obeys a normal distribution. ${\mathit{Y}}_{net}^a={\mathit{H}}_{net}^a\mathit{X}+{\mathit{V}}_{net}^a$ is the cooperative angle measurement equation of heterogeneous multi-aircraft including the $n\left(n-1\right)$ cooperative angle measurement equations of mutual angle measurement between the label aircraft, and the $2n*m$ cooperative angle measurement equations of angle measurement between the anchor aircraft and the label aircraft.

Based on Formulas (7) and (8), the measurement model can be established as follows:

$$\begin{array}{l}\left[\begin{array}{l}{\mathit{Y}}_{net}^r\\ {\mathit{Y}}_{net}^a\end{array}\right]=\left[\begin{array}{l}{\mathit{H}}_{net}^r\\ {\mathit{H}}_{net}^a\end{array}\right]\mathit{X}+\left[\begin{array}{l}{\mathit{V}}_{net}^r\\ {\mathit{V}}_{net}^a\end{array}\right]\\ {\mathit{Y}}_{net}={\mathit{H}}_{net}\mathit{X}+{\mathit{V}}_{net}\triangleq \\ \left[\begin{array}{l} ⋮\\ r_{ij}^m-r_{ij}^I\\ {\theta }_{ij}^I-{\theta }_{ij}^m\\ {\phi }_{ij}^I-{\phi }_{ij}^m\\ ⋮\\ r_{ik}^m-r_{ik}^I\\ {\theta }_{ik}^I-{\theta }_{ik}^m\\ {\phi }_{ik}^I-{\phi }_{ik}^m\\ ⋮\\ r_{jk}^m-r_{jk}^I\\ {\theta }_{jk}^I-{\theta }_{jk}^m\\ {\phi }_{jk}^I-{\phi }_{jk}^m\\ ⋮\end{array}\right]=\left[\begin{array}{l} ⋮ ⋮ ⋮ ⋮ ⋮ ⋮\\ -\frac{x_j^I-x_i^I}{r_{ij}^I} -\frac{y_j^I-y_i^I}{r_{ij}^I} -\frac{z_j^I-z_i^I}{r_{ij}^I} \frac{x_j^I-x_i^I}{r_{ij}^I} \frac{y_j^I-y_i^I}{r_{ij}^I} \frac{z_j^I-z_i^I}{r_{ij}^I}\\ \frac{y_j^I-y_i^I}{{\left(d_{ij}^I\right)}^2} -\frac{x_j^I-x_i^I}{{\left(d_{ij}^I\right)}^2} 0 -\frac{y_j^I-y_i^I}{{\left(d_{ij}^I\right)}^2} \frac{x_j^I-x_i^I}{{\left(d_{ij}^I\right)}^2} 0\\ \frac{ \left(x_j^I-x_i^I\right)\left(z_j^I-z_i^I\right)}{d_{ij}^I{\left(r_{ij}^I\right)}^2} \frac{\left(y_j^I-y_i^I\right)\left(z_j^I-z_i^I\right)}{d_{ij}^I{\left(r_{ij}^I\right)}^2} -\frac{d_{ij}^I}{{\left(r_{ij}^I\right)}^2}-\frac{\left(x_j^I-x_i^I\right)\left(z_j^I-z_i^I\right)}{d_{ij}^I{\left(r_{ij}^I\right)}^2}-\frac{\left(y_j^I-y_i^I\right)\left(z_j^I-z_i^I\right)}{d_{ij}^I{\left(r_{ij}^I\right)}^2} \frac{d_{ij}^I}{{\left(r_{ij}^I\right)}^2}\\ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮\\ \\ -\frac{x_k^I-x_i^I}{r_{ik}^I} -\frac{y_k^I-y_i^I}{r_{ik}^I} -\frac{z_k^I-z_i^I}{r_{ik}^I} 0 0 0\\ \frac{y_k^I-y_i^I}{{\left(d_{ik}^I\right)}^2} -\frac{x_k^I-x_i^I}{{\left(d_{ik}^I\right)}^2} 0 0 0 0\\ \frac{ \left(x_k^I-x_i^I\right)\left(z_k^I-z_i^I\right)}{d_{ik}^I{\left(r_{ik}^I\right)}^2} \frac{\left(y_k^I-y_i^I\right)\left(z_k^I-z_i^I\right)}{d_{ik}^I{\left(r_{ik}^I\right)}^2} -\frac{d_{ik}^I}{{\left(r_{ik}^I\right)}^2} 0 0 0\\ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮\\ 0 0 0 \frac{x_k^I-x_j^I}{r_{jk}^I} \frac{y_k^I-y_j^I}{r_{jk}^I} \frac{z_k^I-z_j^I}{r_{jk}^I}\\ 0 0 0 -\frac{y_k^I-y_j^I}{{\left(d_{jk}^I\right)}^2} \frac{x_k^I-x_j^I}{{\left(d_{jk}^I\right)}^2} 0\\ 0 0 0-\frac{\left(x_k^I-x_j^I\right)\left(z_k^I-z_j^I\right)}{d_{jk}^I{\left(r_{jk}^I\right)}^2} -\frac{\left(y_k^I-y_j^I\right)\left(z_k^I-z_j^I\right)}{d_{jk}^I{\left(r_{jk}^I\right)}^2} \frac{d_{jk}^I}{{\left(r_{jk}^I\right)}^2}\\ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮\end{array}\right]\left[\begin{array}{l}⋮\\ \Delta x_i^I\\ \Delta y_i^I\\ \Delta z_i^I\\ \Delta x_j^I\\ \Delta y_j^I\\ \Delta z_j^I\\ ⋮\end{array}\right]+\left[\begin{array}{l}⋮\\ {\epsilon }_{ij}^r\\ {\epsilon }_{ij}^{\theta }\\ {\epsilon }_{ij}^{\phi }\\ ⋮\\ {\epsilon }_{ik}^r\\ {\epsilon }_{ik}^{\theta }\\ {\epsilon }_{ik}^{\phi }\\ ⋮\\ {\epsilon }_{jk}^r\\ {\epsilon }_{jk}^{\theta }\\ {\epsilon }_{jk}^{\phi }\\ ⋮\end{array}\right]\end{array}$$

(9)

The auxiliary measurement of angle can reduce the requirements of the multi-aircraft geometric network on the number of anchor aircraft in the cluster. At the same time, because the observation information of the navigation system is increased, the proposed algorithm can be strengthened in terms of navigation performance.

3.3. Collaborative Localization According to Measurement Network

The cooperative observation equation of heterogeneous multi-aircraft obtained from Section 3.1 is

$${\mathit{Y}}_{net}={\mathit{H}}_{net}\mathit{X}+{\mathit{V}}_{net}$$

(10)

where ${\mathit{Y}}_{net}$ is the cooperative observation, ${\mathit{H}}_{net}$ is the observation matrix, $\mathit{X}$ is the position coordinate error estimate of each aircraft in the cluster to be solved, and ${\mathit{V}}_{net}$ is the cooperative measurement noise. Solving Formula (10) can provide the estimated value of the inertial navigation error $\widehat{\mathit{X}}={\left[{\mathit{H}}_{net}^T{\mathit{P}}_Y{\mathit{H}}_{net}\right]}^{-1}{\mathit{H}}_{net}^T{\mathit{P}}_Y{\mathit{Y}}_{net}$, where ${\mathit{P}}_Y$ is the weight matrix for measurement error.

However, the cooperative observation only cannot determine the absolute position coordinates of each aircraft in the cluster. When $rank\left({\mathit{H}}_{net}\right)<rank\left(\mathit{X}\right)=3n$, Formula (10) is rank deficient and ${\mathit{H}}_{net}^T{\mathit{P}}_Y{\mathit{H}}_{net}$ is irreversible.

In order to solve the observation equation, we wanted to add a restriction matrix to make the rank defect become a non-rank defect and make the equation have a solution. That is, we constructed $0={\mathit{G}}^T{\mathit{P}}_X\mathit{X}+{\mathit{V}}_G$ to make $rank\left(\begin{array}{l}{\mathit{H}}_{net}\\ {\mathit{G}}^T{\mathit{P}}_X\end{array}\right)=3n$, where we observed the full rank of the pseudo-observation equation. ${\mathit{P}}_X$ is the weight matrix of the state quantity, and ${\mathit{V}}_G$ is the pseudo-observation noise.

The pseudo-observation solution of weighted minimum norm adjustment is referred to, which gives the conditions to be satisfied for the construction of pseudo-observation equation, namely Condition 1 $rank\left(\mathit{G}\right)=3n-r$, Condition 2 ${\mathit{H}}_{net}^T{\mathit{P}}_Y{\mathit{H}}_{net}\mathit{G}=0$. This paper uses the zero eigenvalue vector of ${\mathit{H}}_{net}^T{\mathit{P}}_Y{\mathit{H}}_{net}$ to construct $\mathit{G}$. Because the zero eigenvalue vector of ${\mathit{H}}_{net}$ can just be represented by $r$ unrelated vectors, that is, $\mathit{H}\mathit{y}=0$ has basic solution systems, and the matrix $\mathit{G}$ composed of the basic solution systems satisfies Condition 1 and Consition 2.

Therefore, the pseudo-observation equations are constructed using zero eigenvalues as follows:

$$0={\mathit{G}}^T{\mathit{P}}_X\mathit{X}+{\mathit{V}}_G$$

(11)

By adding a pseudo-observation equation to make the measurement matrix full rank, a multi-aircraft cooperative observation equation incorporating the pseudo-observation equation can be constructed as follows:

$$\left\{\begin{array}{l}{\mathit{Y}}_{net}={\mathit{H}}_{net}\mathit{X}+{\mathit{V}}_{net}\\ 0={\mathit{G}}^T{\mathit{P}}_X\mathit{X}+{\mathit{V}}_G\end{array}\right.$$

(12)

When there is no anchor aircraft in the ranging network $rank\left(\mathit{G}\right)=6$, adding the pseudo-observation equation can provide six spatial degrees of freedom for the cluster network, so the observation is no longer rank deficient.

At this time, the cooperative observation equation based on the pseudo-observation solution can be expressed as:

$$\left\{\begin{array}{l}{\mathit{H}}_{net}^T{\mathit{P}}_Y{\mathit{H}}_{net}\widehat{\mathit{X}}={\mathit{H}}_{net}^T{\mathit{P}}_Y{\mathit{Y}}_{net}\\ {\mathit{G}}^{\mathit{T}}{\mathit{P}}_X\widehat{\mathit{X}}=0\end{array}\right.$$

(13)

${\mathit{G}}^{\mathit{T}}{\mathit{P}}_X\widehat{\mathit{X}}=0$ multiplies ${\mathit{P}}_X\mathit{G}$ to the left and the descendants enter ${\mathit{H}}_{net}^T{\mathit{P}}_Y{\mathit{H}}_{net}\widehat{\mathit{X}}={\mathit{H}}_{net}^T{\mathit{P}}_Y{\mathit{Y}}_{net}$, so the following solution formula is obtained:

$$\widehat{\mathit{X}}={\left({\mathit{H}}_{net}^T{\mathit{P}}_Y{\mathit{H}}_{net}+{\mathit{P}}_X\mathit{G}{\mathit{G}}^T{\mathit{P}}_X\right)}^{-1}{\mathit{H}}_{net}^T{\mathit{P}}_Y{\mathit{Y}}_{net}$$

(14)

Because ${\mathit{P}}_X$ is a positive definite weight matrix and $\mathit{G}$ is composed of zero eigenvalues of ${\mathit{H}}_{net}^T{\mathit{P}}_Y{\mathit{H}}_{net}$, ${\mathit{H}}_{net}^T{\mathit{P}}_Y{\mathit{H}}_{net}+{\mathit{P}}_X\mathit{G}{\mathit{G}}^T{\mathit{P}}_X$ is full rank and its inverse exists, which can be solved by Formula (11) for the cooperative observation equation.

When there is no observation rank defect in the multi-aircraft geometric network, the weighted least squares method can be used to solve it. The solution formula is as follows:

$$\widehat{\mathit{X}}={\left[{\mathit{H}}_{net}^T{\mathit{P}}_Y{\mathit{H}}_{net}\right]}^{-1}{\mathit{H}}_{net}^T{\mathit{P}}_Y{\mathit{Y}}_{net}$$

(15)

4. Simulation and Analysis

4.1. Analysis of Collaboration Mode with Hybrid Measurements

In order to verify the effectiveness of the algorithm proposed in this paper, the positioning error before and after using the algorithm was simulated and analyzed in the static positioning mode of multi-aircraft. Since the method proposed in this paper discusses the conditions of ranging cooperation, angle measurement and ranging cooperation, and whether there is an anchor aircraft cooperation, it was necessary to analyze and verify all situations during the simulation exploration in order to simplify the description. The classification of various situations is shown in

Table 1. Collaboration mode and anchor aircraft set in the algorithm simulation analysis.

	Case ①	Case ②	Case ③	Case ④
Collaboration mode	Ranging	Ranging	Ranging and angular	Ranging and angular
Anchor aircraft	Nonexistent	Existent	Nonexistent	Existent

.

In the static simulation, the initial error of each axis setting the inertial navigation position followed a normal distribution with a standard deviation of 10 m, the angle measurement error was 0.1°, and the ranging error was 1 m. The space position of each aircraft is shown in Figure 4, and 1000 Monte Carlo simulations were conducted.

As shown in Figure 5, the simulation statistical results of the axial positioning errors of all label aircraft are displayed in the form of CDF (cumulative distribution function) in order to analyze the algorithm performance more intuitively.

It can be seen from the inertial navigation error curve in Figure 5 that when multi-label aircraft only relied on the inertial navigation system for positioning, the cumulative probability density of the position error at 10 m was about 68%, and the cumulative probability density at 20 m was about 98%, because the initial error of each axis of the inertial navigation position followed a normal distribution with a standard deviation of 10 m. It can be seen from the case ① curve that after increasing the mutual ranging cooperation between the label aircraft, the cumulative probability density of the position error at 10 m was about 93%, and the cumulative probability density at 20 m was 100%. It can be seen from the case ③ curve that after adding the mutual ranging and angle measurement cooperation between the label aircraft, the cumulative probability density of the position error at 10 m was about 94%, and the cumulative probability density at 20 m was 100%. It can be seen from the curves of cases ② and ④ that the cumulative probability density of the position error of ranging and angular positioning in 10 m increased to 98% and 100%, respectively, by increasing the coordination of the anchor aircraft. Therefore, the co-correction method for navigation errors of heterogeneous multi-aircraft based on pseudo-observation can effectively correct the inertial navigation error, and the performance of the algorithm can be enhanced by adding the assistance of the reference vehicle. This effect is particularly significant in the cooperative mode of ranging and angle measurement.

4.2. Analysis of Influential Factors on Collaborative Performance

Theoretically, the mutual observation accuracy of multi-aircraft, that is, the size of ranging and angle measurement error, the number of aircraft, and the configuration of aircraft are important factors that affect the collaborative correction method for navigation errors of heterogeneous multi-aircraft based on pseudo-observation. Therefore, this section simulated and analyzed the impact of key factors such as aircraft configuration, collaborative measurement accuracy, and the number of aircraft on the algorithm performance.

(1) Multi-aircraft configuration factor.

The collaborative correction method for navigation errors of heterogeneous multi-aircraft based on pseudo-observation proposed in this paper is closely related to the rank of the multi-aircraft cooperative observation equation. The configuration of aircraft has a great impact on the number of observation ranks, so the impact of configuration was analyzed first. The configuration of the aircraft to be simulated and analyzed was set as shown in Figure 6. The label aircraft were conical, square, and straight.

According to the situation in Table 1, the observed rank defects of each configuration in Figure 6 were classified and counted, and the statistical results are shown in

Table 2. Observed rank deficiency numbers of aircraft configurations.

Aircraft Configuration	Case ①	Case ②	Case ③	Case ④
Taper	6	3	6	0
Square	7	5	7	3
Line	9	7	9	5

.

For the conical cluster in Figure 6a, in case ①, the measurement equation was calculated to have a rank of 6 and a rank deficiency of 6. At this point, the measurement equation ${\mathit{Y}}_{net}={\mathit{H}}_{net}\mathit{X}+{\mathit{V}}_{net}$ has no solution, and the cluster cannot perform the collaborative positioning calculation. By using the method proposed in Section 3, the eigenvector $G$ ($rank\left(\mathit{G}\right)=3n-r=6$ and $rank\left(\begin{array}{l}{\mathit{H}}_{net}\\ {\mathit{G}}^T{\mathit{P}}_X\end{array}\right)=3n=12$) corresponding to the eigenvalue of ${\mathit{H}}_{net}^T{\mathit{P}}_Y{\mathit{H}}_{net}$ was calculated to compensate for the rank deficiency of the measurement equation. Furthermore, the measurement equation was constructed as $\left\{\begin{array}{l}{\mathit{Y}}_{net}={\mathit{H}}_{net}\mathit{X}+{\mathit{V}}_{net}\\ 0={\mathit{G}}^T{\mathit{P}}_X\mathit{X}+{\mathit{V}}_G\end{array}\right.$ and was solvable after rank supplementation. The cluster collaborative positioning calculation was used to suppress the divergence of inertial navigation errors.

Table 2 shows that in cases ①–④, the number of observed rank defects of multi-aircraft in the square configuration and line configuration was higher than that in taper configuration. By comparing case ①, case ②, case ③, and case ④, it was found that the addition of the anchor aircraft reduced the observation rank defects of each configuration. Moreover, in the conical configuration, the addition of the anchor aircraft reduced the observation rank defect number more. Due to the increase in the rank defect number, the effectiveness of the algorithm proposed in this paper in the square and line configurations of the label aircraft will be reduced, so the navigation accuracy in the square and line configurations will be reduced.

In order to verify the above reasoning, the simulation of the average position error of multi-aircraft after using the algorithm under the above configuration was carried out, and the probability density function (PDF) of each average position error was calculated. With the exception of the configuration, the other simulation parameters were consistent with Section 4.1. The simulation results are shown in Figure 7.

It can be seen from Figure 7a that the algorithm had an obvious correction effect on the inertial navigation error under various conditions in the conical configuration, and the addition of the anchor aircraft significantly increased the probability of the average positioning error within 10 m. It can be seen from Figure 7b that in the square configuration, the magnitude of partial positioning error in case ① and case ③ was too large, and the probability of average positioning error within 10 m was significantly lower than that in the conical configuration, especially when there was no anchor aircraft coordination. It can be seen from Figure 7c that in the linear configuration, there were still some cases where the magnitude of positioning error was too large, and the probability of the average position error within 10 m in the conical configuration and the square configuration was significantly reduced, and the addition of the anchor aircraft could not be improved. Therefore, when multi-aircraft perform tasks together, the algorithm proposed in this paper should be used to correct the position error of inertial navigation, and the situation of coplanar or collinear aircraft should be avoided as much as possible.

(2) Sensor accuracy factor.

The algorithm is based on the range and angle measurement cooperation among aircraft, so the range and angle measurement accuracy will have a certain impact on the performance of the algorithm. The configuration of aircraft was maintained as shown in Figure 6a. Except for the range and angle measurement error, the other simulation conditions were consistent with the settings in Section 4.1. The variation curve of the average position error of multi-aircraft with the measurement accuracy under 1000 times of Monte Carlo simulation is shown in Figure 8.

It can be seen from Figure 8a that with the increase in ranging error, the position error of cluster aircraft also increased, and the increase in the position error was more obvious after the anchor aircraft participated in the cooperative navigation. It can be seen from Figure 8b that increasing the angle measurement error had little impact on the performance of the algorithm when the non-anchor aircraft participated in the cooperation. After increasing the anchor aircraft cooperation, the position error also increased with the increase in the angle measurement error. Therefore, the collaborative correction method for heterogeneous multi-aircraft navigation errors based on pseudo-observation proposed in this paper was restricted by sensor accuracy to a certain extent, so it was necessary to select high-precision measurement sensors within the allowable range to improve the overall navigation performance of multi-aircraft.

(3) Aircraft quantity factor.

In order to explore the impact of the number of aircraft on the algorithm performance, a simulation of the change in the average position error of multi-aircraft with the number of label aircraft was carried out. Except for the number of label aircraft and the distribution of aircraft, the other simulation conditions were consistent with the settings in Section 4.1. The distribution of aircraft is shown in Figure 9a, and the curve of the average position error of multi-aircraft with the number of label aircraft is shown in Figure 9b.

As shown in Figure 9, with the increase in the number of label aircraft, the overall average position error of multiple aircraft decreased gradually. However, in case ④ of the range and angle measurement cooperation and anchor aircraft cooperation, the average position error was not affected by the number of label aircraft and remained at a low level. Therefore, when the cluster cooperatively observes the rank defect, as in cases ①–③, appropriately increasing the number of label aircraft in the cluster within a certain range can improve the overall positioning accuracy of the cluster.

4.3. Analysis of Collaborative Navigation Performance

In order to verify the effectiveness of the algorithm proposed in this paper in the real-time navigation of heterogeneous multi-aircraft, this paper also carried out a dynamic simulation and analysis. The simulation settings of the airborne inertial sensors, ranging, and angle measurement sensors of the aircraft are shown in

Table 3. Airborne navigation sensor error parameter settings of the aircraft.

Sensor Error Parameters	Value
Constant drift error of gyroscope	0.3(°)/h
Gyro first-order Markov process time	3600 s
Accelerometer bias error	1 × 10−4 g
Accelerometer first-order Markov process time	1800 s
Range sensor accuracy	1 m
Angle measuring sensor accuracy	0.1°
Number of label aircraft	7

. The position of the anchor aircraft and the track of each label aircraft in the dynamic simulation are shown in Figure 10. The simulation time was 400 s, and the dynamic simulation was the statistical result of 20 Monte Carlo simulations.

In the dynamic simulation, the position of the anchor aircraft and the track of each label aircraft are shown in Figure 10. The cluster aircraft are divided into the anchor aircraft and the label aircraft. The anchor aircraft is located in high-altitude areas with good satellite signals, which can be approximately considered to be accurately known. The label aircraft measure each other through distance measurement sensors and angle measurement sensors, and exchange navigation information through airborne communication equipment. The anchor aircraft can provide the label aircraft with relative distance angle measurement information and its own position information. Because it was necessary to simulate and explore the cooperative correction method for navigation errors of heterogeneous multi-aircraft based on pseudo-observation under the harsh GNSS environment, at most only one anchor aircraft was considered to participate in the simulation of the algorithm. Taking label aircraft 1 as an example, the observation information changes of each aircraft relative to it in the navigation simulation are shown in Figure 11.

For the four cases described in Table 1, the average position error change curve of the seven label aircraft before and after correction using the method proposed in this paper is shown in Figure 12.

From the simulation curve, it can be seen that for the flight time of 400 s, the average position error accumulation of the label aircraft could reach more than 450 m only by relying on the inertial navigation system for navigation and positioning. After using the cooperative correction method for navigation errors of heterogeneous multi-aircraft based on pseudo-observation, the algorithm for only ranging cooperation reduced the cumulative position error to less than 300 m. On this basis, an anchor aircraft was added, and the accumulated position error was reduced to less than 150 m. It can be seen that the algorithm improved the overall positioning accuracy of the cluster by solving the observation rank defect, and the addition of the anchor aircraft further aligned the observation geometric network with the real geometric network, additionally improving the navigation accuracy. With the addition of line-of-sight angle assistance, the pseudo-observation-based coordinated correction method for navigation errors of heterogeneous multi-aircraft could also reduce the position error to less than 250 m without the addition of an anchor aircraft, which was superior to the algorithm simulation results that only relied on the range coordination method. With the addition of anchor aircraft assistance, the positioning accuracy was also greatly improved due to the fact that the observation itself was not rank defect, and the average position error was limited to 50 m. The simulation results verify the effectiveness of the co-correction method for navigation errors of heterogeneous multi-aircraft based on pseudo-observation in dynamic navigation.

5. Conclusions

Aiming at the problem of the insufficient observability of inter-aircraft cooperative measurement, this paper studied the cooperative correction method for navigation errors based on pseudo-observation. In this paper, we analyzed the observation rank defect of the heterogeneous multi-aircraft navigation and integrated the cooperative observation equation and the pseudo-observation equation between the aircraft. We also constructed a new multi-aircraft navigation model and derived the model solution method. The simulation results show that the collaborative correction method proposed in this paper can play a role in mitigating the divergence of navigation errors of multi-aircraft when the external available information is insufficient in the harsh GNSS environment. Simulation analyses of the cluster configuration, measurement accuracy, and the number of aircraft that affect the performance of the algorithm were carried out. The results show that the square configuration and line configuration of multiple aircraft reduce the performance of the algorithm, while the improvement in the measurement accuracy and the increase in the number of aircraft can enhance the effectiveness of the navigation error collaborative correction method within a certain range.