# Integrated GNSS/IMU-Gyrocompass with Rotating IMU. Development and Test Results
## 4 Methods
### Filtering Algorithms Used for GNSS/IMU Gyrocompass Measurements
When the satellite number was changed in the receiver channel, in EKF covariance equations the error variance of phase ambiguity \(\delta Cf^{i}\) relevant for this channel was restored to the initial value, and the previous estimate was zeroed. During the processingof phase measurements from each visible satellite, measurements with enhanced errors were revealed according to the criterion generated using the EKF covariance:  
\[\sigma_{z\Delta\phi i(k+1)}=\sqrt{diag\big{(}H_{k+1}P_{k/k+1}H_{k+1}^{T}+R_{k+1} \big{)}}, \tag{17}\]  
where \(\sigma_{z\Delta\phi i(k+1)}\) are the calculated RMS errors of measurements \(\overline{z}_{\Delta\phi i}\); \(P_{k/k+1}\) is the predicted EKF covariance matrix; \(H_{k+1}\) is the measurement matrix at step k+1; \(R_{k+1}\) is the measurement noise covariance matrix; \(k_{d}=4...6\). These relations are given for the composite vector (8) with account for \(x_{GNSS}=[\delta\rho^{j}\delta vj,\delta Cf]^{T}\) and for measurements (10)-(12). If \(\left|\overline{z}_{\Delta\phi i}\right|>k_{d}\sigma_{z\Delta\phi i(k+1)}\), GNSS receiver measurements for the i-th satellite were not used. Additionally, to reduce the effect of perturbed phase measurements on the system errors, for each satellite the EKF was restarted using the relevant estimate \(\delta\hat{Cf}^{i}\). The following criterion was used:  
\[\left|L_{kfi}\right|<L_{kfi}^{dop}, \tag{18}\]  
where \(L_{kfi}\) is the value of residuals \(L_{kfi}=\overline{z}_{\Delta\phi i}\left|\overline{b}\right|\) of differential phase measurements smoothed by the low-pass filter as a first-order with a time constant equal to the period of antenna module modulation rotation; \(L_{kfi}^{dop}\) is the acceptable value of smoothed residuals. With \(\left|L_{kfi}\right|\geq L_{kfi}^{dop}\) the current measurement \(\overline{z}_{\Delta\phi i(k+1)}\) was not used, and the Kalman filter was restarted. In case of lost or ineffective satellite signals the relevant signal is generated in the switch \"GNSS is available\" (Figure 5). Then INS is corrected by additional external measurements of linear velocity and altitude (depth) \(h^{ext}\), and filtering is performed for the state vector including only INS errors (\(x_{INS}\)) and components of inertial sensors' error models (\(x_{IMU}\)). With no GNSS signals, the system state vector is given by  
\[ \begin{array}{l}x=\left[\begin{array}{c}x_{INS}\\ x_{IMU}\end{array}\right],\\ x_{INS}=\left[\begin{array}{cc}\alpha&\beta&\gamma&\delta\mathbf{V_{n}^{e}}^{T }&\delta\mathbf{p}^{T}\end{array}\right]^{T},\\ x_{IMUI}=\left[\begin{array}{cc}\delta\mathbf{f_{0}}^{T}&\delta\omega_{0} ^{T}&\delta S_{gZ_{s}}&A_{X_{s}}&B_{X_{s}}&A_{Y_{s}}&B_{Y_{s}}\end{array} \right]^{T}.\end{array}  \tag{19}\]  
External data can be generated using absolute or relative velocity sensors, and barometric altitude (depth) sensor. Note that rotation of inertial sensors and applying tactical grade FOGs allows heading determination with the acceptable accuracy for a long time after satellite signal loss. However it should be noted that the previous period of fault-free satellite signal reception needs to be rather long to have enough time to estimate all the components of inertial sensor error models. As seen further from the experimental results, this period can reach 1500 s. In this mode, filtering is realized using differential measurements \(z_{V_{E}},z_{V_{N}},z_{h}\) generated in unit \(\Sigma 2\) (Figure 4) as a difference between the estimated \(V_{N}^{INS},V_{E}^{INS},h^{INS}\) and measured \(V_{N}^{ext},V_{E}^{ext}.h^{ext}\) values:  
\[ \begin{array}{l}z_{V_{E}}=V_{E}^{INS}-V_{E}^{ext},\\ z_{V_{N}}=V_{N}^{INS}-V_{N}^{ext},\\ z_{h}=h^{INS}-h^{ext}.\end{array}  \tag{20}\]  
If we use the sensors of object velocity relative to the environment (water or air) \(V_{L}\) projected onto the longitudinal axis, velocity measurements can be rewritten as follows:  
\[