 Original article
 Open Access
 Published:
A MIMU/GPS/magnetometer integrated alignment method based on sequential robust estimation
The Journal of Global Positioning Systems volume 16, Article number: 10 (2018)
Abstract
For a microelectromechanical system inertial measurement unit (MEMSIMU), 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 elementwise way. Robust factor, used to suppress the influence of outliers, is constructed according to the Mahalanobis distance and the Chisquare 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 MEMSIMU 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 multisensor 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 multipath 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 nonGaussian 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 Chisquared 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 highdimensional measurements to multiple lowdimensional measurements. Oh (2010) combined the sequential updating method with an Unscented Kalman filter (UKF) in the lowcost multisensor 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 innovationbased 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 quaternionbased 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 Chisquare 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 leverarm errors as the 18 dimensional state vectors of the system state model (Farrell, 2008):
The navigation coordinate system is based on the eastnorthup (ENU) coordinates, and the error equations of the inertial navigation system are shown as follows:
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^{n} 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 firstorder Markov processes:
Where β_{g} and β_{a} are inverse correlation time constants, h8 w_{g} and w_{a} are Gaussian white noises. δl^{b} represents the leverarm error, and the error equation is:
The state equation of the integrated alignment system of the above formulas is:
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:
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.
Methods
Robust Kalman filter model
The discrete models of the state equation and measurement equation are:
It can be seen from the above two formulas that the standard Kalman filter update equations are:
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_{k}X_{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:
Where m is defined as the dimension of Z_{k}. We determine the index parameters are as follows:
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 Chisquare test for γ_{k}(m), the probability of the event can be obtained formula:
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:
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:
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 multisensors 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.
Where L_{k} is a nonsingular upper triangular matrix.
Let \( {L}_k^{1} \) left multiplication eq. (9), available:
The above formula can be abbreviated as.
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:
among them,
In the kth epoch the first j measurement update, available:
Where: \( {\widehat{x}}_{k,j1} \) 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 Chisquare test judge Z_{k, j} whether it contains outliers, robustness factor is:
For a given significance level value α and corresponding to the upper quantile \( {\chi}_{1,\alpha}^2 \), the judgment condition can be expressed as:
If \( {\tilde{Z}}_{k,j} \) contains outliers, and then modified by the robustness factor innovation vector covariance matrix:
Then we can obtain the Kalman filter updating equations:
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°.
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.
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 microbased INS/GPS system in the Ellipse2N 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 Ellipse2N outputs are considered as the reference values (i.e., the true values). For the chosen algorithm, a looselycoupled 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.
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.
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.
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 elementwise 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.
Abbreviations
 EKF:

Extended Kalman filter
 GPS:

Global Positioning System
 MEMSIMU:

Microelectromechanical system inertial measurement unit
 SRKF:

Sequential robust Kalman filter
 UAV:

Unmanned aerial vehicle
 UKF:

Unscented Kalman filter
References
Ali A, Elsheimy N (2013) Lowcost memsbased pedestrian navigation technique for gpsdenied areas. J Sens 2013(4):572–575
Azarbad MR, Mosavi MR (2014) A new method to mitigate multipath error in singlefrequency gps receiver with wavelet transform. GPS Solutions 18(2):189–198
Bai M, Li M (2012) Application of an adaptive sequential Kalman filter to SINS/GPS navigation data fusion. IEEE Intelligent Control and Automation, Beijing, China, pp 4309–4314
Bhatti UI, Feng S (2012) Performance of rate detector algorithms for an integrated gps/ins system in the presence of slowly growing error. GPS Solutions 16(3):293–301
Chang G (2014) Robust kalman filtering based on mahalanobis distance as outlier judging criterion. J Geod 88(4):391–401
Chang L, He H, Qin F (2017) Inmotion initial alignment for odometer aided strapdown inertial navigation system based on attitude estimation. IEEE Sens J 17(3):766–773
Dan S (2006) Optimal state estimation: Kalman, H∞, and nonlinear approaches. WileyInterscience, Hoboken, New Jersey
Farrell J (2008) Aided navigation: GPS with high rate sensors. McGrawHill, Inc, New York, San Francisco, Lisbon, London, Mandrid, Mexico City, Milan, New Delhi, San Juan, Seoul, Singapore, Sydney, Toronto
Georgy J, Karamat T, Iqbal U, Noureldin A (2011) Enhanced memsimu/odometer/gps integration using mixture particle filter. GPS Solutions 15(3):239–252
Groves PD (2013) Principles of gnss, inertial, and multisensor integrated navigation systems, 2nd edn. Artech House, Boston
Huang G, Yang Y, Zhang Q (2011) Estimate and predict satellite clock error using adaptively robust sequential adjustment with classified adaptive factors based on opening windows. Acta Geodaetica Et Cartographica Sinica 40(1):15–21
Khoder W, Jida B (2014) A quaternion scaled unscented kalman estimator for inertial navigation states determination using ins/gps/magnetometer fusion. J Sensor Tech 04(2):101–117
Lee JK, Choi MJ (2017) A sequential orientation kalman filter for AHRS limiting effects of magnetic disturbance to heading estimation. J Electr Eng Technol 12:1675–1682
Mahalanobis PC (1936) On the generalized distance in statistics. Proc Nat Inst Sci 2:49–55
Noureldin A, Karamat TB, Georgy J (2013) Fundamentals of inertial navigation, satellitebased positioning and their integration. Springer Berlin Heidelberg, New York, Dordrecht, London
Oh SM (2010) Multisensor fusion for autonomous UAV navigation based on the unscented Kalman filter with sequential measurement updates, Multisensor fusion and integration for intelligent systems. IEEE, Salt Lake City, UT, USA, pp 217–222
Sasani S, Asgari J, AmiriSimkooei AR (2016) Improving MEMSIMU/GPS integrated systems for land vehicle navigation applications. GPS Solutions 20(1):89–100
Teng Y, Shi Y, Zheng Z (2011) Research on gps receiver positioning algorithm under bad conditions. Chin J Sci Instrum 32(8):1879–1884
Xu P (2005) Signconstrained robust least squares, subjective breakdown point and the effect of weights of observations on robustness. J Geod 79(4–5):288–288
Yang Y, Hai HE, Tian XU (2001) Adaptive robust filtering for kinematic gps positioning. J Geod 75(23):109–116
Zhou Z, Li Y, Zhang J, Rizos C (2016) Integrated navigation system for a lowcost quadrotor aerial vehicle in the presence of rotor influences. J Surv Eng 143(1):05016006x
Acknowledgments
We thank Pengfei Yu of Tongji University and Changqing Zhang of Wuhan University for the paper writing advices.
Funding
This work was supported by the National Key Research and Development Program of China (2016YFC080103) awarded to Jian Wang.
Availability of data and materials
Please contact author for data requests.
Author information
Author notes
Affiliations
Contributions
SW developed the algorithm and analyzed the results of the experiments; JW helped algorithm development and participated in the design of the study; HY carried out revised this manuscript; HL conceived of the experiments. All authors read and approved the final manuscript.
Corresponding author
Correspondence to Jian Wang.
Ethics declarations
Ethics approval and consent to participate
Not applicable.
Competing interests
The authors declared that they have no competing interests.
Publisher’s Note
Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.
Rights and permissions
Open Access This article is distributed under the terms of the Creative Commons Attribution 4.0 International License (http://creativecommons.org/licenses/by/4.0/), which permits unrestricted use, distribution, and reproduction in any medium, provided you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons license, and indicate if changes were made.
About this article
Received
Accepted
Published
DOI
Keywords
 Initial alignment
 MIMU/GPS/magnetometer
 Mahalanobis distance
 Robust estimation
 Sequential Kalman filter