A MIMU/GPS/magnetometer integrated alignment method based on sequential robust estimation

Abstract

For a micro-electro-mechanical system inertial measurement unit (MEMS-IMU), Global Positioning System (GPS) and magnetometer integrated navigation system, the procedure of initial alignment is a necessity. However, both magnetometer and GPS receiver are susceptible to external complex environment. Outliers in the observations of the two devices are inevitable. In order to reduce the influence of the outliers and improve the initial alignment accuracy, a sequential robust Kalman filter (SRKF) algorithm based on the square dimension of the innovation vector is proposed. The proposed SRKF algorithm can reasonably decrease the contribution of outlier affected elements of the observation vector in an element-wise way. Robust factor, used to suppress the influence of outliers, is constructed according to the Mahalanobis distance and the Chi-square distribution significance level. The results of different simulations and field tests all indicate the feasibility and the effectiveness of the proposed method.

Introduction

Due to the fact that MEMS-IMU has the advantages of small size, low cost, low power consumption, high reliability and convenient batch production, it has been widely applied to engineering fields and has become an important developing direction for inertial navigation system (Groves 2013; Noureldin et al. 2013). However, the gyro has a poor precision and is unable to complete the azimuth angle alignment independently, which needs other sensors to assist the initial alignment. In recent years, integrated alignment method has been applied gradually, which includes GPS, magnetometer, odometer, IMU and other sensors (Georgy et al., 2011; Ali and Elsheimy, 2013; Chang et al. 2017). With the development of multi-sensor information fusion techniques, MIMU/GPS/magnetometer integrated navigation system has been widely used in navigation and positioning, which could improve the initial alignment accuracy effectively (Khoder and Jida, 2014). However, magnetometer often causes observation outliers for it is highly susceptible to abnormal interference of unknown magnetic field (Zhou et al. 2016), the influence of GPS receiver signal occlusion (Teng et al. 2011), and the multi-path effect (Azarbad and Mosavi, 2014), which may produce observation outliers and result in large errors in initial alignment.

The Kalman filter is a standard algorithm of integrated navigation system. In linear system, if the process noise and measurement noise are independent Gaussian white noise, then the Kalman filter is the optimal algorithm. However, if sensors were in a complex observation environment, it may cause observation outliers and the observed noise will be displayed as non-Gaussian distribution, then the effect of the Kalman filter will decline rapidly.

In order to suppress the influence of outliers, Yang et al., 2001; Huang et al., 2011 proposed an adaptive robust Kalman filter algorithm based on innovation vectors, which overcomes the influence of dynamic model errors and random errors in dynamic navigation and positioning. Xu (2005) proposed a robust algorithm with symbol constraints to deal with the robustness against 50% data pollution and obtain a higher estimation efficiency. Bhatti and Feng (2012) proposed a rate detection algorithm to cope with the slowly growth errors in the GPS/INS integrated system. In recent years, some scholars have proposed to take the Mahalanobis distance (Mahalanobis, 1936), indicating the distance between observed and predicted values, as the criterion of error judgment, and determined the outliers by hypothesis testing method. Chang (2014) proposed a robust Kalman filter method based on the Mahalanobis distances and Chi-squared test to detect outliers in a batch method. When only part of the measurements show anomalies, a sequential updating strategy is promising.

A sequential updating is a method of reducing the updating of high-dimensional measurements to multiple low-dimensional measurements. Oh (2010) combined the sequential updating method with an Unscented Kalman filter (UKF) in the low-cost multi-sensor unmanned aerial vehicle (UAV) system, which solves the problem of updating the rate of different measurements of multiple sensors and reducing the matrix inversion calculation effectively. Bai and Li (2012) applied the innovation-based sequential adaptive Kalman filter to the GPS/INS integrated navigation system to estimate the covariance matrix of measurement noise online. Lee and Choi (2017) applied the quaternion-based sequential Kalman filter to the navigation attitude reference system, which could effectively detect the effects of extra acceleration and magnetic interference and improve the accuracy of the navigation attitude reference system.

In order to suppress the observation outliers during the initial alignment, this paper constructs a MIMU/GPS/Magnetometer integrated navigation model that takes the squared Mahalanobis distance as the indicator. This method monitor the outliers by using the anomaly detection algorithm of Chi-square distribution. Moreover, it constructs the innovation vector correction factor to realize the sequential updating process of the Kalman filtering measurements, and finally establishes the sequential robust Kalman filter method. The proposed method is applied to the procedure of MIMU/GPS/magnetometer initial alignment, the results of simulations and vehicle experiments have indicated the validity and robustness of the proposed method.

The remainder of this paper is organized as follows: in section “Basic knowledge”, the MIMU/GPS/magnetometer integrated alignment model is presented. In section “Methods”, the sequential robust Kalman filter method is derived. Simulations and field tests are illustrated and analyzed in section “Results and Discussions”. Finally, conclusions are summarized in section “Conclusions”.

Basic knowledge

MIMU/GPS/magnetometer integrated alignment model

Integrated alignment state space model is mainly described by IMU error model and IMU sensor system errors, the integrated alignment state space model takes the attitude errors, velocity errors, position errors, gyro drifts, accelerometer bias and lever-arm errors as the 18 dimensional state vectors of the system state model (Farrell, 2008):

$$ X={\left[\phi \kern0.5em \delta {v}^n\kern0.5em \delta {p}^n\kern0.5em {\varepsilon}^b\kern0.5em {\nabla}^b\kern0.75em \delta {l}^b\right]}^T $$

(1)

The navigation coordinate system is based on the east-north-up (ENU) coordinates, and the error equations of the inertial navigation system are shown as follows:

$$ {\displaystyle \begin{array}{l}\dot{\phi}=\phi \times {\omega}_{in}^n+{\delta \omega}_{in}^n-{C}_b^n{\delta \omega}_{ib}^b\\ {}\delta {\dot{v}}^n={C}_b^n\delta {f}^b\times \phi +{v}^n\times \left(2{\delta \omega}_{ie}^n+{\delta \omega}_{en}^n\right)\\ {}\operatorname{}-\left(2{\omega}_{ie}^n+{\omega}_{en}^n\right)\times \delta {v}^n+{C}_b^n\delta {f}^b+\delta {g}^n\\ {}\delta {\dot{p}}^n=\delta {v}^n\end{array}} $$

(2)

Whereδp, δv, ϕ represent the position error, velocity error and attitude error respectively. Superscript subscript i, n, e, b represent the inertial system, navigation system, earth system and carrier system respectively. ${\omega}_{ib}^b$ represents the gyroscope carrier system relative to the angular velocity of inertial system, f^b is the measurement of accelerometer, ${C}_b^n$ represents the direction cosine matrix of the carrier system relative to the navigation system, and δgⁿ represents the local gravity error. ${\omega}_{in}^n$ means the navigation system relative to the rotation angular speed under the inertial system in the navigation system, and ${\omega}_{en}^n$ represents the navigation system relative to the rotational angular velocity of the Earth system under the navigation system. ${\delta \omega}_{ib}^b$ and $\delta {f}_{sf}^b$ are the drift errors of the gyro and accelerometer, which are similar to the first-order Markov processes:

$$ {\displaystyle \begin{array}{l}{\dot{\varepsilon}}^b=-{\beta}_g{\varepsilon}^b+{w}_g\\ {}{\dot{\nabla}}^b=-{\beta}_a{\nabla}^b+{w}_a\end{array}} $$

(3)

Where β_g and β_a are inverse correlation time constants, h8 w_g and w_a are Gaussian white noises. δl^b represents the lever-arm error, and the error equation is:

$$ \delta {\dot{l}}^b=0 $$

(4)

The state equation of the integrated alignment system of the above formulas is:

$$ \dot{X}= FX+W $$

(5)

Where F is the state transition matrix, W is the angular velocity of the gyro measure white noise and the accelerometer ratio measures white noise.The differences between the azimuthZ_φ calculated by MIMU solution φ_I and the magnetometer φ_M are the azimuth observations. On the basis of the differential velocity Z_v and position Z_p of MIMU and GPS, the MIMU/GPS/magnetometer integrated alignment (Sasani et al. 2016) observation equation can be expressed as:

$$ Z= HX+V $$

(6)

Where Z is the observation vector, as shown in eq. (7). H is the matrix of measurement equations, as shown in eq. (8). V is the observation noise, which composed of the azimuth noise of the magnetometer and GPS receiver velocity measurement noise and position measurement noise. The observation noises are all independent.

$$ Z=\left[\begin{array}{c}{Z}_{\varphi}\\ {}{Z}_v\\ {}{Z}_p\end{array}\right] $$

(7)

$$ H=\left[\begin{array}{cccccc}\left[0\kern1em 0\kern1em 1\right]& {0}_{1\times 3}& {0}_{1\times 3}& {0}_{1\times 3}& {0}_{1\times 3}& {0}_{1\times 3}\\ {}{0}_{3\times 3}& {I}_{3\times 3}& {0}_{3\times 3}& {0}_{3\times 3}& {0}_{3\times 3}& {0}_{3\times 3}\\ {}{0}_{3\times 3}& {0}_{3\times 3}& {I}_{3\times 3}& {0}_{3\times 3}& {0}_{3\times 3}& {0}_{3\times 3}\end{array}\right] $$

(8)

Methods

Robust Kalman filter model

The discrete models of the state equation and measurement equation are:

$$ {X}_k={FX}_{k-1}+{W}_{k-1} $$

(9)

$$ {Z}_k={H}_k{X}_k+{V}_k $$

(10)

It can be seen from the above two formulas that the standard Kalman filter update equations are:

$$ {\widehat{X}}_k^{-}=F{\widehat{X}}_{k-1}^{+} $$

(11)

$$ {P}_k^{-}={FP}_{k-1}^{+}{F}^T+{Q}_{k-1} $$

(12)

$$ {P}_{e_k}={H}_k{P}_k^{-}{H}_k^T+{R}_k $$

(13)

$$ {K}_k={P}_k^{-}{H}_k^T{P_{e_k}}^{-} $$

(14)

$$ {X}_k^{+}={X}_k^{-}+{K}_k\left({Z}_k-{H}_k{X}_k\right) $$

(15)

$$ {P}_k^{+}={P}_k^{-}-{K}_k{H}_k{P}_k^{-} $$

(16)

Where ${\widehat{X}}_k^{-}$ and ${X}_k^{+}$ are the prior estimation and posterior estimation of the filtering state, respectively. ${P}_k^{-}$ and ${P}_k^{+}$ are the prior covariance matrix and the posterior covariance matrix, respectively. Z_k is the actual observation value, and K_k is the Kalman filter gain matrix. e_k = Z_k − H_kX_k is the difference between the observed and predicted values, which is usually called the innovation vector whose covariance matrix is as shown in Eq. (13). Assume that the state equation and the measured equation do not contain outliers, and the observation noise obeys Gauss white noise distribution, then the probability density function Z_k is:

$$ \rho \left({Z}_k\right)=\frac{1}{\sqrt{{\left(2\pi \right)}^m\left|{P}_{e_k}\right|}}\exp \left(-\frac{1}{2}{e}_k^T{\left({P}_{e_k}\right)}^{-1}{e}_k\right) $$

(17)

Where m is defined as the dimension of Z_k. We determine the index parameters are as follows:

$$ {\gamma}_k(m)={M}_k^2=-\frac{1}{2}{e}_k^T{\left({P}_{e_k}\right)}^{-1}{e}_k $$

(18)

Where M_k is known as Mahalanobis distance, if there is no outliers, the index parameter γ_k(m) should obey the Chi square distribution of degrees of freedom m. According to the given significance level α (small value), having a Chi-square test for γ_k(m), the probability of the event can be obtained formula:

$$ \Pr \left[\gamma \left(\mathrm{m}\right)>{\chi}_{\alpha, m}^2\right]<\alpha $$

(19)

Where ${\chi}_{\alpha, m}^2$ is the significance level α corresponding to the upper quantile, Pr is the probability of occurrence of the Eq. (19). The probability of occurrence is very small. If occurs, it can reject the original hypothesis, which means the measurement information is affected by the anomalous outliers. Thus, the detection of the outliers could be realized. For the detected outliers, the filter updating weight should be reduced, which can be achieved by correcting the innovation vector covariance matrix.

Robust Estimation factor can be expressed as:

$$ \lambda =\left\{\begin{array}{c}\frac{\gamma \left(\mathrm{m}\right)}{\chi_{\alpha, m}^2}, if\ \gamma \left(\mathrm{m}\right)>{\chi}_{\alpha, m}^2\\ {}1,\kern2.75em otherwise\end{array}\right. $$

(20)

Eq. (20) indicated that its introduction will increase the covariance matrix of the innovation, and reduce the filter gain in order to achieve the suppression of the observed outliers. The index parameters defined by Eq. (18) not only have considered the correlations between the elements of the innovation vector, but also are theoretically more rigorous. The detection of the outliers is based on the standard hypothesis testing process. The threshold selection has a clear statistical meaning.

The correction formula is:

$$ {P}_{e_k}=\lambda {P}_{e_k} $$

(21)

Eq. (14), Eq. (15), Eq. (16) can be obtained by modifying the P_e to obtain the robust Kalman filter algorithm.

Sequential robust Kalman filter algorithm

There are different types of observation information when the multi-sensors are integrated. If the outliers were determined only by an index parameter and then use a robustness factor to modify the innovation vector covariance matrix, it may lose a reliable prediction of state parameters accuracy and the state parameter weight of large outliers margin cannot reduced effectively.

In order to solve the above problems, we apply the sequential updating method to the robust Kalman filter algorithm to detect and correct outliers for different observations. Sequential measurement update method (Dan, 2006) is a method of reducing high dimensional measurement updates to multiple low dimensional measurement updates. In particular, the m dimension observation vector is decomposed into m scalar. In the process of finding the filter gain, the matrix inverse problem is transformed into the reciprocal operation of the scalar, which could effectively reduce the inverse calculation of the matrix and enhance the stability of the numerical calculation.

If the measurement variance matrix R_k is not a diagonal matrix, it can realize the observation vector diagonalization by Cholesky decomposition.

$$ {R}_k={L}_k{L}_k^T $$

(22)

Where L_k is a non-singular upper triangular matrix.

Let ${L}_k^{-1}$ left multiplication eq. (9), available:

$$ {L}_k^{-1}{Z}_k={L}_k^{-1}{H}_k{X}_k+{L}_k^{-1}{V}_k $$

(23)

The above formula can be abbreviated as.

$$ {Z}_k^{\ast }={H}_k^{\ast }{X}_k+{V}_k^{\ast } $$

(24)

among them, ${Z}_k^{\ast }={L}_k^{-1}{Z}_k$,${H}_k^{\ast }={L}_k^{-1}{H}_k$,${V}_k^{\ast }={L}_k^{-1}{V}_k$, and the measurement noise variance matrix in eq. (23) is:

$$ {\displaystyle \begin{array}{l}{R}_k^{\ast }=E\left[{v}_k^{\ast }{\left({v}_k^{\ast}\right)}^T\right]=E\left[{L}_k^{-1}{v}_k{\left({L}_k^{-1}{v}_k\right)}^T\right]\\ {}\kern1em ={L}_k^{-1}E\left[{v}_k{\left({v}_k\right)}^T\right]{\left({L}_k^{-1}\right)}^T={L}_k^{-1}{R}_k{\left({L}_k^{-1}\right)}^T=I\end{array}} $$

(25)

among them,

$$ {H}_k^{\ast }=\left[{h_1^{\ast}}^T\kern0.5em {h_2^{\ast}}^T\kern0.5em \cdots \kern0.5em {h_m^{\ast}}^T\right] $$

(26)

In the kth epoch the first j measurement update, available:

$$ {\widehat{y}}_{k,j}^{\ast }={h}_j^{\ast }{\widehat{x}}_{k,j-1} $$

(27)

$$ {\sigma}_{{\widehat{y}}_{k,j}^{\ast}}^2={h}_j^{\ast }{P}_{k,j}^{-}{h_j^{\ast}}^T+1 $$

(28)

Where: ${\widehat{x}}_{k,j-1}$ is first j − 1 the predicted value of the vector, and ${\widehat{x}}_{k,0}={\widehat{x}}_{k\mid \mathrm{j}\hbox{-} 1}$, ${\widehat{y}}_{k,j}^{\ast }$ is ${\widehat{y}}_k$ first j the predicted value of the element. By a degree of freedom Chi-square test judge Z_{k, j} whether it contains outliers, robustness factor is:

$$ \gamma \left({\tilde{Z}}_{k,j}\right)=\frac{{\left({\tilde{Z}}_{k,j}-{\widehat{y}}_{k,j}^{\ast}\right)}^2}{\sigma_{{\widehat{y}}_{k,j}^{\ast}}^2} $$

(29)

For a given significance level value α and corresponding to the upper quantile ${\chi}_{1,\alpha}^2$, the judgment condition can be expressed as:

$$ \gamma \left({\tilde{Z}}_{k,j}\right)>{\chi}_{1,\alpha}^2 $$

(30)

If ${\tilde{Z}}_{k,j}$ contains outliers, and then modified by the robustness factor innovation vector covariance matrix:

$$ \lambda =\left\{\begin{array}{c}\frac{\gamma \left({Z}_{k,j}\right)}{\chi_{1,\alpha}^2}, if\ \gamma \left({Z}_{k,j}\right)>{\chi}_{1,\alpha}^2\\ {}1,\kern2.75em otherwise\end{array}\right. $$

(31)

$$ {\sigma}_{{\widehat{y}}_{k,j}^{\ast}}^2={\lambda \sigma}_{{\widehat{y}}_{k,j}^{\ast}}^2 $$

(32)

Then we can obtain the Kalman filter updating equations:

$$ K=\frac{P_{k,j}^{-}{h_j^{\ast}}^T}{\sigma_{{\widehat{y}}_{k,j}^{\ast}}^2} $$

(33)

$$ {X}_{k,j}^{+}={X}_{k,j}^{-}+K\left({Z}_{k,j}-{\widehat{y}}_{k,j}^{\ast}\right) $$

(34)

$$ {P}_k^{+}={P}_k^{-}-{\sigma}_{{\widehat{y}}_{k,j}^{\ast}}^2{KK}^T $$

(35)

Results and discussions

Simulations

The simulation was designed to verify the performance of the proposed method. As shown in Fig. 1, the simulation trajectory includes uniform motion, acceleration, deceleration, turning, climbing, descent and other movements. The zero bias stability of the accelerometer and gyro are 16mg and 100°/h, respectively, and its sampling frequency is 100Hz. GPS system horizontal and vertical positioning accuracy are 2.5m and 5m respectively. Moreover, the accuracy of head angle is 2°.

Fig. 1

Simulation trajectory

In order to verify the efficiency of the proposed SRKF algorithm, outliers are added to the simulation data. The outliers to the GPS positions were set as 10 m and 20 m, which obtained at the epoch of 200 s and 400s, respectively. In addition, the outliers to the magnetometer observations were set as 10° and 30°, which obtained at the same epoch of 200 s and 400s as the GPS position outliers. The estimated attitude errors and position errors are shown in Fig. 2.

Fig. 2

Attitude error results of simulations

From Fig. 2, the root mean squared errors in pitch, roll and head angles of the EKF algorithm and SRKF algorithm are 0.4067°, 0.4910°, 1.5831° and 0.1305°, 0.1216°,0.3796°, respectively. It can be concluded that the accuracy of the EKF is nearly equivalent to that of the SRKF algorithm in the absence of outliers. The EKF algorithm has great attitude errors at the epochs of adding measurement outliers. Whereas, the SRKF algorithm can restrain the outliers effectively at these moments, and the attitude accuracy are obviously better than EKF algorithm. The simulation results show that the SRKF algorithm can effectively suppress the influence of the measurement outliers and maintain a better attitude accuracy.

Vehicle experiment

Vehicle based navigation experiment was conducted to verify the efficiency of the proposed method. The experimental data were collected on Huanhai Road in the coastline of Weihai using the micro-based INS/GPS system in the Ellipse2-N series of the SBGSYSTEMS corporation with 2m and 2.5m horizontal and vertical positioning accuracies, respectively. Moreover, the accuracy of its pitch and roll angles are 0.1°, and the accuracy of head angle is 0.5°. We chose the GPS point positioning mode with the sampling frequency of 5Hz. The zero bias stability of the accelerometer and gyro are 57mg and 0.2°/s, respectively, and its sampling frequency is 200Hz. The zero bias stability of the magnetometer is 0.1mGS with its sampling frequency of 25Hz. The Ellipse2-N outputs are considered as the reference values (i.e., the true values). For the chosen algorithm, a loosely-coupled scheme is considered. The total travelled time is about 1 h. The vehicle trajectory is shown in Fig. 3. For expressive simplicity, we only use the first 10 min of data for analysis. The estimated attitude and velocity results are shown in Fig. 4.

Fig. 3

Vehicle trajectory of the field test

Fig. 4

The attitude and velocity results of the field test

Firstly, we compare the extended Kalman filter (EKF) and the proposed SRKF in the absence of outliers. Fig. 5 has shown the attitude and positioning errors of MIMU/GPS/magnetometer integrated alignment based on the EKF and the SRKF. Fig. 5 illustrates that the accuracy of the EKF is nearly equivalent to that of the SRKF algorithm in the absence of outliers, and the specific accuracy is shown in Table 1. The root mean squared errors in pitch, roll and head angles of the SRKF algorithm are 0.1305°, 0.1216° and 0.3796°, respectively. While the root mean squared errors in east, north and up directions are 0.6758m, 1.0425m and 0.5505m, respectively. Therefore, in the absence of outliers, the effect of SRKF algorithm is nearly equivalent to the EKF algorithm.

Fig. 5

Error results of in the absence of outliers

Table 1 Root mean squared errors in attitude and position estimations

Secondly, in order to verify the efficiency of the proposed SRKF algorithm, outliers are added to the experimental data. The following three schemes are designed to make comparisons.

Scheme 1: Only GPS position errors are affected by outliers.

We add different magnitude of outliers, which are 5 m, 10 m and 15 m, to the GPS positions obtained by single point positioning mode at the epoch of 100 s, 200 s and 300 s, respectively.

Scheme 2: Only magnetometer observations are affected by outliers.

We add different magnitude of outliers, which are 100 mGS, 150 mGS and 200 mGS, to the magnetometer observations at the epoch of 150 s, 200 s and 350s, respectively.

Scheme 3: Both GPS positions and magnetometer observations are affected by outliers, and the magnitude and the epochs where outliers are added are the same as the schemes 2 and 3.

The estimated attitude errors and position errors are shown in Figs. 6, 7 and 8, and the specific accuracy is shown in Table 2.

Fig. 6

Error results of scheme 1

Fig. 7

Error results of scheme 2

Fig. 8

Error results of scheme 3

Table 2 Root mean squared errors in attitude and position estimations

From Fig. 6 to Fig. 8, we can conclude that if only add the position errors in scheme 1, the pitch angle, roll angle and position of EKF algorithm all have obvious errors at 100s, 200s, 300s and 400s. While the pitch and roll angle errors of SRKF algorithm are relatively small, and the errors increase with the increasing of the added errors. And it is nearly the same for the head angles between these two methods, for the magnetometer measurements are relatively accurate and could restrain the head angle divergence effectively. In addition, if only add the magnetometer errors in scheme 2, the head angle errors of EKF algorithm are relatively larger than SRKF algorithm at 150s, 200s, 350s and 400s. However, the pitch and roll angle errors and position errors are essentially the same between these two algorithms. The reason lies in the magnetometer errors could cause outliers of head angle, however, due to the fact that GPS measurements are rather accurate, it could restrain the divergence of head angle of EKF algorithm to some extent, and SRKF could effectively restrain the outliers. In Scheme 3, this paper add the position errors and magnetometer errors simultaneously, the EKF algorithm has great attitude and position errors at the time of adding measurement outliers. Whereas, the SRKF algorithm can restrain the outliers effectively at these moments, and the attitude and position accuracy are obviously better than the EKF algorithm. In conclusion, when the measurement outliers exist for the MIMU/GPS/Magnetometer integrated system, the effect of the SRKF algorithm is a lot better than the traditional EKF algorithm, which could effectively restrain the outliers and maintain a better attitude and position accuracy.

Conclusions

In order to handle the problem that the initial alignment of the MIMU/GPS/magnetometer integrated system is susceptible to the outliers, this paper takes the squared Mahalanobis distance as a judging index to detect the existence of outliers. A sequential measurement updating strategy is proposed and combined with the Kalman filter to sequentially suppress the outlier in the observation vector in an element-wise way. Thus, a sequential robust Kalman filter is formulated, which is a new extension of the existing of the robust Kalman filter algorithm. The field test shows that the proposed SRKF algorithm can effectively suppress the influence of outliers in the initial alignment process, and has a better robustness and stability.