## 4 Establishing the Data Fusion Kalman Filter Equations
### Measurement Equation  
In the n-frame of \(SRINS_{1}\), the positional relationship between the two sets of SRINSs can be expressed as follows:  
\[\left[ \begin{array}{c}L_{p_{i}}^{n}\\ \lambda_{p_{i}}^{n}\\ h_{p_{i}}^{n}\end{array} \right]=\left[ \begin{array}{c}L_{p_{1}}^{n}\\ \lambda_{p_{1}}^{n}\\ h_{p_{1}}^{n}\end{array} \right]+\left[ \begin{array}{ccc}0&\frac{1}{R_{M}+ h_{p_{1}}^{n}}&0\\ \frac{\sec L_{p_{1}}^{n}}{R_{N}+h_{p_{1}}}&0&0\\ 0&0&1\end{array} \right]\mathbf{C}_{p_{1}}^{n}\cdot\mathbf{r}_{i}^{p_{1}} \tag{28}\]  
In the formula, \(\mathbf{p}_{p_{1}}^{n}=\left[ \begin{array}{cc}L_{p_{1}}^{n}&\lambda_{p_{1}}^{n}& h_{p_{1}}^{n}\end{array} \right]^{T}\) and \(\mathbf{p}_{p_{i}}^{n}=\left[ \begin{array}{cc}L_{p_{i}}^{n}&\lambda_{p_{i}}^{n}& h_{p_{i}}^{n}\end{array} \right]^{T}\) represent the positions of the two sets of SRINSs in the navigation coordinate system of \(SRINS_{1}\), respectively. \(\mathbf{r}_{i}^{p_{1}}=\left[ \begin{array}{cc}r_{xi}^{p_{1}}&r_{yi}^{p_{1}}&r_{ zi}^{p_{1}}\end{array} \right]\) represents the lever arm between these two sets of SRINSs in the p-frame of \(SRINS_{1}\). Therefore, the measurement matrix can be established as:  
\[\mathbf{Z}=\left[ \begin{array}{c}\mathbf{Z}_{2}\\ \mathbf{Z}_{3}\end{array} \right] \tag{29}\]  
\[\mathbf{Z}_{i}=\mathbf{p}_{p_{1}}^{n}+\left[ \begin{array}{ccc}0&\frac{1}{R_{M}+h_{p _{1}}^{n}}&0\\ \frac{\sec L_{p_{1}}^{n}}{R_{N}+h_{p_{1}}^{n}}&0&0\\ 0&0&1\end{array} \right]\mathbf{C}_{p_{1}}^{n}\cdot\mathbf{r}_{i}^{p_{1}}-\mathbf{p}_{p_{i}} ^{n} \tag{30}\]  
Due to the presence of measurement and calculation errors, the actual measurement equation can be obtained by adding error terms to equation above:  
\[\mathbf{Z}_{i}=\mathbf{\tilde{p}}_{p_{1}}^{n}+\left[ \begin{array}{ccc}0&\frac{1}{R _{M}+h_{p_{1}}^{n}}&0\\ \frac{\sec L_{p_{1}}^{n}}{R_{N}+h_{p_{1}}^{n}}&0&0\\ 0&0&1\end{array} \right]\mathbf{\tilde{C}}_{p_{1}}^{n}\cdot\mathbf{\tilde{r}}_{i}^{p_{1 }}-\mathbf{\tilde{p}}_{p_{i}}^{n} \tag{31}\]  
In this formula above, \(i=2,3\) and:  
\[ \begin{array}{l}\mathbf{\tilde{p}}_{p_{1}}^{n}=\mathbf{p}_{p_{1}}^{n}+\delta\mathbf{p}_{ p_{1}}^{n}\\ \mathbf{\tilde{p}}_{p_{i}}^{n}=\mathbf{p}_{p_{1}}^{n}+\delta\mathbf{p}_{p_{1}}^{n}\\ \mathbf{\tilde{v}}_{i}^{n}=\mathbf{r}_{i}^{n}+\delta\mathbf{r}_{i}^{n}\\ \mathbf{\tilde{C}}_{p_{1}}^{n}=(\mathbf{1}+\mathbf{\phi}\times)\mathbf{C}_{p_{1}}^{n^{\prime}} \\ \delta\mathbf{p}_{p_{1}}^{n}=\left[\begin{array}{ccc}\delta L_{p_{1}}^{n}&\delta \lambda_{p_{1}}^{n}&\delta h_{p_{1}}^{n}\end{array}\right]^{T}\\ \delta\mathbf{p}_{p_{i}}^{n}=\left[\begin{array}{ccc}\delta L_{p_{1}}^{n}&\delta \lambda_{p_{1}}^{n}&\delta h_{p_{1}}^{n}\end{array}\right]^{T}\\ \delta\mathbf{r}_{i}^{p_{1}}=\left[\begin{array}{ccc}\delta r_{xi}^{p_{1}}&\delta r _{yi}^{p_{1}}&\delta r_{zi}^{p_{1}}\end{array}\right]^{T}\\ \delta\mathbf{r}_{p_{1}}^{n}=h_{p_{1}}^{n}+\delta h_{p_{1}}^{n}\\ \mathbf{\tilde{L}}_{p_{1}}^{n}=L_{p_{1}}^{n}+\delta L_{p_{1}}^{n}\end{array} \]\(\tilde{\mathbf{p}}_{p_{1}}^{n}\) denotes the position calculated by \(SRINS_{1}\), \(\delta{\mathbf{p}}_{p_{1}}^{n}\) denotes the position error of \(SRINS_{1}\), \(\tilde{\mathbf{p}}_{p_{1}}^{n}\) denotes the position calculated by \(SRINS_{i}\), \(\delta{\mathbf{p}}_{p_{1}}^{n}\) denotes the position error of \(SRINS_{i}\), \(\tilde{\mathbf{r}}_{i}^{p_{1}}\) denotes the estimated arm length between \(SRINS_{1}\) and \(SRINS_{i}\), \(\delta{\mathbf{r}}_{i}^{p_{1}}\) denotes the error in the arm length between \(SRINS_{1}\) and \(SRINS_{i}\), \({\mathbf{c}}_{p_{1}}^{n^{\prime}}\) denotes the attitude matrix from the p-frame to the n-frame calculated by \(SRINS_{1}\), and \(\phi\) denotes the misalignment angle error of \(SRINS_{1}\). By neglecting second-order small quantities, the measurement equation can be expressed as:  
\[