Simulation Experiment and Analysis of GNSS/INS/LEO/5G Integrated Navigation Based on Federated Filtering Algorithm

Abstract

This article examines the positioning effect of integrated navigation after adding an LEO constellation signal source and a 5G ranging signal source in the context of China’s new infrastructure construction. The tightly coupled Kalman federal filters are used as the algorithm framework. Each signal source required for integrated navigation is simulated in this article. At the same time, by limiting the range of the azimuth angle and visible height angle, different experimental scenes are simulated to verify the contribution of the new signal source to the traditional satellite navigation, and the positioning results are analyzed. Finally, the article compares the distribution of different federal filtering information factors and reveals the method of assigning information factors when combining navigation with sensors with different precision. The experimental results show that the addition of LEO constellation and 5G ranging signals improves the positioning accuracy of the original INS/GNSS by an order of magnitude and ensures a high degree of positioning continuity. Moreover, the experiment shows that the federated filtering algorithm can adapt to the combined navigation mode in different scenarios by combining different precision sensors for navigation positioning.

1. Introduction

Compared with early navigation methods, modern navigation has entered the information age centered on integrated navigation systems. The integrated navigation system is also called multi-sensor information fusion. According to different task scenarios, using multiple sensors for integrated navigation and optimally fusing multiple types of information according to a certain optimal fusion criterion can be expected to improve positioning accuracy. Commonly used single-sensor navigation methods today include the Inertial Navigation System (Inertial Navigation System, INS), which has autonomous navigation capabilities, but INS positioning error drifts greatly over time [1]. The other one is the Satellite Navigation System (Global Navigation Satellite System, GNSS), which has all-weather, high-precision positioning ability. However, GNSS has the shortcomings of insufficient satellite signal reception and an instantaneous increase in positioning error when the signal is blocked [2]. Integrated navigation is designed based on the complementary performance of a single navigation system; that is, the combination of GNSS/INS can achieve autonomous and high-precision navigation and positioning to a certain extent [3,4]. This combination method has been studied in academia and has been applied in industry. However, in areas such as tall buildings, or tunnels and mines, GNSS signals cannot be received for a long time, and at the same time, the INS positioning error drifts greatly. For example, under the mine, there is no GNSS signal, and it needs to rely on INS for a long time. However, the positioning error of tactical IMU will drift to about 10 m within one minute [5]. In this context, this article proposes to use Low Earth Orbit constellation enhancement and 5G signals as new sensor signal sources for integrated navigation simulation experiments to solve the positioning failure caused by the lack of GNSS signals in specific areas.

Since 2020, the discussion of “new infrastructure” in China has heated up dramatically. As can be seen from relevant research reports [6,7], in the new infrastructure, both satellite internet and 5G infrastructure construction can be used as a National Positioning Infrastructure (NPI).

Satellite internet projects such as Telesat, OneWeb, and Starlink are mainly distributed in the orbit altitude range of 400 km to 1400 km. In China, constellations represented by “Hongyan Constellation”, “Hongyun Project”, “Xingyun Project”, “Celestial Constellations”, and “Milky Way 5G” were designed with heights ranging from 500 km to 1200 km. The above satellite constellations can all be called Low Earth Orbit Satellite Systems (LEO). Nowadays, the accuracy of low-orbit satellite orbit calculation can reach the centimeter-level [8,9,10]. The satellite launch and networking technologies have become more mature so that the number of available satellites has greatly increased, and some technical problems of low-orbit satellite navigation algorithms have been solved. Combining satellite load and application requirements, designing a low-orbit constellation that can be used for navigation, and forming a low-orbit constellation with integrated communication and navigation is an inevitable trend of constellation development [11,12,13]. At the same time, the research of combining LEO and INS for navigation is in the ascendant. Existing research shows that when there is no GNSS signal, the accuracy of integrated navigation using LEO constellation signal and INS is obviously higher than that using INS alone. This proves that using LEO/INS is feasible [14]. Taking advantage of the fast speed of LEO satellite, Doppler frequency shift measurement and positioning technology can be used in LEO satellite. The effect of tight coupling between this technology and INS is also very good, when GNSS is unavailable for 30 s, the final error is reduced from 31.7 m to 8.9 m [15,16,17,18].

As another main component of the new infrastructure, the 5th-generation mobile communication system (5th-generation, 5G) is widely regarded as the foundation of the new generation of the internet. The Internet of Things and Industrial Internet based on 5G have also received widespread attention [19]. The international standards organization 3GPP (3rd Generation Partnership Project), which is leading the 5G communication system protocol standard, has formally defined the commercial application scenarios of 5G positioning requirements in the TR22.872 standard [20]. The use of 5G communication systems for indoor and outdoor positioning is a current research hotspot. Indoor and outdoor positioning algorithms based on the characteristics of 5G millimeter-wave signals have also been extensively studied, and the existing algorithms have reached sub-meter positioning accuracy [21,22,23]. Hybrid positioning schemes based on the fusion of 5G cellular, GNSS/INS are to be studied and developed towards a universal solution for robust positioning of aerial or ground vehicles in urban, rural, and indoor scenarios [24]. Studies have also shown that positioning 5G base stations on both sides of the expressway can improve the robustness and accuracy of the car navigation system on the road [25]. The research of indoor and outdoor joint positioning using 5G shows that when there is no GNSS signal, it is a good choice to use 5G signal instead. Although there have studies use federated filtering, INS is not used as a reference system [26,27]. These studies show that integrated navigation using new sensors needs to be more comprehensive.

In the above context, this article finds that algorithm research and technical application of combined GNSS/INS/LEO/5G navigation and positioning are ascendant in the existing research. Therefore, based on the new sensors included in the new infrastructure, this article proposes implementing a GNSS/INS/LEO/5G integrated navigation simulation experiment by using a federated filtering algorithm to verify the feasibility and positioning accuracy of different integrated navigation positioning schemes.

The integrated navigation simulation verification scheme in this article uses GNSS and LEO satellite constellations simulations to derive the satellite position and speed in the simulation time, which is used as the data source for the integrated navigation satellite positioning. The IMU error model is used to build IMU output specific force and angular velocity models, which are used for integrated navigation, as the INS’s data sources. The 5G ranging signal is simulated by adding random noise based on the given theoretical coordinates of the base station and receiver, based on the 5G ranging signal error model, to provide the ranging value of the 5G signal. Through the implementation of a federated filtering algorithm, the combined navigation and positioning results of the above multi-source signals are output and compared with the true value to verify the navigation and positioning accuracy.

The structure of this article is as follows. The first section is the introduction; the second section introduces the algorithm model used in this article, including algorithm structure frame, simulation scene construction, and simulation method of each signal source; in the third section, the experimental results are compared and explained; finally, in the fourth section, the author puts forward the summary and conclusions of this article. The potential innovations of this article are as follows: simulation verifies the advantages of new integrated navigation using LEO constellation and 5G over traditional integrated navigation; the applicability of the new integrated navigation proposed in this article is presented for the positioning effect in different scenarios. A proposal of information factor allocation using a federated filtering algorithm is proposed when different precision sensors are used.

2. The Model for GNSS/INS/LEO/5G Integrated Navigation

At present, the most successful and most applied multi-sensor data fusion method is the federated Kalman filtering method. The federated filter was proposed by Carlson [28]. Because of its flexibility, a small amount of calculation, and good fault tolerance, it has been extensively studied. This method is very flexible for multi-source heterogeneous navigation systems and can support the introduction of new sensor types at any time. This article uses GNSS, INS, LEO, and 5G multi-source heterogeneous navigation information to perform integrated navigation and positioning. There are many types of sensors and rich application scenarios. Therefore, the federated Kalman filter meets the requirements of this article for algorithm functions. The basic algorithm framework of source integrated navigation is the result of this article.

2.1. The Structure of Federated Filtering Algorithm

The structure diagram of the federated filter model used in this article is shown in Figure 1.

Each sub-filter of the federated filtering model used in this paper is introduced as follows.

2.1.1. INS Reference System

The reference system is the INS navigation system, whose navigation calculation is mechanically arranged in the ECEF coordinate system. They are:

• Attitude update:

$$\begin{array}{c}\begin{array}{c}\begin{array}{c}\begin{array}{c}{C}_b^e(+)=\left(\begin{array}{ccc}\cos {\omega }_{ie}{\tau }_i& \sin {\omega }_{ie}{\tau }_i& 0\\ -\sin {\omega }_{ie}{\tau }_i& \cos {\omega }_{ie}{\tau }_i& 0\\ 0& 0& 1\end{array}\right)C_b^e(-)C_{b+}^{b-}\\ \approx C_b^e(-)C_{b+}^{b-}-{\Omega }_{ie}^e{C}_b^e(-){\tau }_i\end{array}\end{array}\end{array}\end{array}$$

(1)

where ${\omega }_{ie}$ is the Earth’s rotational velocity. ${\tau }_i$ is the IMU update interval time. $C_b^e(-)$ is the pose matrix for the previous moment which means the transformation is from body coordinate system to ECEF coordinate system. ${\Omega }_{ie}^e$ is the antisymmetric matrix of ${\omega }_{ie}$. $C_{b+}^{b-}$ is the attitude transfer matrix, calculated using the Rodriguez formula:

$$\begin{array}{c}{C}_{b+}^{b-}=I_3+\frac{\sin \left|{\alpha }_{ib}^b\right|}{\left|{\alpha }_{ib}^b\right|}\left[{\alpha }_{ib}^b\wedge \right]+\frac{1-\cos \left|{\alpha }_{ib}^b\right|}{{\left|{\alpha }_{ib}^b\right|}^2}{\left[{\alpha }_{ib}^b\wedge \right]}^2\end{array}$$

(2)

where ${\alpha }_{ib}^b$ is the rotation vector of the body coordinate system relative to the inertial coordinate system and project in the body coordinate system. ${\alpha }_{ib}^b\wedge$ is the antisymmetric matrix of the rotation vector ${\alpha }_{ib}^b$. The subscripts $b+$ and superscripts $b-$ in $C_{b+}^{b-}$ means the transformation is from $b+$ to $b-$. The + means the current moment. The $–$ means the previous moment.

2.

The specific force coordinate transformation:

$$\begin{array}{c}{C}_b^i=C_b^i(-)C_b^{b-},C_b^{b-}=I_3+\frac{1-\cos \left|{\alpha }_{ib}^b\right|}{{\left|{\alpha }_{ib}^b\right|}^2}\left[{\alpha }_{ib}^b\wedge \right]+\frac{1}{{\left|{\alpha }_{ib}^b\right|}^2}\left(1-\frac{\sin \left|{\alpha }_{ib}^b\right|}{\left|{\alpha }_{ib}^b\right|}\right){\left[{\alpha }_{ib}^b\wedge \right]}^2\end{array}$$

(3)

$$\begin{array}{c}{f}_{ib}^e=C_b^e{f}_{ib}^b,C_b^e=C_b^e(-)C_b^{b-}-\frac{1}{2}{\Omega }_{ie}^e{C}_b^e(-){\tau }_i\end{array}$$

(4)

where $C_b^i$ means the transformation is from body coordinate system to inertial coordinate system. $I_3$ is the Identity matrix. $f_{ib}^e$ is the specific force of the body coordinate system relative to the inertial coordinate system and project in the ECEF coordinate system. $f_{ib}^b$ is the specific force of the body coordinate system relative to the inertial coordinate system and project in the body coordinate system. The descriptions of other symbols are mentioned above.

3.

Speed and position update:

Speed update:

$$\begin{array}{c}\begin{array}{c}{v}_{eb}^e(+)\approx v_{eb}^e(-)+\left(f_{ib}^e+g_b^e\left(r_{eb}^e(-)\right)-2{\Omega }_{ie}^e{v}_{eb}^e(-)\right){\tau }_i\\ =v_{eb}^e(-)+v_{ib}^e+\left(g_b^e\left(r_{eb}^e(-)\right)-2{\Omega }_{ie}^e{v}_{eb}^e(-)\right){\tau }_i\end{array}\end{array}$$

(5)

where $v_{eb}^e(+)$ is velocity of the body coordinate system relative to the ECEF coordinate system and project in the ECEF coordinate system in current moment. $g_b^e$ is the gravity acceleration projected from body system to ECEF system.

Position update:

$$\begin{array}{c}\begin{array}{c}{r}_{eb}^e(+)=r_{eb}^e(-)+\left(v_{eb}^e(-)+v_{eb}^e(+)\right)\frac{{\tau }_i}{2}\\ \approx r_{eb}^e(-)+v_{eb}^e(-){\tau }_i+\left(f_{ib}^e+g_b^e\left(r_{eb}^e(-)\right)-2{\Omega }_{ie}^e{v}_{eb}^e(-)\right)\frac{{\tau }_i^2}{2}\end{array}\end{array}$$

(6)

where $r_{eb}^e(+)$ is the coordinate of the body coordinate system relative to the ECEF coordinate system and project in the ECEF coordinate system in current moment.

The output of the INS mechanical arrangement solution is position, speed, and attitude, and the calculated results are used as the input of sub-filter and federated filter (the initial value of state estimation). After filtering, the error state quantity of the sub-filter is added to the state estimation for state correction.

2.1.2. GNSS/INS Sub-Filter

The GNSS/INS sub-filter uses tight coupling, and its flow chart is shown in Figure 2 below. In this paper, closed-loop correction is used as the final output of integrated navigation to reduce the error of linearization.

In this paper, the state vector of GNSS/INS sub-filter is based on the error state model, that is:

$$\begin{array}{c}x=\left\{\delta \psi ,\delta v,\delta r,b_a,b_g,\delta {\rho }_c^a,\delta {\dot{\rho }}_c^a\right\}\end{array}$$

(7)

where $\delta \psi$ is the attitude error. $\mathsf{\delta }v$ is the velocity error. $\mathsf{\delta }r$ is the position error. $b_a$ is the accelerometer offset. $b_g$ is the gyroscope offset. $\delta {\rho }_c^a$ is the receiver clock error. $\delta {\dot{\rho }}_c^a$ is the receiver clock drift.

The transfer matrix of the GNSS/INS sub-filter system is the first-order term:

$$\begin{array}{c}{\Phi }_{GNSS, INS}^e\approx \left[\begin{array}{ccccccc}{I}_3-{\Omega }_{ie}^e{\tau }_s& 0_3& 0_3& 0_3& {\hat{C}}_b^e{\tau }_s& 0& 0\\ F_{21}^e{\tau }_s& I_3-2{\Omega }_{ie}^e{\tau }_s& F_{23}^e{\tau }_s& {\hat{C}}_b^e{\tau }_s& 0_3& 0& 0\\ 0_3& I_3{\tau }_s& I_3& 0_3& 0_3& 0& 0\\ 0_3& 0_3& 0_3& I_3& 0_3& 0& 0\\ 0_3& 0_3& 0_3& 0_3& I_3& 0& 0\\ 0& 0& 0& 0& 0& 1& {\tau }_s\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]\end{array}$$

(8)

where $F_{21}^e$ and $F_{23}^e$ are:

$$\begin{array}{c}{F}_{21}^e=\left[-\left({\widehat{C}}_b^e{\widehat{f}}_{ib}^b\right)\wedge \right],\end{array}$$

(9)

$$\begin{array}{c}{F}_{23}^e=-\frac{2{\widehat{\gamma }}_{ib}^e}{r_{eS}^e\left({\widehat{L}}_b\right)}\frac{{\widehat{r}}_{eb}^{e^T}}{\left|{\widehat{r}}_{eb}^e\right|}\end{array}$$

(10)

where, the $\wedge$ in ${\widehat{C}}_b^e$ means that this is an estimate of the rotation matrix. ${\widehat{\gamma }}_{ib}^e$ is the gravitational acceleration at the estimated position. $r_{eS}^e$ is coordinates of the satellite in ECEF system.

The system covariance matrix is:

$$\begin{array}{c}P = \left[\begin{array}{ccccccc}S{D}_{att}{}_3^2& 0_3& 0_3& 0_3& 0_3& 0& 0\\ 0_3& S{D}_{vel}{}_3^2& 0_3& 0_3& 0_3& 0& 0\\ 0_3& 0_3& S{D}_{pos}{}_3^2& 0_3& 0_3& 0& 0\\ 0_3& 0_3& 0_3& S{D}_{ba}{}_3^2& 0_3& 0& 0\\ 0_3& 0_3& 0_3& 0_3& S{D}_{bg}{}_3^2& 0& 0\\ 0& 0& 0& 0& 0& S{D}_c^a{}^2& 0\\ 0& 0& 0& 0& 0& 0& S{D}_{\mathit{cd}}^a{}^2\end{array}\right]\end{array}$$

(11)

where, SD_i is the standard deviation of attitude angle, velocity, position, accelerometer deviation, gyroscope deviation, receiver clock error, and receiver clock drift, which is calculated in the form of constant in the simulation.

The innovation matrix is:

$$\delta z_{G,k}^{-} = \left(\begin{array}{c}\delta z_{\rho ,k}^{-}\\ \delta z_{r,k}^{-}\end{array}\right),\begin{array}{l}\delta z_{\rho ,k}^{-} = {\left( {\tilde{\rho }}_{a,C}^1-{\hat{\rho }}_{a,C}^{1-}, {\tilde{\rho }}_{a,C}^2-{\hat{\rho }}_{a,C}^{2-},\cdots {\tilde{\rho }}_{a,C}^m- {\hat{\rho }}_{a,C}^{m-}\right)}_k\\ \delta z_{r,k}^{-} = {\left( {\tilde{\dot{\rho }}}_{a,C}^1-{\widehat{\dot{\rho } }}_{a,C}^{1-}, {\tilde{\dot{\rho }}}_{a,C}^2-{\widehat{\dot{\rho } }}_{a,C}^{2-},\cdots {\tilde{\dot{\rho }}}_{a,C}^m-{\widehat{\dot{\rho } }}_{a,C}^{m-}\right)}_k\end{array}$$

(12)

where ${\tilde{\rho }}_{a,C}^1$, ${\tilde{\dot{\rho }}}_{a,C}^1$ is the pseudo-range and pseudo-range rate for measurement $. {\widehat{ \rho }}_{a,C}^{1-}$, ${\widehat{\dot{\rho }}}_{a,C}^{1-}$ is the estimated pseudo-range and pseudo-range rate.

The observation matrix is:

$$\begin{array}{c}{H}_{G,k}^{\gamma }\approx {\left(\begin{array}{ccccccc}{0}_{1,3}& 0_{1,3}& u_{a1}^{\gamma T}& 0_{1,3}& 0_{1,3}& 1& 0\\ 0_{1,3}& 0_{1,3}& u_{a2}^{\gamma T}& 0_{1,3}& 0_{1,3}& 1& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ 0_{1,3}& 0_{1,3}& u_{am}^{\gamma T}& 0_{1,3}& 0_{1,3}& 1& 0\\ 0_{1,3}& u_{a1}^{\gamma T}& 0_{1,3}& 0_{1,3}& 0_{1,3}& 0& 1\\ 0_{1,3}& u_{a2}^{\gamma T}& 0_{1,3}& 0_{1,3}& 0_{1,3}& 0& 1\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ 0_{1,3}& u_{am}^{\gamma }T& 0_{1,3}& 0_{1,3}& 0_{1,3}& 0& 1\end{array}\right)}_{x = {\hat{x}}_k^{-}}\end{array}$$

(13)

where $u_{a1}^{\gamma T}$ is the line-of-sight unit vector from the user antenna, a, to satellite, 1.

The observation noise matrix is:

$$\begin{array}{c}{R}_{G,k}^{\gamma }\approx {\left(\begin{array}{cc}S{D}_{\rho }{}^2\ast I_{m,m}& 0_{m,m}\\ 0_{m,m}& {\dot{S{D}_{\rho }}}^2\ast I_{m,m}\end{array}\right)}_{x={\widehat{x}}_k^{-}}\end{array}$$

(14)

The Kalman filtering algorithm is Formulas (15)–(19):

$$\begin{array}{c}{\widehat{x}}_k^{-}={\Phi }_{k-1}{\widehat{x}}_{k-1}^{+}\end{array}$$

(15)

$$\begin{array}{c}{P}_k^{-}={\Phi }_{k-1}\left(P_{k-1}^{+}+\frac{1}{2}{Q}_{k-1}^{\prime }\right){\Phi }_{k-1}^T+\frac{1}{2}{Q}_{k-1}^{\prime }\end{array}$$

(16)

$$\begin{array}{c}{K}_k=P_k^{-}{H}_k^T{\left(H_k{P}_k^{-}{H}_k^T+R_k\right)}^{-1}\end{array}$$

(17)

$$\begin{array}{c}\begin{array}{c}{\widehat{x}}_k^{+}={\widehat{x}}_k^{-}+K_k\left(z_k-H_k{\widehat{x}}_k^{-}\right)={\widehat{x}}_k^{-}+K_k\delta z_k^{-}\end{array}\end{array}$$

(18)

$$\begin{array}{c}{P}_k^{+}=\left(I-K_k{H}_k\right)P_k^{-}\end{array}$$

(19)

where ${\widehat{x}}_{k-1}^{+}$ is system state vector in the moment of $k-1$. ${\Phi }_{k-1}$ is the system transfer matrix in the moment of $k-1$. $P_{k-1}^{+}$ is the covariance matrix in the moment of $k-1$. $Q_{k-1}^{\prime }$ is system noise matrix. $R_k$ measurement noise matrix. $K_k$ is the Gain matrix. $I$ is the unit matrix.

2.1.3. LEO/INS Sub-Filter

The structure of the LEO/INS sub-filter is similar to that of the GNSS/INS sub-filter. The difference is that the parameters of the LEO constellation model are different from those of the GNSS constellation. The main difference lies in the constellation orbit height, the number of orbital planes, and other parameters. See Section 2.3 for details.

2.1.4. 5G/INS Sub-Filter

The 5G/INS sub-filter adopts a tight coupling structure similar to the GNSS/INS sub-filter. The difference is that the measured value of the 5G/INS sub-filter is the 5G pseudo-range measurement value and pseudo-range rate obtained by adding Gaussian noise to the theoretical distance between the receiver and four 5G base stations. The innovation matrix of the 5G/INS sub-filter is calculated using the measurements derived from INS mechanical arrangement of pseudo-range and pseudo-range rate measurements generated by 5G signals.

2.1.5. Federated Main Filter

The inertial navigation system, as a reference system, is separately combined with GNSS, LEO, and 5G systems to construct a set of local error state Kalman filters, which can be regarded as the first stage of federated filter integrated navigation. In the second stage of federated filter integrated navigation, the output of each sub-filter is combined to form the integrated navigation result. In this paper, the federated main filter is used for information fusion.

The federated master filter is used for information fusion, and the state vector is:

$$\begin{array}{c}{\widehat{X}}_{fk}=\left\{\delta \psi ,\delta v,\delta r,b_a,b_g,\rho ,\dot{\rho }\right\}\end{array}$$

(20)

The relationship between the state vector of the main filter and the sub-filter is:

$$\begin{array}{c}{\widehat{X}}_{fk}=P_{fk}{\displaystyle \sum _{j=1}^N}{P}_{jk}^{-1}{\widehat{X}}_{jk}\end{array}$$

(21)

where $P_{jk}^{-1}$ is the covariance matrix of sub-filter $j. {\widehat{X}}_{jk}$ is the state estimate of sub-filter $j$. $N$ is the number of sub-filter. $P_{fk}$ is the federated filtering covariance matrix, that is:

$$P_{fk}=\left({\displaystyle \sum _{j=1}^N}{P}_{jk}^{-1}\right)$$

(22)

The covariance matrix of the federated main filter $P_{fk}$ is obtained by summing the inverse covariance matrices of each sub-filter, and then inverting. Then, find the sum of the product of the state values of each sub-filter ${\widehat{X}}_{jk}$ and the covariance inverse matrix $P_{jk}^{-1}$. Finally, the final state solution after federated filtering ${\widehat{X}}_{fk}$ is obtained by multiplying the $P_{fk}$ matrix with the sum ${\sum }_{j=1}^N{P}_{jk}^{-1}{\widehat{X}}_{jk}$. By combining the state values and covariance of each sub-filter, the federated filter value is finally formed according to Formula (21), which is the mathematical significance of federated filtering.

According to the different applications of federated filtering to state vector and covariance matrix, this paper mainly compares the following federated filtering combinations:

• Federation no reset combination, FNR:

In FNR mode, Formulas (21) and (22) are used as the combination of single point fusion algorithms [29]. When the fusion algorithm is updated, it can be directly used to update the federated integrated navigation output without feedback on the state estimation and covariance of the local filter.

2.

Federation fusion reset combination, FFR:

In the FFR mode, the state estimation and error covariance matrix obtained from the information fusion of the federated main filter is fed back to the sub-filter. The sub-filter replaces the corresponding state vector, covariance matrix, and the feedback covariance matrix is multiplied by the coefficient ${\beta }_j$ [28]. ${\beta }_j$ is the information distribution factor of sub-filter $\mathrm{j}$, which satisfies the principle of information fusion; that is, for N sub-filters, there are ${\sum }_{j=1}^N{\beta }_j=1$.

3.

Federation Zero reset combination, FZR:

In FZR mode, after the state estimation result of the sub-filter is input to the main filter, the state values of all the sub-filters are set to zero, and the corresponding covariance matrix element values are set to the initial value [29].

2.2. Simulation Scene Construction

The motion truth file in this article refers to the data provided by the open-source project “UrbanNavDataset” [30]. The data set was collected in a typical urban canyon in Tokyo on 19 December 2018, and its trajectory in the northeast geodetic coordinate system is shown in Figure 3.

The experimental scene in this paper is vehicle navigation at a low speed on the ground (in the ECEF coordinate system, the speed of each axis is less than that of 20 m/s), the site is the urban high-rise canyon area, and the carrier vehicle has multiple right-angle turning maneuvers. At the same time, this paper simulates and constructs different experimental scenes by setting the range of mask angle and azimuth angle.

The parameter patterns of each simulation scene in this paper are shown in

Table 1. Simulation scene parameters.

Category	Mask Angle (deg)	Azimuth Range (deg)
Open scene	10	[−180, 180]
Semi-occluded scene	10	[−90, 90]
Surrounding occluded scene	45	[−180, 180]
Headspace occluded scene	[10, 45]	[−180, 180]

.

The schematic diagram of each simulation scene is shown in Figure 4.

It can be seen from Table 1 and Figure 4 that the simulation scene in this paper includes four modes: Open scene, Semi-occluded scene, Surrounding occluded scene, and Headspace occluded scene. In the Open scene (Figure 4a), there is no restriction on azimuth angle, the mask angle is low so that there are more visible satellite data. The azimuth of the Semi-occluded scene (Figure 4b) ranges from −90° to 90°, significantly reducing the number of visible satellites. For the Surrounding occluded scene (Figure 4c), the mask angle is raised to 50°, and the azimuth angle is not restricted to simulate the urban canyon environment. As a result, the number of visible satellites in Surrounding occluded scene is also small. In the Headspace occluded scene (Figure 4d), the mask angle is limited to 10° to 45°, and the azimuth angle is not limited to simulate the environment under the urban viaduct. Naturally, the number of visible satellites in Headspace occluded scene is lower.

2.3. Simulation Method of Each Signal Source

2.3.1. GNSS and LEO Constellation Simulation

Both GNSS and LEO constellation simulations in this paper are Walker constellations and aim to optimize constellation coverage with a given number of satellites. Walker constellation is proposed after Ballard’s improvement in literature [31], forming a constellation system widely used in the field of orbit design [32].

The parameters of the Walker constellation include the number of satellites T, the number of orbits P, and the number of satellites per orbit S. All P orbital planes have the same orbital inclination i (relative to the equatorial plane). On each orbital plane, S satellites are evenly distributed on the orbital plane with an angular distance of 360°/S. The ascending points of each orbital plane are evenly distributed on the equatorial plane with an interval of 360°/P. In order to maintain the relative position relationship between satellites of different orbital planes, the strategy of the equal time interval between satellites of adjacent orbital planes passing through their ascending intersection is adopted. Therefore, the relative phase must be an integer multiple of 360°/T, F can be any integer between 0 and $P-1$. By giving three parameters T, P, S, and orbital inclination i, the constellation form can be completely determined, namely the Walker constellation.

Table 2. GNSS/LEO Walker constellation parameters.

Parameters	GNSS	LEO
Number of satellite—T	30	120
Number of orbits—P	6	12
Number of satellites per orbit—S	5	10
Number of relative phase multiple—F	1	1
Angle of orbital inclination—i	55 deg	40 deg
Altitude of orbit—h	20,183.65 km	1139.9 km

shows the parameters of the GNSS/LEO Walker constellation simulation. The constellation distribution operation diagram after STK software simulation is shown in the following figure.

The simulation results after GNSS and LEO were simulated and connected with the ground are shown in Figure 5.

The visibility analysis is shown in Figure 6, Figure 7 and Figure 8. As can be seen from Figure 6, Figure 7 and Figure 8, the selected target area is 139.776° E~139.802° E and 35.569° N~35.687° N, and the visibility of the target area is 100% within 24 h of simulation. According to the global visibility analysis report of the GNSS+LEO constellation in Figure 8, it can be concluded that the GNSS and LEO constellation simulated in this paper can provide satellite-ranging signal data source for integrated navigation in the target area.

Based on the GNSS/LEO constellation simulation to obtain the satellite position and speed, the pseudo-range measurement value of satellite navigation can be obtained using Formula (23) below:

$$\begin{array}{c}\widehat{\rho }=\rho +\delta {\rho }_I+\delta {\rho }_T+\delta {\rho }_{SIS}+\delta {\rho }_{ct}+\delta {\rho }_{sc}+\delta {\rho }_c+\delta {\dot{\rho }}_{cd}\times ∆t\end{array}$$

(23)

where $\widehat{\rho }$ is the measured value of satellite navigation pseudo-range. $\rho$ is the theoretical distance between satellite position and receiver. $\delta {\rho }_I$ is the ionospheric error. $\delta {\rho }_T$ is tropospheric error. $\delta {\rho }_{SIS}$ is the spatial signal propagation error, which is mainly caused by multipath, non-line-of-sight, and diffraction phenomena. $\delta {\rho }_{ct}$ is code tracking error. $\delta {\rho }_{sc}$ is the clock error of satellite. $\delta {\rho }_c$ is the clock error of receiver. $\delta {\dot{\rho }}_{cd}$ is the receiver clock drift. $∆t$ is the time interval for satellite navigation.

The measurement value of pseudo distance ratio is obtained by Formula (24):

$$\begin{array}{c}\widehat{\dot{\rho }}=\dot{\rho }+\delta {\dot{\rho }}_{cd}+\delta {\dot{\rho }}_{ct}\end{array}$$

(24)

where $\widehat{\dot{\rho }}$ is the measurement of satellite navigation pseudo-range ratio. $\dot{\rho }$ is the theoretical distance. $\delta {\dot{\rho }}_{cd}$ is the receiver clock drift. $\delta {\dot{\rho }}_{ct}$ is the range-rate tracking error.

The error parameters used for satellite navigation in this paper are given in

Table 3. GNSS/LEO navigation model parameters.

Parameters	Value
	GNSS	LEO
Satellite positioning time interval (s)— $∆t$	1	1
Mask Angle (deg, it can be adjusted according to different scenarios)	10	10
Zenith ionosphere error SD (m)— $\delta {\rho }_I$	2	1.5
Zenith troposphere error SD (m)— ${\delta \rho }_T$	0.2	0.15
Space signal error SD (m)— $\delta {\rho }_{SIS}$	1	1
Code tracking error SD (m)— $\delta {\rho }_{ct}$	0.5	1
Satellite clock error SD(m)— $\delta {\rho }_{sc}$	0.5	1
Distance rate tracking error SD (m/s)— $\delta {\dot{\rho }}_{ct}$	0.02	0.05
Initial estimated position of receiver (m, in ECEF coordinate system)	[0,0,0]	[0,0,0]
Receiver clock offset (m)— $\delta {\rho }_c$	10,000	10,000
Receiver clock drift (m/s)— $\delta {\dot{\rho }}_{cd}$	100	100

.

Table 3 shows related parameters used by GNSS and LEO satellite navigation models. On the basis of theoretical distance and range rate obtained by simulation, range error and range rate error are added, which are the measurement values of pseudo-range and pseudo-range rate of GNSS and LEO satellite navigation models.

2.3.2. IMU Model Simulation

The IMU error model in this paper is:

$$\begin{array}{c}\begin{array}{c}{\tilde{f}}_{ib}^b=b_a+\left(I_3+M_a\right)f_{ib}^b+w_a,\\ {\tilde{\omega }}_{ib}^b=b_g+\left(I_3+M_g\right){\omega }_{ib}^b+G_g{f}_{ib}^b+w_g\end{array}\end{array}$$

(25)

where ${\tilde{f}}_{ib}^b$ is the output specific force of IMU. $f_{ib}^b$ is the theoretical value of specific force. $b_a$ is the acceleration deviation. $M_a$ is the scale factor and cross-coupling error of the accelerometer. $w_a$ is the random noise of the accelerometer. $b_g$ is the gyroscope bias. ${\tilde{\omega }}_{ib}^b$ is the IMU output angular rate. ${\omega }_{ib}^b$ is the theoretical value of angular velocity. $M_g$ is the gyroscope scale factor and cross-coupling error. $G_g$ is the acceleration of gravity-related gyro bias. $w_g$ is the gyroscope random noise.

Table 4. IMU model parameters.

Parameters	Value
Accelerometer bias(m/s2)—$b_a$	$\left[0.0088, -0.0127, 0.0078\right]$
gyroscope bias (deg/h)—$b_g$	$\left[10, 10, 10\right]$
Accelerometer scale factor and cross-coupling error— $M_a$	$\left[\begin{array}{ccc}500& -300& 200\\ -150& -600& 250\\ -250& 100& 450\end{array}\right]\times {10}^{-6}$
Gyroscope scale factor and cross-coupling error— $M_g$	$\left[\begin{array}{ccc}400& -300& 250\\ 0& -300& -150\\ 0& 0& -350\end{array}\right]\times {10}^{-6}$
Gyroscope gravity acceleration correlation bias (deg/h/g)—$G_g$	$\left[\begin{array}{ccc}0.9& -1.1& -0.6\\ -0.5& 1.9& -1.6\\ 0.3& 1.1& -1.3\end{array}\right]$
Accelerometer random noise PSD root(m·s−1.5)$—w_a$	$9.80665\times {10}^{-4}$
Gyro random noise PSD root(rad·s−0.5)—$w_g$	$2.9089\times {10}^{-6}$

shows the simulation parameters of the IMU model. In this paper, INS navigation calculation is carried out under the ECEF coordinates system, and the IMU error model value (Table 4) is added based on the theoretical value of specific force $f_{ib}^b$ and theoretical value of angular velocity ${\mathsf{\omega }}_{ib}^b$ obtained by simulation, which is the measured value of IMU.

2.3.3. 5G Signal Simulation

The simulation of 5G ranging signal in this paper, obtained from Formulas (26) and (27), is obtained by adding simulation signal error based on theoretical distance value and range rate between the base station and receiver, namely:

$$\begin{array}{c}{\widehat{\rho }}_{5G}={\rho }_{5G}+\delta {\rho }_{c-5G}+\delta {\dot{\rho }}_{cd-5G}\times ∆t\end{array}$$

(26)

$$\begin{array}{c}{\widehat{\dot{\rho } }}_{5G}={\dot{\rho }}_{5G}+\delta {\dot{\rho }}_{cd-5G}+\delta {\dot{\rho }}_{cd-5G}\end{array}$$

(27)

where ${\widehat{\rho }}_{5G}$ and ${\widehat{\dot{\rho }}}_{5G}$ are the pseudo-range measurement and pseudo-range rate measurement of the 5G signal, respectively. ${\rho }_{5G}$ and ${\dot{\rho \dot{}}}_{5G}$ are the theoretical distance and the distance rate. $\delta {\rho }_{c-5G}$ is the clock bias of the receiver. $\delta {\dot{\rho }}_{cd-5G}$ is the clock drift deviation. ${\dot{\rho }}_{ct-5G}$ is the distance rate tracking error. $∆t$ is a 5G positioning time interval.

Table 5. 5G navigation model parameters.

Parameters	Value
Time interval (s)—∆t	1
Receiver clock offset (m)— $\delta {\rho }_{c-5G}$	10,000
Receiver Clock drift (m/s)— $\delta {\dot{\rho }}_{cd-5G}$	100
The number of base stations	4
Base station signal coverage radius (m)— $\mathrm{r}$	400
Signal number tracking error (m)— $\delta {\rho }_{ct-5G}$	0.2
Range rate tracking error (m/s)— $\delta {\dot{\rho }}_{ct-5G}$	0.1

shows the parameters of the 5G-ranging models used in this paper. In order to simulate the coverage of 5G signal on the motion track, in this paper, according to the coordinates of the center point of the motion track, taking the base station coverage radius r as the axis, four base stations are evenly distributed in the same height plane with an azimuth interval of 90°.The coordinates of each base station are calculated by the above distribution method and 5G model parameters.

3. Experimental Results

3.1. Comparison of Location Results in Different Scenarios

After setting parameters in different scenarios, the results of the simulation experiment are as follows. Note that FNR mode is adopted for federated filtering in this section.

3.1.1. Open Scene

The positioning result after setting the azimuth angle and mask angle of the open scene in Section 2.2 is shown in the figures below.

As can be seen from Figure 9, after the LEO constellation was introduced in the open scene, the LEO/INS position accuracy increased by an order of magnitude to sub-meter in the east direction compared with GNSS/INS position accuracy. Affected by the poor accuracy of 5G/INS position, the overall federated filtering accuracy is in meter level, but it is higher than the accuracy of single 5G/INS position, and the positioning error is more stable than 5G/INS, the positioning convergence speed is faster, and the positioning continuity is more guaranteed. According to the DOP value analysis of GNSS and LEO constellations in Figure 10, it can be seen that the geometric distribution of the LEO constellation is significantly improved compared with that of the GNSS constellation. The results of velocity error and attitude error of the Open scene can be seen from Figure A1 and Figure A2 in Appendix A. The error values after using the federated filtering algorithm in this article both are relatively stable.

3.1.2. Semi-Occluded Scene

The positioning result after setting the azimuth angle and mask angle of the semi-occlusion scene in Section 2.2 is shown in the figure below.

It can be seen from Figure 11 that in the Semi-occluded scene, because the azimuth range is reduced by half compared with the Open scene, the positioning accuracy of LEO/INS has obvious influence, and the position error in the north direction is significantly larger than that of GNSS/INS. It can be seen from Figure 12 that the geometric configuration of the LEO constellation in the Semi-occluded scene is obviously worse than that of GNSS in three-dimensional position and horizontal position, which further affects the positioning accuracy of LEO. Due to the influence of high orbit height, the positioning accuracy in the semi-occluded scene has little influence on the GNSS constellation. Due to the better fault tolerance of the federated filtering algorithm and the visible feature of the 5G base station, the 5G ranging data used in this paper is not occluded, so 5G/INS positioning is used as a supplement, which makes the positioning accuracy of the whole federated filtering about 2 m. The results of velocity error and attitude error of the Semi-occluded scene can be seen from Figure A3 and Figure A4 in Appendix A. The fluctuation of velocity error and attitude error of the Semi-occluded is larger than the value in the Open scene.

3.1.3. Surrounding Occluded Scene

The positioning results are shown in the figure below after setting the azimuth and mask angle of the Surrounding occluded scene in Section 2.2.

As can be seen from Figure 13, under the influence of the increase of mask angle, the LEO/INS positioning accuracy becomes worse, but the accuracy after GNSS/INS filtering is improved. The overall accuracy after federated filtering is about 2 m. As can be seen in Figure 14, in the Semi-occlusion scenario, the geometric configuration of the LEO constellation is greatly limited due to the high mask angle limit. The results of velocity error and attitude error of the Surrounding occluded scene can be seen from Figure A5 and Figure A6 in Appendix A. The error value of the sub-filter will be reduced after using federated filtering.

3.1.4. Headspace Occluded Scene

The positioning result after setting the azimuth angle and mask angle of the headspace occluded scene in Section 2.2 is shown in the figure below.

As can be seen from Figure 15, under the Headspace occluded scenario, the GNSS constellation is limited by the range of mask angle, so the GNSS/INS positioning error increases by one order of magnitude in the celestial direction. The LEO constellation geometry has an obvious advantage in this scene, so the positioning error is not affected. Due to the geometric configuration advantage of the LEO constellation, the positioning accuracy of the federated filter is higher than that of GNSS/INS in this scene. As can be seen from Figure 16, the DOP value of the GNSS constellation in the Headspace occlusion scenario is significantly higher than that of the LEO constellation. The results of velocity error of the Headspace occluded scene can be seen from Figure A7 in Appendix A. The velocity error of INS/GNSS positioning has obvious fluctuation, and the fluctuation of attitude error has exceeded the acceptable range. The attitude angle error increases by one order of magnitude which can be seen from Figure A8 in Appendix A.

It can be seen from the positioning results in Section 3.1, the positioning errors and constellation DOP values are quite different in different scenarios. The positioning results of the federated filtering algorithm in FNR mode are compared in the following table.

In

Table 6. Positioning results.

Category		Positioning Results
		Open Scene		Semi-Occluded Scene		Surrounding Occluded Scene		Headspace Occluded Scene
Position error RMS (m)	North	2.1763	mean (m) 2.1057	2.5278	mean (m) 2.2964	2.5424	mean (m) 2.4211	2.3332	mean (m) 2.8475
	East	2.8946		2.9576		2.4884		1.9913
	Down	1.2462		1.4039		2.2325		4.2179
Velocity error RMS (m/s)	North	0.4228	mean (m/s) 0.3541	0.5425	mean (m/s) 0.3697	0.5485	mean (m/s) 0.4284	0.6874	mean (m/s) 1.0380
	East	0.3724		0.2947		0.3506		0.5531
	Down	0.2670		0.2720		0.3862		1.8735
Attitude error RMS (deg)	North	1.7171	mean (deg) 2.2723	1.0962	mean (deg) 2.2385	1.6801	mean (m) 3.3344	22.6026	mean (deg) 34.0473
	East	2.1207		1.0798		3.2356		37.8043
	Down	2.9790		4.5395		5.0875		41.7349
GNSS	PDOP	0.7943		0.7919		0.7943		1.1255
	HDOP	0.6432		0.6376		0.6432		2.0309
	VDOP	1.0798		1.0731		1.0798		4.4642
LEO	PDOP	0.6947		4.0836		1.8337		0.1811
	HDOP	0.6319		1.7258		4.0196		0.8646
	VDOP	0.9153		1.7811		4.5272		1.8796

, this subsection compares the positioning results of four scenes. Experiments show that GNSS/INS, LEO/INS, and 5G/INS alone have poor positioning accuracy in a certain scene. However, by using the federated filtering algorithm, the positioning accuracy can be stabilized. At the same time, by comparing the positioning results in different scenarios, it can be found that the convergence effect of the federated filtering positioning results after the introduction of LEO constellation and 5G signals is more stable and faster than GNSS/INS. Plus, the positioning continuity is better guaranteed. In particular, the LEO/INS positioning effect is better in the Headspace occlusion scene.

3.2. Comparison of Different Information Factor Allocation and Location Results of Different Modes for Federated Filtering

After comparing the positioning accuracy of integrated navigation in different scenes according to the different performance of positioning accuracy of GNSS, LEO, and 5G signals, we conclude that different sensors can be set with different information factors. So, the next experimental comparison can be made to verify the influence of information factor allocation of federated filtering in the same mode on positioning accuracy. Therefore, in the Open scene, the experimental results of different information factors under the FZR mode of federated filtering are compared, as shown in the following figures.

In Figure 17, the position error of different information factors is shown. The velocity error and attitude error can be seen from Figure A9 and Figure A10 in Appendix A. The values of different information factors can be seen in the following table.

As shown in

Table 7. GNSS/INS, LEO/INS, and 5G/INS sub-filter information factor ratio.

Category	The Percentage (%)
	GNSS	LEO	5G
Case1	33	34	33
Case2	80	10	10
Case3	10	80	10
Case4	10	10	80

, if a sub-filter positioning accuracy is higher than several orders of magnitude than another sub-filter, the information factor ratio of this sub-filter needs to be close to 1. The above conclusion can be seen from the compared simulation results. In this way, the positioning results of federated filter can be significantly improved. It can be seen from Figure 17, since the LEO/INS positioning accuracy is higher than GNSS/INS and 5G/INS, Case3 can improve the overall positioning accuracy by increasing the proportion of the LEO/INS information factor to 80%.

At the same time, this paper also compares the federated filtering positioning accuracy of different modes under the same scale factor (using the percentage in Case1), as shown in the figure below.

From the comparison of Figure 18, Figure 19 and Figure 20, it is found that the federated filter under different modes has no obvious difference in position error. However, the FFR mode performs poorly in speed error and attitude angle error, which is reflected in the large error fluctuations, and the attitude angle error reaches 10° in magnitude. After analysis, it is considered that the filter in this paper adopts the error state vector model, and different modes have different ways to deal with the covariance matrix. The covariance matrix of FFR mode feedback leads to the instability of the sub-filter, which leads to the speed error increasing to the level of 1 m/s after 100 s and the attitude angle error increasing to the level of 10° after 200 s. FNR mode does not affect the sub-filter, so the positioning result has no influence. The velocity error in FNR mode is less than 0.5 m/s, and the attitude angle error is less than 2.5°. The FZR mode resets the covariance matrix of the sub-filter, so the error of velocity and attitude angle does not change much. The velocity error is less than 0.3 m/s and the attitude angle error is less than 1.2° in the FZR mode.

In this subsection, the positioning results under different federated filtering modes are compared, and it is found in the experiment that FZR mode and FNR mode have less influence on the sub-filter covariance matrix than FFR mode, and the positioning accuracy is better.

In Section 3, by comparing positioning results in different scenarios, positioning results in different information factors, and positioning results in different federated filtering modes, it is found that higher information factors should be set for sensors with higher accuracy according to different application scenarios. If the error state vector modeling is adopted, the FNR and FZR mode positioning results of federated filtering are better.

4. Conclusions

In this work, INS, GNSS, LEO, and 5G signal sources were simulated, and integrated navigation simulation experiments were carried out using tightly coupled and federated filtering algorithms. By setting the azimuth angle and the satellite visible altitude angle, the positioning results in four occlusion scenes, namely, Open scene, Semi-occluded scene, Surrounding occluded scene, and Headspace occluded scene, are compared. The main conclusions that the paper can support are as follows: (1) the experimental results show that, after adding the LEO constellation, the geometric configuration of the LEO constellation can significantly improve the accuracy factor, which provides strong support for the positioning effect of the Headspace occlusion scene, and improve the positioning accuracy of the original INS/GNSS by an order of magnitude; (2) after the addition of the 5G ranging signal, due to the uninterrupted characteristics of the 5G ranging signal, the overall positioning continuity of federated filtering was greatly improved; (3) by allocating different scale factors, the experiments show that the federated filtering algorithm can combine sensors with different precision for navigation and positioning, to adapt to the integrated navigation modes in different scenes, and open up a new idea for new sensor integrated navigation.

The future improvement of this paper lies in that this paper only tests the single point positioning mode with new LEO and 5G sensors based on INS/GNSS. In the future, we can study higher-precision positioning algorithms, including but not limited to RTK positioning and PPP positioning. The data used in LEO navigation and positioning in this paper is the satellite position and velocity obtained by simulating the satellite constellation, and there is no measured data source for verification. The simulation of 5G ranging signal only adopts pseudo-range measurement simulation with noise added based on theoretical value. There is no contrast experiment with the way of positioning by specifying positioning protocol in communication standard. The difference of positioning accuracy between the two ways has not been specified yet.