## 2 The Model for GNSS/INS/LEO/5G Integrated Navigation
### The Structure of Federated Filtering Algorithm
#### 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:  
1. Attitude update: \[\begin{split} 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{split}\] (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: \[C_{b+}^{b-}=I_{3}+\frac{\sin\left|\omega_{ib}^{b}\right|}{\left|\omega_{ib}^ {b}\right|}\left[a_{ib}^{b}\wedge\right]+\frac{1-\cos\left|a_{ib}^{b}\right| }{\left|a_{ib}^{b}\right|^{2}}\left[\alpha_{ib}^{b}\wedge\right]^{2}\] (2) where \(a_{ib}^{b}\) is the rotation vector of the body coordinate system relative to the inertial coordinate system and project in the body coordinate system. \(a_{ib}^{b}\wedge\) is the antisymmetric matrix of the rotation vector \(a_{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{split} C_{b}^{i}=C_{b}^{i}(-)C_{b}^{b-},C_{b}^{b-}=I_{3}+ \frac{1-\cos\left|a_{ib}^{b}\right|}{\left|a_{ib}^{b}\right|^{2}}\left[a_{ib}^ {b}\wedge\right]+\frac{1}{\left|a_{ib}^{b}\right|^{2}}\left(1-\frac{\sin \left|a_{ib}^{b}\right|}{\left|a_{ib}^{b}\right|}\right)\left[a_{ib}^{b} \wedge\right]^{2}\\ \end{split}\] (3) \[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}\] (4)  
Figure 1: Structure diagram of federated filtering model.  
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}{l}v_{eb}^{e}(+)\approx v_{eb}^{e}(-)+\left(f_{ib}^{e}+g_{b}^{e}( r_{eb}^{e}(-))-2\Omega_{ie}^{e}v_{eb}^{e}(-)\right)\tau_{i}\\ =v_{eb}^{e}(-)+v_{ib}^{e}+\left(g_{b}^{e}(r_{eb}^{e}(-))-2\Omega_{ie}^{e}v_{eb} (-)\right)\tau_{i}\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}{l}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} \tag{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.