## 2 Algorithm Principle
### Observation Equation  
In the ECEF coordinate system, let \(\left(x_{s},y_{s},z_{s}\right)^{T}\) be the satellite position obtained by GNSS ephemeris. The corresponding pseudorange solved by INS is:  
\[\rho_{k}=\sqrt{\left(x_{I}-x_{s}^{k}\right)^{2}+\left(y_{I}-y_{s}^{k}\right)^ {2}+\left(y_{I}-y_{s}^{k}\right)^{2}} \tag{13}\]  
where \(k\) is the \(k\)-th satellite. Assuming that the real coordinate position of the INS position is (\(x\),\(y\),\(z\)), the observation equation of the pseudorange difference between INS and GNSS is [26]:  
\[\delta\rho_{k}=\frac{\partial\rho_{k}}{\partial x}\delta x+\frac{\partial\rho_ {k}}{\partial y}\delta y+\frac{\partial\rho_{k}}{\partial z}\delta z-v_{\rho k} \tag{14}\]  
in the Formula (14), \(v_{\rho k}\) represents the pseudorange measurement white noise of the \(k\)-th satellite \(S_{k}\), and the first three terms of Formula (14) can be expressed as:  
\[\left\{ \begin{array}{l}\frac{\partial\rho_{k}}{\partial x}=\frac{x-x_{s}^{ k}}{\sqrt{\left(x-x_{s}^{k}\right)^{2}+\left(y-y_{s}^{k}\right)^{2}}+\left(y-y_{s}^{k} \right)^{2}}=\frac{x-x_{s}^{k}}{\rho}\\ \frac{\partial\rho_{k}}{\partial y}=\frac{y-x_{s}^{k}}{\sqrt{\left(x-x_{s}^{k} \right)^{2}+\left(y-y_{s}^{k}\right)^{2}+\left(y-y_{s}^{k}\right)^{2}}}=\frac{ y-y_{s}^{k}}{\rho}\\ \frac{\partial\rho_{k}}{\partial z}=\frac{z-x_{s}^{k}}{\sqrt{\left(x-x_{s}^{k} \right)^{2}+\left(y-y_{s}^{k}\right)^{2}}+\left(y-y_{s}^{k}\right)^{2}}=\frac {z-x_{s}^{k}}{\rho}\end{array} \right. \tag{15}\]  
where \(\rho=\sqrt{\left(x-x_{s}^{k}\right)^{2}+\left(y-y_{s}^{k}\right)^{2}+\left(y -y_{s}^{k}\right)^{2}}\). Finally, the pseudorange observation equation can be obtained as:  
\[\mathbf{Z_{\rho}}\mathbf{=}\mathbf{H_{\rho}}\mathbf{X+}\mathbf{V_{\rho}} \tag{16}\]  
where  
\[\mathbf{H_{\rho}}\mathbf{=}[\mathbf{O_{k\times 6}}\mathbf{H_{\rho 1}}\mathbf{ O_{k\times 6}}\mathbf{H_{\rho 2}}] \tag{17}\]  
\[\mathbf{H_{\rho 1}}\mathbf{=}\left[ \begin{array}{ccc}\frac{\partial\rho_{1}}{ \partial x}&\frac{\partial\rho_{1}}{\partial y}&\frac{\partial\rho_{1}}{ \partial z}\\ \frac{\partial\rho_{2}}{\partial x}&\frac{\partial\rho_{2}}{\partial y}&\frac{ \partial\rho_{2}}{\partial z}\\ \vdots&\vdots&\vdots\\ \frac{\partial\rho_{k}}{\partial x}&\frac{\partial\rho_{k}}{\partial y}&\frac{ \partial\rho_{k}}{\partial z}\end{array} \right]\Delta\mathbf{C_{n}^{e}} \tag{18}\]\[\mathbf{H}_{\boldsymbol{\rho}2}=\left[ \begin{array}{cc}-1&0\\ -1&0\\ \vdots&\vdots\\ -1&0\end{array} \right] \tag{19}\]  
where \(\Delta\mathbf{C}_{n}^{e}\) is the coordinate conversion matrix, and its expression is [27]:  
\[\Delta\mathbf{C}_{n}^{e}=\left[ \begin{array}{cc}-(R_{n}+h)\cos\lambda\sin L& -(R_{n}+h)\cos L\sin\lambda&\cos L\cos\lambda\\ -(R_{n}+h)\sin\lambda\sin L&(R_{n}+h)\cos\lambda\cos L&\cos L\sin\lambda\\ [R_{n}(1-e^{2})+h]\cos L&0&\sin L\end{array} \right] \tag{20}\]  
The pseudorange rate difference observation equation of INS and GNSS is [26]:  
\[\delta\dot{\rho}_{k}=\frac{\partial\rho_{k}}{\partial x}\delta\dot{x}+\frac{ \partial\rho_{k}}{\partial y}\delta\dot{y}+\frac{\partial\rho_{k}}{\partial x }\delta\dot{z}-v_{\dot{\rho}_{k}} \tag{21}\]  
We convert the velocity error into the ECEF coordinate system, and we can obtain [28]:  
\[\left[ \begin{array}{c}\dot{\delta x}\\ \dot{\delta y}\\ \dot{\delta z}\end{array} \right]=\mathbf{C_{n}^{e}}\left[ \begin{array}{c} \delta V_{E}\\ \delta V_{N}\\ \delta V_{U}\end{array} \right] \tag{22}\]  
Comparing Equation (16), we can obtain the pseudorange rate measurement equation as follows:  
\[\mathbf{Z}_{\dot{\rho}}\mathbf{=}\mathbf{H}_{\dot{\rho}}\mathbf{X+V}_{\dot{ \rho}} \tag{23}\]  
where  
\[\mathbf{H}_{\dot{\rho}}\mathbf{=}[\mathbf{O}_{\mathbf{k}\times 3}\mathbf{H}_{ \dot{\rho}1}\mathbf{O}_{\mathbf{k}\times 9}\mathbf{H}_{\dot{\rho}2}] \tag{24}\]  
\[\mathbf{H}_{\dot{\rho}1}=\left[