*2.3. IMU/GNSS Integration*

In this section, an Extended Kalman Filter (EKF), based on linearization of nonlinear models, is used as the data fusion algorithm [30]. The state vector for the EKF is:

$$\mathbf{X} = \begin{bmatrix} \left(\delta \mathbf{r}\_{\mathrm{INS}}^{\varepsilon}\right)^{T} & \left(\delta \mathbf{r}\_{\mathrm{INS}}^{\varepsilon}\right)^{T} & \left(\mathbf{\phi}\_{\mathrm{INS}}^{\varepsilon}\right)^{T} & \mathbf{b}\_{\mathrm{g}}^{\mathrm{T}} & \mathbf{b}\_{\mathrm{a}}^{\mathrm{T}} & \mathbf{s}\_{\mathrm{g}}^{\mathrm{T}} & \mathbf{t}\_{\mathrm{GPS}} & \delta \mathbf{t}\_{\mathrm{GPS}} & \mathbf{t}\_{\mathrm{BUS}} & \delta \mathbf{t}\_{\mathrm{BUS}} \end{bmatrix} \tag{12}$$

where, *δr<sup>e</sup> INS*, *<sup>δ</sup>v<sup>e</sup> INS*, and *<sup>φ</sup><sup>e</sup> INS* are the three-axis error vectors of IMU position, velocity, altitude in the ECEF framework *e*; *bg*, *ba*, *sg*, and *s<sup>a</sup>* are the three-axis acceleration and gyroscope bias and scale factor error; *tGPS* and *δtGPS* are the receiver clock error and drift rate of GPS satellite; *tBDS* and *δtBDS* are the clock error and drift rate of Beidou satellite. The system model is then formed as a first-order state equation in (13):

$$
\dot{\mathbf{X}} = \mathbf{F}\mathbf{X} + \mathbf{G}\mathbf{W} \tag{13}
$$

where . *X* is the first derivative of *X*. *F* is the dynamic transition matrix, *G* is the noise driven matrix, and *W* is the system noise. The measurement model is given by:

$$\mathbf{Z} = \mathbf{H}\mathbf{X} + \mathbf{V} \tag{14}$$

where *Z* is the measurements vector, *H* is the measurement mapping matrix, and *V* represents the measurement noise. In this paper, if the number of visible satellites is *n*, the

pseudorange error and the Doppler measurement error are used to form measurement vector *Z* as: ⎡ ⎤

$$\mathbf{Z} = \begin{bmatrix} \rho\_{1,GDS}^{IMS} - \rho\_1^{GPS} \\ \vdots \\ \rho\_{1,GPS}^{IAM} - \rho\_1^{GPS} \\ \rho\_{1,BDS}^{IMI} - \rho\_1^{BPS} \\ \vdots \\ \rho\_{m,BDS}^{IMIS} - \rho\_m^{BPS} \\ \vdots \\ \rho\_{1,GPS}^{IMIS} - f\_1^{GPS} \\ \vdots \\ f\_{1,GPS}^{IMIS} - f\_1^{GPS} \\ f\_{1,BDS}^{IMIS} - f\_1^{BPS} \\ \vdots \\ f\_{m,BDS}^{IMIS} - f\_m^{BPS} \end{bmatrix} \tag{15}$$

where *ρIMU GNSS* and *<sup>f</sup> IMU GNSS* denote IMU-derived GNSS pseudorange and Doppler measurements respectively. Based on the derivations in [30], *ρGNSS* and *f GNSS* refer to pseudorange and Doppler measurements decoded from GNSS observation data, respectively. *l* and *m* refer to the number of GPS and BDS visible satellites. After discretization of (13) and (14), the discrete form of the Kalman filtering procedure can be split into two stages, as follows:

Prediction stage:

$$
\hat{\mathbf{X}}\_{k,k-1} = \Phi\_{k,k-1} \hat{\mathbf{X}}\_{k-1} \tag{16}
$$

$$P\_{k,k-1} = \Phi\_{k,k-1} P\_{k-1} \Phi\_{k,k-1}^T + Q\_{k-1} \tag{17}$$

Update stage:

$$\mathbf{K}\_{k} = \mathbf{P}\_{k,k-1} \mathbf{H}\_{k}^{\top} \left( \mathbf{H}\_{k} \mathbf{P}\_{k,k-1} \mathbf{H}\_{k}^{\top} + \mathbf{R}\_{k} \right)^{-1} \tag{18}$$

$$\mathbf{\hat{X}}\_{k} = \mathbf{\hat{X}}\_{k,k-1} + \mathbf{K}\_{k}(\mathbf{Z}\_{k} - \mathbf{H}\_{k}\mathbf{\hat{X}}\_{k,k-1}) \tag{19}$$

$$P\_k = (I - \mathcal{K}\_k H\_k) P\_{k,k-1} \left( I - \mathcal{K}\_k H\_k \right)^T + \mathcal{K}\_k \mathcal{R}\_k \mathcal{K}\_k^{\top} \tag{20}$$

where,*X*ˆ *<sup>k</sup>* is the system state vector estimates at time epoch *k*; **Φ***<sup>k</sup>* is the system transition matrix at time epoch *k*; *P<sup>k</sup>* is the error covariance matrix at time epoch *k*; *Q<sup>k</sup>* is the system noise covariance matrix at time epoch *k*; *R<sup>k</sup>* is the measurement noise covariance matrix at time epoch *k*; *H<sup>k</sup>* is the measurement matrix at time epoch *k*; *K<sup>k</sup>* is the Kalman gain matrix at time epoch *k*; Θ*k*,*k*−<sup>1</sup> is the matrix/vector Θ propagation from time epoch *k* − 1 to *k*.Table <sup>1</sup> has illustrated the parameters and their value or initial value used for the EKF. The setting of the system noise covariance matrix *Q* is based on experience. The *diag* means that the matrix is a diagonal matrix and the values in the bracket are the diagonal elements. The initial value of error covariance matrix of the state vector *P*, noted as *P*0, is calculated by the historical data collected from the IMU and GNSS receiver. The covariance matrix of the measurement noise *R* is set based on the statistical data collected from GNSS receiver.

**Table 1.** The parameters used for the EKF.


If positions calculated by all subsets do not pass the range detection test or the number of satellites cannot meet the condition of the w-test, then all the satellite measurements and the inertial navigation output information are fused through the robust algorithm. The robust algorithm introduces a fault detection factor *D* to scale *R*. *D* is given as:

$$D\_{ii} = \begin{cases} 1, & \left| \stackrel{\curvearrowleft}{\mathfrak{R}\_{k,i}} \right| \le Tm \\ \frac{\left| \stackrel{\curvearrowleft}{\mathfrak{R}\_{k,i}} \right|}{Tm}, & \left| \stackrel{\curvearrowleft}{\mathfrak{R}\_{k,i}} \right| > Tm \end{cases} \tag{21}$$

$$
\widetilde{\mathfrak{R}}\_{k,i} = \frac{\mathfrak{R}\_{k,i}}{\sqrt{\mathbb{C}\_{k,ii}}} \tag{22}
$$

*<sup>k</sup>* <sup>=</sup> *<sup>Z</sup><sup>k</sup>* <sup>−</sup> *<sup>H</sup>kX*<sup>ˆ</sup> *<sup>k</sup>*,*k*−<sup>1</sup> is the innovation sequence, which exhibits a white Gaussian sequence of mean zero and covariance <sup>C</sup>*<sup>k</sup>* where <sup>C</sup>*<sup>k</sup>* <sup>=</sup> *<sup>H</sup>kPk*,*k*−1*H<sup>k</sup> <sup>T</sup>* + *Rk*. *Tm* is a constant value, which is valued according to the specific scenario. Then, the elements in *R* are given as:

$$\mathcal{R}\_{k,ii} = D\_{ii} \cdot \mathcal{R}\_{k,ii} \tag{23}$$
