Abstract
A constrained low-cost SINS/OD filter aided with magnetometer is proposed in this paper. The filter is designed to provide a land vehicle navigation solution by fusing the measurements of the microelectromechanical systems based inertial measurement unit (MEMS IMU), the magnetometer (MAG), and the velocity measurement from odometer (OD). First, accelerometer and magnetometer integrated algorithm is studied to stabilize the attitude angle. Next, a SINS/OD/MAG integrated navigation system is designed and simulated, using an adaptive Kalman filter (AKF). It is shown that the accuracy of the integrated navigation system will be implemented to some extent. The field-test shows that the azimuth misalignment angle will diminish to less than 1°. Finally, an outliers detection algorithm is studied to estimate the velocity measurement bias of the odometer. The experimental results show the enhancement in restraining observation outliers that improves the precision of the integrated navigation system.
1. Introduction
The recent advances in sensors technologies and sensor fusion algorithms contributed to the realization of highly integrated low-cost navigation systems. In most systems, GPS signal and the inertial measurement unit (IMU), which consists of three-axe accelerometers and three-axe rate gyros, are fused to estimate the position and velocity of the vehicle [1, 2]. Nevertheless, the SINS/GPS is not a complete self-contained system since the GPS signal may be disturbed or unavailable under some circumstances. The odometer is a common navigation instrument for ground vehicles. High-resolution odometer can afford accurate distance increment, and the velocity measured is bounded and of high accuracy. References [3–5] present the algorithms that use odometer as the external information source to revise the accumulated SINS velocity error.
In an attempt to cut down on the cost of navigation systems, MEMS based IMU are always used; see [6, 7]. Today’s MEMS sensors are still much less precise than the expensive one to meet the requirement of pure SINS. In [6], an attitude determination system which is based on a low-cost MEMS IMU, a triad of magnetometers, and a commercial global positioning system (GPS) receiver is studied. In [7], an effective adaptive Kalman filter (AKF) is proposed to improve the computational efficiency and dynamic performance of low-cost MIMU/magnetometer integrated Attitude and Heading Reference Systems (AHRS).
The vehicle attitude can be estimated by using gyros, accelerometers, magnetometers, and the global positioning system (GPS) [8]. In [9], tailored filtering and extended Kalman filters (EKF) are applied for the fusion of inertial measurement system (the gyros, accelerometers, and magnetometers) in the prediction of the vehicle attitude. A MEMS IMU and a two-antenna GPS are fused to determine the vehicle attitude for vehicle-mounted satellite-communication Satcom-on-the-move (SOTM), using an adaptive Euler angle based unscented Kalman filter (UKF) [10]. The observability analysis for MEMS based IMU/GPS from many researches shows that the azimuth angle is unobservable in straight motion with constant vehicle acceleration. To solve this problem, [11] presents integration of low-cost SINS/GPS aided with odometer and heading from two-antenna GPS receiver. The test results show that the integrated systems’ accuracy in both position and heading is improved significantly.
Integrated navigation systems often use the Kalman filter which requires correct measurement noise covariance and system model [12]. But the environment of land vehicles may be extremely complex due to bumpiness roads they suffer. Reference [13] proposes an adaptive Kalman filter (AKF) to avoid the nonliner problem, considering the impact of the dynamic acceleration on the filter. In [14], an adaptive Kalman filter for SINS/OD is studied. In the AKF formulation, the SINS/OD uses the velocity integrate form. Reference [14] also does some research on inspecting and correcting outliers of sensors. The odometer measurement and SINS have been fused with a 9-state Kalman filter when low-cost sensors are used; see [15]. The velocity constraint uses the fact that a vehicle moves along its longitudinal axis only and no vehicle side slip motion is possible. If the precision of velocity measurement degrades over time or the IMU cannot be precisely located at the center of the vehicle, then the fault detection method is necessary to assure the accuracy of the navigation system. In [16], techniques based on fuzzy logic are applied for adjusting the measurement matrix based on its relative quantity. The proposed algorithm is tested and compared to other faulty detection methods and it shows its capability of detecting both soft and hard sensor errors.
In this paper, a fault-tolerant AKF is proposed for low-cost SINS/OD/MAG. The velocity’s differences between SINS and odometer in the navigation frame and azimuth error are regarded as the system’s measurement vector. A closed Kalman filter is adopted to estimate and correct system errors. A fault detection method is proposed to detect possible bias faults of the measurement from sensors. A sensor of Xsens MTi is chosen and studied to conduct vehicle experiments. Results of vehicle tests with simulated biases estimation on the odometer measurement are shown in the paper. The format of this paper is as follows. In Section 2, the orientation determination method and SINS error model are described. In Section 3, the measurement equation of MIMU/OD/MAG is derived. In Section 4, an AKF with fault detection method is proposed. In Section 5, experimental results are presented. Finally, a short conclusion of this paper is drawn in Section 6.
2. Inertial Navigation Systems
2.1. Orientation Determination
The main task of SINS is to determine orientation angles, velocity, and position of vehicles. The accelerometer measures the vehicle’s specific force , in three directions of the body frame relative to the inertial frame. The gyroscope measures the vehicle’s angular velocity . The superscript represents the vector measured in the body frame. Both velocity and position of the vehicle are calculated by integrating the specific force once and twice in the navigation frame. An attitude direction cosine matrix (DCM) is often used to transform the specific force to the navigation coordinate frame. The INS attitude, velocity, and position differential equations are discussed in [4]. However, due to the large bias and sensor noises of the MEMS sensors, the orientation angles results may drift quickly. Therefore, a magnetometer aided inertial navigation algorithm is discussed to estimate the gyros’ drift.
When the system is stationary, the direction of the gravity and local magnetic field can be used to stabilize the attitude [17] and the attitude angles and can be calculated as
Then, the magnetic heading is determined as where , , and are the accelerometer sensed specific force in right-forward-up body frame. , , and are the geomagnetic vector measured in the body frame.
In practice, the magnetometer needs to be calibrated for soft iron and hard iron effects which has already been carried out by a calibration procedure based on ellipse-specific fitting error compensation method (MFM) inside the MTi; see [18]. Actually, (1a) and (1b) only work effectively under stationary or low acceleration conditions. The dynamic acceleration of the vehicle will influence the gravity measurement in the body frame and that will lead to an inaccurate inclination (roll/pitch). Therefore, the electric compass (which consists of three-direction accelerometers and magnetometers) and gyros are usually fused as the Attitude and Heading Reference Systems (AHRS) to get a better estimate of the attitude angle.
2.2. Error State Model
The SINS error model is commonly used by the design of Kalman process model. In the present proposed system, the errors are always defined in the navigation frame. Choose the local geographic frame ENU (East-North-Up) as the navigation frame. The body frame is defined by the VBF frame (the vehicle-fixed body frame, which is aligned with its longitudinal, lateral, and vertical axes), assuming that the IMU is exactly located at the center of the vehicle. The error propagation equations for attitude, velocity, and position in the navigation coordinate system can be expressed by the following equations:where , is the Euler angle error of the attitude; and represent the velocity and position error, respectively; , are the earth rotation rate vector and transport rate vector in frame; is the DCM from frame to frame; and are the random constant error of gyros and accelerometers, respectively; is the specific force vector measured in the frame. is the transformation matrix from the vehicle velocity to the angular: where and are the meridian and uniaxial circle radius of the earth, respectively.
In order to estimate the biases of the gyroscopes and the accelerometers and the error of the scale factor, the 16-dimension state vector is chosen as follows:where is the scale factor of odometer which contains both constant and random error [19]. The error of gyroscopes and accelerometers and the scale factor of odometer can be regarded as a constant within a certain period time. Then we have
The error state model is then defined aswhere is the state matrix [20, 21] and is the state noise vector which is assumed to be a Gaussian white noise.
3. Measurement Equation of SINS/OD/MAG
3.1. Odometer Measurements
The output of odometer is the distance increment at time interval along the vehicle forward direction. The odometer measurement is defined as , which can be seen as a continuous variable without loss of generality. Assuming that the vehicle does not slip and will not leave the ground, the velocity in the - and -axes (body frame), and , is constrained to zero mean. Then its measurement equation can be represented as
Considering the attitude error and the scale factor error , the odometer measurement in the frame can be represented aswhere is the skew-symmetric matrix of .
Expanding the above equation,
With the two-order small quantity term assumed negligible, the above equation can be written as
Considering the velocity calculated by SINS,where is the SINS velocity error in the frame.
Velocity residual between SINS and odometer in the frame is
Define the measurement matrix of as Then, (12) can be given in matrix form:where is the observation noise vector of the odometer.
3.2. Magnetometer Constraints
The heading residual between the electronic compass and gyroscopes can be written aswhere is the azimuth angle calculated by SINS and is calculated by (1b). is the azimuth error of the SINS.
However, (1a) and (1b) are only valid under stationary or low acceleration conditions. In [10, 12], a dynamic scalar is used to yield the optimal performance: where is the gravity vector .
The vehicle dynamic acceleration has the following scenarios:where , , and are the variances of accelerometer measurements in the body frame. is the trigger threshold determined by experiments and the design requirements. The rule is interpreted as follows: nonacceleration mode: when , there is no vehicle acceleration, and the yaw angle can be determined by the acceleration and magnetometers; low-acceleration model: when , the dynamic acceleration could be regarded as a part of measurement noise; high-acceleration model: when , the accelerometers measurements are far from the value of the gravity, and then, the yaw angle calculated by (1b) is far away from the truth and the yaw angle is actually determined by the gyros [12].
3.3. System Observation Model
Therefore, with (14) and (15), the measurement vector is given as
Then, the measurement equation is obtained aswhere is the observation noise vector which is assumed to be a Gaussian white noise.
4. Measurements Fusion Using AKF
4.1. Adaptive Kalman Filter
An adaptive Kalman filter is designed to get a better vehicle’s position, velocity, and attitude. The brief structure of the integrated system is shown in Figure 1. Consider the following linear discrete system:where is the state transition matrix; and are the independent Gaussian white noise with zero mean, respectively: ; .
In conventional KF, the measurement noise covariance matrix and process noise covariance matrix are fixed and known a priori. However, actual circumstances may be complex and beyond predictable. In this paper, an adaptive algorithm is proposed to improve , considering that the dynamic acceleration in the land vehicles mainly influences the measurement procedures [22]. The proposed algorithm adopts the innovation sequence to estimate online. The innovation sequence is calculated aswhere is the state vector determined by one-step algorithm.
The estimation error and its corresponding covariance accumulate during the propagated stage. The propagated covariance matrix, , is obtained aswhere is the process noise covariance.
According to KF theory, can also be estimated by the innovation sequence with following equation:where is the measurement noise covariance.
Using above equations, the measurement noise matrix at time is given aswhere is the number of samples.
Using above equations, the measurement noise covariance matrix can be estimated online and unexpected bias on the odometer and electric compass can be detected.
4.2. Online Fault Detection for Odometer and Magnetometer
Even so, the accuracy of velocity estimate will still degrade when vehicles slipping or sliding, or when the magnetic field is interfered. Then, a threshold is applied to detect this situation:where and is the threshold decided by experiments.
The measurements of sensors are determined as normal when the above formula is established; otherwise, the measurements are outliers. The threshold is decided by experiment in advance. When outliers appear, the innovation sequence is calculated aswhere is the correction factor which is given as
By using (27), biases presented in the measurements can be removed. Figure 2 summarizes the flowchart of the proposed algorithm. To test the proposed navigation solution, experimental results are presented in the next section.
5. Experimental Testing
The integrated navigation system was tested onboard a vehicle which was driven along the path shown in Figure 3. The experimental data were logged by an AHRS (Xsens MTi-30) sensor and the odometer and a precise GPS. These sensors were installed on the experimental platform shown in Figure 4. The Xsens MTi-30 is an AHRS sensor comprised of three-axis MEMS accelerometers, gyros, and magnetometers. The MTi-30 can output original MIMU and magnetometer data and precise attitude angle. The algorithm inside the MTi works well, and then, the attitude information is used to provide the reference attitude to an accuracy of 0.4° (in dynamic). The specifications of the MTi unit are shown in the Table 1 [18]. A magnetic calibration procedure was executed before the experiment, and then the initial attitude was obtained from the MTi. The odometer data was obtained from the CAN bus, using VECTOR CAN bus equipment. A precise GPS was installed on the roof of the vehicle to provide the reference position to an accuracy of 1 m (1σ STD) for comparison. The algorithm was running in a Texas Instruments’ C6000 series digital signal processor (DSP) that was connected to a laptop to display and record the results in real time. The data refresh frequency of the MTi-30 was set to 100 Hz. The filter updating frequency was 50 Hz. In Figures 5–8, the results of the test are shown.
(a)
(b)
(c)
(d)
(e)
(f)
5.1. No Bias on the Odometer
In this test, the attitude, velocity, and position are estimated separately by pure SINS and SINS/OD/MAG with no bias on the odometer. Figure 5 shows the attitude estimation with Euler angle derived from the AHRS, SINS, and the proposed navigation system. It is shown that the attitude derived from the pure SINS which is actually calculated by gyroscopes suffers from the long-term accumulated errors caused by large sensor biases. The pitch and roll angles estimated by gyros are not increasing all the time because of the presence of large vehicle vibration. The yaw angle has nothing to do with vehicle acceleration, but it fluctuates when turning occurs and that will increase the yaw angle error. Thus, it is necessary to fuse these sensors together. By using SINS/OD/MAG, the attitude, velocity, and position error will be implemented to some extent. Seen from Figure 5, the attitude calculated output from the proposed SINS/OD/SINS is stable and can track the reference value. It is shown that the mean error of the pitch, roll, and yaw angle are below 0.44°, 0.39°, and 1°, respectively. The path taken by the vehicle is estimated and shown in Figure 6. To illuminate the effect of the filter, the positions decomposed in east and north axes are compared with a precise GPS in Figure 7. The vehicle travels about 1 km after 150 s. It can be seen that the path estimated by the AKF closely follows the real path and the differences between the GPS and filtering results in east and north directions are less than 10 meters. Figures 8(a) and 8(b) show the velocity estimate with time. From Figures 8(a) and 8(b) it can be seen that, throughout the journey, velocity errors in east and north are around 0 km/h. This validates the performance of the AKF algorithm implemented in this paper. To validate the function of the fault detection method, a bias of 10 km/h on the odometer is tested in the next section.
5.2. A Bias of 10 km/h on the Odometer
In this test, a bias of 10 km/h is simulated on the odometer velocity readings to simulate a faulty odometer. The fault detection algorithm implemented should detect this bias and successfully remove it from the measurement. Figures 8(c) and 8(d) show the accuracy of velocity estimate. Figures 8(e) and 8(f) show the estimate of velocity with and without fault detection method. The bias is simulated on the odometer velocity readings in 50–150 s. During the first 50 seconds, the velocity is precisely estimated and the mean error is around 0.08 m/s. The fault detection method starts functioning soon after the time a bias added. The error increases slowly with time which is as expected owing to the presence of faulty odometer measurement. The threshold is decided by experiment as before applying the fault detection algorithm. The results validate the performance of the threshold used in the AKF algorithm. It can be seen from Figures 8(e) and 8(f) that the velocity estimated with fault detection method closely follows the actual velocity and that validates that the filter provides high-accuracy velocity estimate even when the vehicle is slipping or sliding.
6. Conclusion
In this paper, a low-cost SINS/OD filter aided with magnetometer is proposed. An adaptive Kalman filter with an outliers detection method is utilized for the fusion of sensors. The error model of SINS/OD has been derived, and the measurement equation is established by analyzing the azimuth error and velocity residuals between the SINS and odometer. A threshold is put up to detect innovation sequence so that any biases presented in the measurements can be removed. Experiment results are shown to demonstrate the performance of the filter. The results are compared with a precise GPS, showing that the integrated system provides high-accuracy position and attitude estimates even under the presence of a bias on the odometer measurement.
When low-cost MIMU is used for SINS, the error will accumulate quickly because of the large MEMS sensor measurement noise. To solve this problem, the magnetometer and odometer are fused together to estimate and revise SINS accumulating errors. The odometer and magnetometer are common navigation instruments for ground vehicles. Therefore, the filter could be realized onboard a car with a core of a Texas Instruments’ C6000 series digital signal processor [23] for real time applications. As a result, the costs for the whole system will be much cheaper than a commercial integrated navigation system [24].
However, a limitation of the proposed method is that the magnetometers cannot be precisely calibrated for the reason that magnet field environment onboard vehicles may be extremely complex. This problem will be focused in our future research.
Competing Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgments
This work was supported by the Foundation of National Key Laboratory of Automotive Simulation and Control (20121107), China.