# Robust Kalman Filter Aided GEO/IGSO/GPS Raw-PPP/INS Tight Integration
## 2 Methods
### INS Update  
INS can provide navigation solutions by processing measurements from IMU sensors after giving the initial status. As illustrated in Figure 2, the original IMU outputs are the specific force (\(\mathbf{f}\)) and angular rate (\(\mathbf{\omega}\)) vectors, which contain bias (\(\mathbf{S}\)) and scale factor (\(\mathbf{B}\)) errors  
\[\mathbf{\dot{t}}^{b}=(\mathbf{I}+\mathbf{S}_{t})\mathbf{\dot{t}}^{b}+\mathbf{B}_{t}\Delta t+\mathbf{ \varsigma}_{t} \tag{13}\]  
\[\mathbf{\omega}_{ib}^{b}=(\mathbf{I}+\mathbf{S}_{\omega})\mathbf{\dot{\omega}}_{ib}^{b}+\mathbf{B} _{\omega}\Delta t+\mathbf{\varsigma}_{\omega} \tag{14}\]  
where \(\mathbf{B}\), \(\mathbf{S}\), \(\Delta t\), and \(\widetilde{()}\) are the sensor biases, sensor scale factor, IMU time interval, and theoretical value, respectively; \(i\) and \(b\) denote the inertial and body coordinate frames, respectively. \(\mathbf{\varsigma}\) represents the observing noise. After sensor error correction, motion errors, such as the rotational and sculling on velocity and coning on attitude [25], are also removed by using  
\[\Delta\mathbf{v}_{Rot+Scu}=\frac{\left(\Delta\mathbf{\theta}_{\omega,k}\times\Delta\bm {v}_{t,k}^{b}\right)}{2}+\frac{\left(\Delta\mathbf{\theta}_{\omega,k-1}\times\Delta \mathbf{v}_{t,k}^{b}+\Delta\mathbf{v}_{t,k-1}^{b}\times\Delta\mathbf{\theta}_{\omega,k} \right)}{12} \tag{15}\]  
\[\Delta\mathbf{\theta}_{Con}=\frac{\Delta\mathbf{\theta}_{\omega,k-1}\times\Delta\mathbf{ \theta}_{\omega,k}}{12} \tag{16}\]  
with  
\[\Delta\mathbf{v_{t}}=\int_{k-1}^{k}fdt,\Delta\mathbf{\theta}_{\omega}=\int_{k-1}^{k} \mathbf{\omega}_{ib}^{b}dt \tag{17}\]  
where \(\Delta\mathbf{\theta}_{k}\) and \(\Delta\mathbf{v}_{k}\) are the attitude and velocity increments at epoch \(k\), and \(\times\) refers to the cross-product computation. After corrections from Equation (14) to (17), the INS updated position (Equation (18)), velocity (Equation (19)), and attitude (Equation (20)) in the navigation frame (\(n\)) can be obtained by  
\[\mathbf{p}_{r,INS,k}^{n}=\left(\begin{array}{c}B\\ L\\ H\end{array}\right)=\left(\begin{array}{c}-2\cdot\mathrm{atan}(\mathbf{q}_{n}^{n} [2]/\mathbf{q}_{n}^{e}[0])-0.5\pi\\ 2\cdot\mathrm{atan}(\mathbf{q}_{n}^{e}[3]/\mathbf{q}_{n}^{e}[0])\\ H_{k-1}-\gamma_{D,mid}\cdot\Delta t\end{array}\right) \tag{18}\]  
\[\mathbf{v}_{r,INS,k}^{n}=\mathbf{v}_{r,INS,k-1}^{n}+\int_{k-1}^{k}\left[\mathbf{C}_{b}^{n}f ^{b}-\left(2\mathbf{\omega}_{ie}^{n}+\mathbf{\omega}_{en}^{n}\right)\times\mathbf{v}_{r,INS,k-1}^{n}+g^{n}\right]dt \tag{19}\]  
\[\left(\begin{array}{c}\gamma\\ \varphi\\ \psi\end{array}\right)=\left(\begin{array}{c}\mathrm{atan}\bigg{(}-\mathbf{C}_{ b}^{n}[3,1]/\sqrt{\mathbf{C}_{b}^{n}[3,2]^{2}+\mathbf{C}_{b}^{n}[3,3]^{2}}\bigg{)}\\ \mathrm{atan}2(\mathbf{C}_{b}^{n}[3,2],\mathbf{C}_{b}^{n}[3,3])\\ \pi+\mathrm{atan}2(\mathbf{C}_{b}^{n}[2,3]+\mathbf{C}_{b}^{n}[1,2],\mathbf{C}_{b}^{n}[1,3]+ \mathbf{C}_{b}^{n}[2,2])\end{array}\right) \tag{20}\]  
where \(B\), \(L\), and \(H\) are geodetic latitude, longitude, and height; \(v_{D,mid}\) is the velocity at middle time between epochs \(k\)-\(1\) and \(k\); \(e\) is the earth-fixed-earth-centered coordinate frame; \(\mathbf{\omega}_{AD}^{C}\) refers to the rotation angular rate of \(A\) (e.g., \(i\), \(e\), and \(n\)) frame with respect to \(D\) frame (e.g., \(i\), \(e\), and \(n\)), projected in \(C\) frame (e.g., \(i\), \(e\), and \(n\)); \(\mathbf{g}^{n}\) and \(\bigotimes\) stand for the normal gravity vector at \(\mathbf{p}_{r,k}^{n}\) and the quaternion product; and \(\mathbf{q}_{n}^{e}[k1]\) and \(\mathbf{C}_{b}^{n}[k2,k3]\) denote the \(k1\) element in quaternion \(\mathbf{q}_{n}^{e}\) and the \(k2\)-row \(k3\)-column in the direction cosine matrix \(\mathbf{C}_{b}^{n}\).