Mathematical Problems in Engineering

Volume 2017 (2017), Article ID 1691320, 9 pages

https://doi.org/10.1155/2017/1691320

## MEMS Based SINS/OD Filter for Land Vehicles’ Applications

^{1}School of Mechanical Engineering, Shandong University, Jinan 250000, China^{2}Key Laboratory of High-Efficiency and Clean Mechanical Manufacture, Ministry of Education, Jinan 250000, China

Correspondence should be addressed to Zengcai Wang

Received 29 July 2016; Revised 29 November 2016; Accepted 5 December 2016; Published 16 January 2017

Academic Editor: Antonino Laudani

Copyright © 2017 Huisheng Liu et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

#### 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: ; .