# An Algorithm to Assist the Robust Filter for Tightly Coupled RTK/INS Navigation System
## 2 Materials and Methods
### The Prediction Step of the TC RTK/INS Integration  
#### 2.2.1 The State Vector  
The state vector can be divided into states related to INS and states related to GNSS. When it comes to INS, we choose the error states of INS to form TC integration's state vector. The definition of the error states is vital as the feedback procedure and the fixing procedure depend on it. The INS states include the attitude represented in the rotation matrix \(\mathbf{C}_{\text{b}}^{\text{h}}\), the velocity in the navigation frame \(\mathbf{v}^{\text{n}}\), the position in the geographic coordinate system \(\mathbf{p}\), the gyroscope bias in the body frame \(\mathbf{e}^{\text{b}}\), and the accelerometer bias in the body frame \(\mathbf{\
abla}^{\text{b}}\). The body frame is defined as \"Right-Front-Up,\" whereas the navigation frame is defined as \"East-North-Up.\" The error states' definition in our implementation is the same as that in PSINS, which is shown in Equation (5):  
Figure 1: SD measurements and DD measurements. (**a**) Two receivers track the signals of two satellites. (**b**) Steps to calculate SD measurements and DD measurements.  
\[\begin{split} C_{\text{n}^{\prime}}^{\text{n}}&=C_{\text{b}}^ {\text{b}}C_{\text{n}^{\prime}}^{\text{b}},\\ \delta\text{v}^{\text{n}}&=v_{\text{est}}^{\text{n}}-v_ {\text{true}}^{\text{n}},\\ \delta\text{\mathbf{\mathbf{p}}}&=\text{\mathbf{p}}_{\text{est}}- \text{\mathbf{p}}_{\text{true}},\\ \delta\text{\mathbf{e}}^{\text{b}}&=\text{\mathbf{e}}_{\text {true}}^{\text{b}}-\text{\mathbf{e}}_{\text{est}}^{\text{b}},\\ \delta\text{\mathbf{\
abla}}^{\text{b}}&=\text{\mathbf{\
abla} }_{\text{true}}^{\text{b}}-\text{\mathbf{\
abla}}_{\text{est}}^{\text{b}},\end{split} \tag{5}\]  
where \"est\" is used to describe the estimation, whereas \"true\" describes the true value; n and b denote the navigation frame and the body frame, respectively; n\({}^{\prime}\) denotes a frame corresponding to the attitude estimation; \(\delta(\cdot)\) describes the state error. We generally use \(\mathbf{\varphi}\) to denote the attitude error. The relationship between \(\mathbf{\varphi}\) and \(\mathbf{C}_{\text{n}^{\prime}}^{\text{n}}\) is shown in Equation (6):  
\[\mathbf{C}_{\text{n}^{\prime}}^{\text{n}}\approx\mathbf{I}+\mathbf{\varphi}\times, \tag{6}\]  
where the operator\(\times\) produces the cross-product matrix of a vector.  
When it comes to GNSS, we choose the SD ambiguity to form the TC integration's state vector. Accordingly, the state vector of TC RTK/INS integration is formulated as Equation (7) shows:  
\[\mathbf{x}_{k}=\begin{bmatrix}\mathbf{\varphi}\\ \delta\text{v}^{\text{n}}\\ \delta\text{\mathbf{p}}\\ \delta\text{\mathbf{e}}^{\text{b}}\\ \delta\text{\mathbf{\
abla}}^{\text{b}}\\ \kappa\end{bmatrix}, \tag{7}\]  
where \(\kappa\) refers to the SD ambiguity vector, including multi-GNSS and multi-frequency SD ambiguity. In our implementation, \(\kappa\) contains GPS/BDS single-frequency SD ambiguity. We can obtain the state transition matrix \(\mathbf{F}_{k-1}^{k}\) based on the discretization of the state differential equation, as Equation (8) shows:  
\[\begin{split}\hat{\mathbf{x}}(t)&=\mathbf{A}(t)\mathbf{x}(t)+\mathbf{ \omega},\\ \mathbf{F}_{k-1}^{k}&\approx\mathbf{I}+\mathbf{A}(t)\Delta t.\end{split} \tag{8}\]  
For more details about the INS state error differential equation, readers can refer to [24; 71].