Research on the automotive sensor–aided low-cost inertial navigation system for land vehicles

Abstract

When a low-cost micro-electro-mechanical system inertial measurement unit is used for a vehicle navigation system, errors will quickly accumulate because of the large micro-electro-mechanical system sensor measurement noise. To solve this problem, an automotive sensor–aided low-cost inertial navigation system is proposed in this article. The error-state model of the strapdown inertial navigation system has been derived, and the measurements from the wheel speed sensor and steer angle sensor are used as the new observation vector. Then, the micro-electro-mechanical system inertial measurement unit/wheel speed sensor/steer angle sensor–integrated system is established based on adaptive Kalman filtering. The experimental results show that the positioning error of micro-electro-mechanical system inertial measurement unit/wheel speed sensor/steer angle sensor is 94.67%, 98.88%, and 97.88% less than the values using pure strapdown inertial navigation system in the east, north, and down directions, respectively. The yaw angle error is reduced to less than 1°, and the vehicle velocity estimation of micro-electro-mechanical system inertial measurement unit/wheel speed sensor/steer angle sensor–integrated navigation system is closer to the reference value. These results show the precision of the integrated navigation solution.

Introduction

A micro-electro-mechanical system (MEMS) sensor has the advantages of small size, low energy consumption and low price, but it has a large measurement error. ¹ The MEMS inertial measurement unit (MIMU) has a wide range of applications in vehicle navigation. The MIMU contains a three-axis accelerometer and a three-axis gyroscope. The estimation of the attitude, velocity, and position of the vehicle can be obtained by integrating the acceleration and angular velocity of the body in the navigation frame. Thus, when the MIMU is used for position estimation, the positioning accuracy is low in a short time and rapidly accumulates with time. The combination of the global positioning system (GPS) and MIMU has been proposed in earlier studies,^2,3 which use the GPS to assist the strapdown inertial navigation system (SINS) to obtain the vehicle position, velocity, and attitude information. Then, the information is applied to the vehicle navigation systems, and the MIMU/GPS is currently widely used in common automotive products. However, the GPS signal is easily disturbed and relies on the satellite information signal, which makes the MIMU/GPS system incomplete and not self-contained. There are common automotive sensors, such as the wheel speed sensor (WSS), acceleration sensor (GX, GY), yaw rate sensor (YRS), and steering wheel angle sensor (SAS), which can be combined with the MIMU to improve the navigation system accuracy and reliability. Abdel-Hafez et al. ⁴ and Yan ⁵ proposed a method to correct the drift error of the SINS using WSSs. This method can solve the positioning accuracy problem of vehicle navigation systems when the GPS signal is invalid. The experimental results show that the combination of MIMU/WSS can provide more accurate navigation and positioning information when the GPS fails.

Although the SINS can estimate the vehicle speed and attitude, the integral error will appear in the measurement process. ⁶ A longer duration of time measurement will result in a larger error, and these integral errors cannot be independently eliminated. Therefore, the SAS of the vehicle is used to provide a new measurement to prevent the gyroscope measurement error in the direction of vehicle travel. The SAS is a common vehicle sensor, which was originally used only in the electronic stability program (ESP) of the vehicle; currently, it is used in various electronic control and auxiliary driving systems, such as the electric steering system, adaptive cruise system (ACC), and automatic driving system. ⁷ Guo, ⁸ through a simulation experiment, proposed the combination of MIMU, SAS, and WSS individually to provide navigation information for the vehicle and concluded that the MIMU/SAS combination method and MIMU/WSS combination method had an increase in positioning accuracy of 62% and 75% compared with the SINS and both have better combination navigation performance. Salmon and Bevly ⁹ combined the vehicle dynamics model with the SAS and WSS sensors to obtain similar positioning data to assist the inertial navigation system (INS)/GPS for vehicle navigation and demonstrate that the combination of low-cost sensor and vehicle model based on real experimental data can increase the positioning accuracy for the vehicle.

The usual method of reducing the INS error is to establish the error-state system model first; then, the appropriate observation is found, and the observation equation is established, particularly through the adaptive Kalman filter (AKF) and other data fusion methods to estimate the error; finally, the attitude, speed, and position estimation information are corrected.^10,11 The WSS in the real driving environment will produce an error because of vehicle skidding, yaw, and other factors, so the AKF is used to correct the wheel speed in real time in this article. Cheng et al. ¹² and Davari and Gholami ¹³ describe the application of the AKF in the MEMS/GPS vehicle-integrated navigation and underwater vehicle navigation. Sebesta and Boizot ¹⁴ proposed a real-time adaptive high-gain extended Kalman filter (EKF) for a quadcopter INS. Adaptive Kalman filtering is a filtering method with suppression filter divergence. On one hand, it can constantly correct the predicted value in the filter calculation process. On the other hand, the AKF can estimate and correct the unknown system model parameters and noise statistics parameters. ¹⁵ In this article, this type of filtering method is used to fuse the signal data of the MIMU/WSS/SAS vehicle-integrated navigation system. Compared to the Kalman filter, the system noise and measurement noise matrix are initially fixed or known. The AKF can correct the inaccurate measurement noise in the actual situations, and this method can achieve better navigation performance.

Based on the existing research, this article uses the combination of odometer and INS, that is, the combination system of MIMU/WSS and uses the steering angle measurement as the new observation to construct the MIMU/WSS/SAS-integrated navigation system. This method integrates SAS and WSS into one system, and the measurements of the sensors are used as observations, which can increase the navigation accuracy. The article structure is as follows. The first part introduces the positioning algorithm of the INS. The second part analyzes the error-state model of each sensor. The third part deduces the observation equation of the system and the theory of AKF. The final part verifies the analysis using actual vehicle experiments and draws the conclusions.

Positioning algorithm

The principle of the SINS is to integrate the angular velocity into the attitude, integrate the acceleration into the velocity, and subsequently integrate the velocity into the position information. The local geographic frame East-North-Up (ENU) is selected as the navigation frame (n frame). The body frame is defined by the vehicle-fixed body frame (the VBF frame (b frame), which is aligned with its longitudinal, lateral, and vertical axes), assuming that the MIMU is exactly located at the center of the vehicle. The acceleration measured by the MIMU is the acceleration of the body frame relative to the inertial frame. The angular velocity is the angular velocity of the body frame relative to the inertial frame. The angular velocity and acceleration are integrated in the navigation frame. The change rates of the attitude, velocity, and position of the vehicle body can be obtained ¹⁶ as follows

$${\stackrel{·}{\mathit{C}}}_{\mathrm{b}}^{\mathrm{n}}={\mathit{C}}_{\mathrm{b}}^{\mathrm{n}}[({\mathit{\omega }}_{\mathrm{ib}}^{\mathrm{b}}-{\mathit{\omega }}_{\mathrm{in}}^{\mathrm{b}})\times ]$$

(1)

$${\stackrel{·}{\mathit{v}}}^{\mathrm{n}}={\mathit{C}}_{\mathrm{b}}^{\mathrm{n}}{\mathit{f}}^{\mathrm{b}}-(2{\mathit{\omega }}_{\mathrm{ie}}^{\mathrm{n}}+{\mathit{\omega }}_{\mathrm{en}}^{\mathrm{n}})\times {\mathit{v}}^{\mathrm{n}}+{\mathit{g}}^{\mathrm{n}}$$

(2)

$$\stackrel{·}{\mathit{P}}=\mathit{\xi }{\mathit{v}}^{\mathrm{n}}$$

(3)

where ${\mathit{C}}_{\mathrm{b}}^{\mathrm{n}}$ is the attitude direction cosine matrix (DCM) from frame b to frame n; DCM is often used to transform the specific force to the navigation coordinate frame; ${\mathit{\omega }}_{\mathrm{ib}}^{\mathrm{b}}$ and ${\mathit{\omega }}_{\mathrm{in}}^{\mathrm{b}}$ are the earth rotation rate vector and transport rate vector in frame b; ${\mathit{\omega }}_{\mathrm{ie}}^{\mathrm{n}}$ and ${\mathit{\omega }}_{\mathrm{en}}^{\mathrm{n}}$ are the earth rotation rate vector and transport rate vector in frame n; $[({\mathit{\omega }}_{\mathrm{ib}}^{\mathrm{b}}-{\mathit{\omega }}_{\mathrm{in}}^{\mathrm{b}})\times ]$ is the antisymmetric matrix based on $({\mathit{\omega }}_{\mathrm{ib}}^{\mathrm{b}}-{\mathit{\omega }}_{\mathrm{in}}^{\mathrm{b}})$ ; n and b represent the corresponding coordinate frames where the parameters are decomposed in the corresponding frame; v and P are the body speed and position vector, respectively; $P=[L\lambda h]$ , where $L,\lambda ,\mathrm{and}h$ are the latitude, longitude, and height, respectively; ${\mathit{f}}^{\mathrm{b}}$ is the specific force vector measured in frame b; g is the local gravity acceleration vector; $\mathit{\xi }$ is the transformation matrix from the vehicle velocity to the angular

$$\mathit{\xi }=\left[\begin{array}{ccc}\frac{1}{R_M+h}\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& \frac{\sec L}{R_N+h}\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 1\hfill \end{array}\right],$$

where $R_M$ and $R_N$ are the meridian and uniaxial circle radius of the earth, respectively.

Using formulas (1)–(3), we can deduce the equations of the attitude, velocity, and position in the discrete case. When the initial alignment is complete, the current attitude, velocity, and position information of the vehicle can be obtained using the integral operation. ¹⁷

Error-state model

In practical applications, it is necessary to estimate and compensate for the errors caused by the accelerometers, gyroscopes, WSSs, and SASs. The error propagation equations of the attitude, velocity, and position in the navigation frame system are expressed as follows

$${\stackrel{·}{\mathit{\varphi }}}^{\mathrm{n}}=-({\mathit{\omega }}_{\mathrm{ie}}^{\mathrm{n}}+{\mathit{\omega }}_{\mathrm{en}}^{\mathrm{n}})\times {\mathit{\varphi }}^{\mathrm{n}}+\delta {\mathit{\omega }}_{\mathrm{ie}}^{\mathrm{n}}+\delta {\mathit{\omega }}_{\mathrm{en}}^{\mathrm{n}}+{\mathit{C}}_{\mathrm{b}}^{\mathrm{n}}{\mathit{\epsilon }}_{\mathrm{g}}^{\mathrm{b}}$$

(4)

$$\begin{array}{c}\multicolumn{1}{c}{\delta {\stackrel{·}{\mathit{v}}}^{\mathrm{n}}=\left({\mathit{C}}_{\mathrm{b}}^{\mathrm{n}}{\mathit{f}}^{\mathrm{b}}\right)\times {\mathit{\varphi }}^{\mathrm{n}}-(2\delta {\mathit{\omega }}_{\mathrm{ie}}^{\mathrm{n}}+\delta {\mathit{\omega }}_{\mathrm{en}}^{\mathrm{n}})}\\ \multicolumn{1}{c}{\times {\mathit{v}}^{\mathrm{n}}-(2{\mathit{\omega }}_{\mathrm{ie}}^{\mathrm{n}}+{\mathit{\omega }}_{\mathrm{en}}^{\mathrm{n}})\times \delta {\mathit{v}}^{\mathrm{n}}+{\mathit{C}}_{\mathrm{b}}^{\mathrm{n}}{\mathit{\epsilon }}_a^{\mathrm{b}}}\end{array}$$

(5)

$$\delta \stackrel{·}{\mathit{P}}=\delta \mathit{\xi }{\mathit{v}}^{\mathrm{n}}+\mathit{\xi }\delta {\mathit{v}}^{\mathrm{n}}$$

(6)

where $\delta$ is the error of the corresponding physical quantity; $\delta \mathit{v}$ and $\delta \mathit{P}$ are the speed and position errors, respectively; ${\mathit{\varphi }}^{\mathrm{n}}$ is the attitude angle error in the navigation frame; ${\epsilon }_g$ and ${\epsilon }_a$ are the gyroscope and accelerometer measurement errors, respectively. The accelerometer and gyroscope measurement errors can be considered a constant value in a short time, so ${\epsilon }_g$ and ${\epsilon }_a$ satisfy equations (7) and (8)

$${\stackrel{·}{\mathit{\epsilon }}}_g=0$$

(7)

$${\stackrel{·}{\epsilon }}_a=0$$

(8)

Equations (4)–(6) describe the propagation process of the pure inertial system error, and the measurement error should be considered when the WSS and steering wheel sensor are used to assist the navigation system. The speed of the WSS is the product of the angular velocity and the rolling radius of the vehicle tire, and the rolling radius is affected by the ambient temperature and road roughness. Therefore, the error source is the change in scale factor, which is mainly caused by the difference between the rolling radius and the vehicle tire radius. In the calibration process, because the effect of the scale factor is more stable in a short time, it can be used as a constant. We assume that the incomplete constraint of the vehicle is satisfied, and the scale factor error of the WSS is $\delta k$ . In addition, $\delta k$ should satisfy

$$\delta \stackrel{·}{k}=0$$

(9)

Ashrafi et al. ¹⁸ analyzed the principle and error source of the measurement of the SAS. The measurement error of SAS consists of the following two parts: random error ${\epsilon }_s$ , which is caused by the magnetic field interference during the measurement, and scale factor error $\delta \stackrel{·}{s}$ , which is caused by the calibration of the front wheel angle. ${\epsilon }_s$ and $\delta \stackrel{·}{s}$ satisfy the following equations

$${\stackrel{·}{\epsilon }}_s=0$$

(10)

$$\delta \stackrel{·}{s}=0$$

(11)

Through the above analysis, the MIMU/WSS/SAS-integrated navigation system error-state model can be established. The error-state vector contains 18 parameters as follows

$$\mathit{X}={\left[\begin{array}{cccccccc}{\mathit{\varphi }}^{\mathrm{nT}}\hfill & \delta {\mathit{v}}^{\mathrm{nT}}\hfill & \delta {\mathit{P}}^{\mathrm{T}}\hfill & {\mathit{\epsilon }}_g^{\mathrm{bT}}\hfill & {\mathit{\epsilon }}_a^{\mathrm{bT}}\hfill & \delta k\hfill & \delta s\hfill & {\epsilon }_s\hfill \end{array}\right]}^{\mathrm{T}}$$

The system-state equation is

$$\stackrel{·}{\mathit{X}}=\mathit{\Phi }\mathit{X}+\mathit{w}$$

(12)

where $\mathit{\Phi }$ is an 18-dimensional system-state transition matrix, ¹⁹ with a value of $\mathit{\Phi }=\left[\begin{array}{cc}{\mathit{F}}_{\mathrm{SINS}}\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill \end{array}\right]$ ; where ${\mathit{F}}_{\mathrm{SINS}}$ is the state transition matrix of the SINS, as shown by Rogers; ¹⁹ w is the system noise vector.

Integrated navigation system

Observation equation

In general, when an MIMU/WSS-integrated navigation system is built, the difference between the measured speed and the calculated inertia using the WSS is used as the measure.^20,21 Assuming that the vehicle does not slip and does not leave the ground, the speeds of the vehicle in the lower axis (x-axis) and vertical axis (z-axis) are $v_{\mathrm{x}}^{\mathrm{b}}=0$ and $v_z^{\mathrm{b}}=0$ , respectively. The WSS can directly measure the speed of the vehicle $(v_{\mathrm{y}}^{\mathrm{b}})$ in the running direction (y-axis). The speed vector of the vehicle in the body frame system ${\mathit{v}}_{\mathrm{wss}}^{\mathrm{b}}$ that the WSS measures is

$${\mathit{v}}_{\mathrm{wss}}^{\mathrm{b}}=\left[\begin{array}{ccc}0\hfill & v_{\mathrm{y}}^{\mathrm{b}}\hfill & 0\hfill \end{array}\right]$$

(13)

The speed measured by the WSS can be converted to the navigation coordinate system using the attitude matrix ${\mathit{C}}_{\mathrm{b}}^{\mathrm{n}}$ . Considering the attitude error angle and WSS scale coefficient error, the vehicle speed vector measured in the body frame system is

$${\tilde{\mathit{v}}}_{\mathrm{wss}}^{\mathrm{n}}=[{\mathit{I}}_{3\times 3}-(\mathit{\varphi }\times )]{\mathit{C}}_{\mathrm{b}}^{\mathrm{n}}(1+\delta k){\mathit{v}}_{\mathrm{wss}}^{\mathrm{b}}$$

(14)

where ${\mathit{I}}_{3\times 3}$ is the $3\times 3$ unit matrix, and $(\mathit{\varphi }\times )$ is the antisymmetric matrix of $\mathit{\varphi }$ .

Extending equation (14) and ignoring the second-order small quantity $-\delta k{\mathit{v}}_{\mathrm{wss}}^{\mathrm{b}}(\mathit{\varphi }\times )$ , we obtain equation (15)

$${\tilde{\mathit{v}}}_{\mathrm{wss}}^{\mathrm{n}}={\mathit{C}}_{\mathrm{b}}^{\mathrm{n}}{\mathit{v}}_{\mathrm{wss}}^{\mathrm{b}}-(\mathit{\varphi }\times ){\mathit{C}}_{\mathrm{b}}^{\mathrm{n}}{\mathit{v}}_{\mathrm{wss}}^{\mathrm{b}}+\delta k{\mathit{C}}_{\mathrm{b}}^{\mathrm{n}}{\mathit{v}}_{\mathrm{wss}}^{\mathrm{b}}$$

(15)

According to equation (15), the residual speed between the velocity of the inertia output ${\tilde{\mathit{v}}}_{\mathrm{sins}}^{\mathrm{n}}$ and ${\tilde{\mathit{v}}}_{\mathrm{wss}}^{\mathrm{n}}$ can be expressed as

$${\mathit{z}}_{\mathrm{v}}={\tilde{\mathit{v}}}_{\mathrm{sins}}^{\mathrm{n}}-{\tilde{\mathit{v}}}_{\mathrm{wss}}^{\mathrm{n}}=\delta {\mathit{v}}^{\mathrm{n}}-({\mathit{v}}_{\mathrm{wss}}^{\mathrm{n}}\times )\mathit{\varphi }-{\mathit{v}}_{\mathrm{wss}}^{\mathrm{n}}\delta k$$

(16)

where $\delta {\mathit{v}}^{\mathrm{n}}$ is the speed error of the INS.

Using equation (16), the difference between the speed of the vehicle measured by the WSS in frame n and the speed output of the inertial guidance system in frame n is taken as the observation quantity, and the navigation system accuracy can be improved. The measurement error of the SAS does not accumulate over time and can also be used to improve the SINS navigation system accuracy.

Most vehicles are front-wheeled vehicles. When the vehicle turns, the steering wheel angle can be converted into the deflection angle ${\alpha }_{\mathrm{SAS}}$ , which is the deflection angle of the vehicle front wheel relative to the center position. The relationship between steering wheel angle ${\alpha }_{\mathrm{SAS}}$ and $\vartheta$ is expressed in (17)

$${\alpha }_{\mathrm{SAS}}=s\vartheta +\eta$$

(17)

where s is the scale factor, and $\eta$ is a constant.

Considering error $\delta s$ in the calibration process and random error ${\epsilon }_s$ caused by the magnetic field interference, the measurement error $\delta {\alpha }_{\mathrm{SAS}}$ is expressed as

$$\delta {\alpha }_{\mathrm{SAS}}={\tilde{\alpha }}_{\mathrm{SAS}}-{\alpha }_{\mathrm{SAS}}=\vartheta \delta s-{\epsilon }_s$$

(18)

In addition to using the steering wheel angle, the front wheel angle can also be obtained from the speed in the vehicle coordinate system

$${\alpha }_{\mathrm{sins}}=-\arctan \left(\frac{v_{\mathrm{x}}^{\mathrm{b}}}{v_{\mathrm{y}}^{\mathrm{b}}}\right)$$

(19)

where $v_{\mathrm{x}}^{\mathrm{b}}$ and $v_{\mathrm{y}}^{\mathrm{b}}$ are the velocity components of the inertial navigation output in the body frame; the relationship between $v_{\mathrm{x}}^{\mathrm{b}}$ and $v_{\mathrm{y}}^{\mathrm{b}}$ is ${\mathit{v}}^{\mathrm{b}}={\mathit{C}}_n^{\mathrm{b}}{\mathit{v}}^{\mathrm{n}}$ .

Considering the attitude error ${\mathit{\varphi }}^{\mathrm{n}}$ and speed error $\delta {\mathit{v}}^{\mathrm{n}}$ , we have

$${\tilde{\mathit{v}}}^{\mathrm{b}}={\mathit{C}}_n^{\mathrm{b}}[1+(\mathit{\varphi }\times )]({\mathit{v}}^{\mathrm{n}}+\delta {\mathit{v}}^{\mathrm{n}})$$

(20)

By extending equation (20) and ignoring the second-order small quantity ${\mathit{C}}_n^{\mathrm{b}}(\mathit{\varphi }\times )\delta {\mathit{v}}^{\mathrm{n}}$ , we obtain

$${\tilde{\mathit{v}}}^{\mathrm{b}}={\mathit{C}}_n^{\mathrm{b}}{\mathit{v}}^{\mathrm{n}}+{\mathit{C}}_n^{\mathrm{b}}\delta {\mathit{v}}^{\mathrm{n}}+{\mathit{C}}_n^{\mathrm{b}}(\mathit{\varphi }\times ){\mathit{v}}^{\mathrm{n}}$$

(21)

The observation error $\delta {\alpha }_{\mathrm{sins}}$ can be expressed as

$$\begin{array}{c}\multicolumn{1}{c}{\delta {\alpha }_{\mathrm{sins}}=\delta [-\arctan \left(\frac{v_{\mathrm{x}}^{\mathrm{b}}}{v_{\mathrm{y}}^{\mathrm{b}}}\right)]=\frac{\partial {\alpha }_{\mathrm{sins}}}{\partial v_{\mathrm{x}}^{\mathrm{b}}}\delta v_{\mathrm{x}}^{\mathrm{b}}+\frac{\partial {\alpha }_{\mathrm{sins}}}{\partial v_{\mathrm{y}}^{\mathrm{b}}}\delta v_{\mathrm{y}}^{\mathrm{b}}}\\ \multicolumn{1}{c}{=-\frac{v_{\mathrm{y}}^{\mathrm{b}}}{{\left(v_{\mathrm{x}}^{\mathrm{b}}\right)}^2+{\left(v_{\mathrm{y}}^{\mathrm{b}}\right)}^2}\delta v_{\mathrm{x}}^{\mathrm{b}}+\frac{v_{\mathrm{x}}^{\mathrm{b}}}{{\left(v_{\mathrm{x}}^{\mathrm{b}}\right)}^2+{\left(v_{\mathrm{y}}^{\mathrm{b}}\right)}^2}\delta v_{\mathrm{y}}^{\mathrm{b}}}\end{array}$$

(22)

Combining equations (21) and (22), we obtain equation (23)

$$\delta {\alpha }_{\mathrm{sins}}=\mathit{M}\delta {\mathit{v}}^{\mathrm{bT}}+\mathit{N}{\mathit{\varphi }}^{\mathrm{T}}$$

(23)

where

$${\mathit{C}}_{\mathrm{K}}^{\mathrm{b}}=-{\mathit{C}}_{\mathrm{n}}^{\mathrm{b}}({\mathit{v}}^{\mathrm{n}}\times )$$

$$\mathit{M}=\left[\begin{array}{ccc}\frac{{\mathit{C}}_n^{\mathrm{b}}(2,1)v_{\mathrm{x}}^{\mathrm{b}}-{\mathit{C}}_n^{\mathrm{b}}(1,1)v_{\mathrm{y}}^{\mathrm{b}}}{{\left(v_{\mathrm{x}}^{\mathrm{b}}\right)}^2+{\left(v_{\mathrm{y}}^{\mathrm{b}}\right)}^2}\hfill & \frac{{\mathit{C}}_n^{\mathrm{b}}(2,2)v_{\mathrm{x}}^{\mathrm{b}}-{\mathit{C}}_n^{\mathrm{b}}(1,2)v_{\mathrm{y}}^{\mathrm{b}}}{{\left(v_{\mathrm{x}}^{\mathrm{b}}\right)}^2+{\left(v_{\mathrm{y}}^{\mathrm{b}}\right)}^2}\hfill & \frac{{\mathit{C}}_n^{\mathrm{b}}(2,3)v_{\mathrm{x}}^{\mathrm{b}}-{\mathit{C}}_n^{\mathrm{b}}(1,3)v_{\mathrm{y}}^{\mathrm{b}}}{{\left(v_{\mathrm{x}}^{\mathrm{b}}\right)}^2+{\left(v_{\mathrm{y}}^{\mathrm{b}}\right)}^2}\hfill \end{array}\right]$$

$$\mathit{N}=\left[\begin{array}{ccc}\frac{{\mathit{C}}_K^{\mathrm{b}}(2,1)v_{\mathrm{x}}^{\mathrm{b}}-{\mathit{C}}_K^{\mathrm{b}}(1,1)v_{\mathrm{y}}^{\mathrm{b}}}{{\left(v_{\mathrm{x}}^{\mathrm{b}}\right)}^2+{\left(v_{\mathrm{y}}^{\mathrm{b}}\right)}^2}\hfill & \frac{{\mathit{C}}_K^{\mathrm{b}}(2,2)v_{\mathrm{x}}^{\mathrm{b}}-{\mathit{C}}_K^{\mathrm{b}}(1,2)v_{\mathrm{y}}^{\mathrm{b}}}{{\left(v_{\mathrm{x}}^{\mathrm{b}}\right)}^2+{\left(v_{\mathrm{y}}^{\mathrm{b}}\right)}^2}\hfill & \frac{{\mathit{C}}_K^{\mathrm{b}}(2,3)v_{\mathrm{x}}^{\mathrm{b}}-{\mathit{C}}_K^{\mathrm{b}}(1,3)v_{\mathrm{y}}^{\mathrm{b}}}{{\left(v_{\mathrm{x}}^{\mathrm{b}}\right)}^2+{\left(v_{\mathrm{y}}^{\mathrm{b}}\right)}^2}\hfill \end{array}\right]$$

The angle residual can be obtained from equations (18) and (22)

$$z_{\varphi }=\delta {\alpha }_{\mathrm{sins}}-\delta {\alpha }_{\mathrm{SAS}}=\mathit{M}\delta {\mathit{v}}^{\mathrm{bT}}+\mathit{N}{\mathit{\varphi }}^{\mathrm{T}}-\theta \delta s+{\epsilon }_s$$

(24)

Therefore, with equations (16) and (24), the measurement vector is

$${\mathit{Z}}_{\mathrm{k}}={\left[\begin{array}{cc}{\mathit{z}}_{\mathrm{v}}\hfill & z_{\varphi }\hfill \end{array}\right]}^{\mathrm{T}}$$

(25)

Then, the measurement equation is

$${\mathit{Z}}_{\mathrm{k}}={\mathit{H}}_{\mathrm{k}}\mathit{X}+\mathit{v}$$

(26)

where ${\mathit{H}}_{\mathrm{k}}={\left[\begin{array}{cc}{\mathit{H}}_1\hfill & {\mathit{H}}_2\hfill \end{array}\right]}^{\mathrm{T}}$ ; ${\mathit{H}}_1=\left[\begin{array}{ccccc}{\mathit{I}}_{3\times 3}\hfill & -({\mathit{v}}_{\mathrm{wss}}^{\mathrm{n}}\times )\hfill & 0_{3\times 9}\hfill & -{\mathit{v}}_{\mathrm{wss}}^{\mathrm{n}}\hfill & 0_{3\times 2}\hfill \end{array}\right]$ ; ${\mathit{H}}_2=\left[\begin{array}{ccccc}\mathit{M}\hfill & \mathit{N}\hfill & 0_{1\times 10}\hfill & -\vartheta \hfill & 1\hfill \end{array}\right]$ ; and v is the system observing noise vector.

AKF

An AKF is designed to better determine the position, velocity, and attitude of the vehicle. The brief structure of the integrated system is shown in Figure 1. Consider the following linear discrete system

$$\{\begin{array}{c}{\mathit{X}}_{\mathrm{k}}={\mathit{\Phi }}_{\mathrm{k},\mathrm{k}-1}{\mathit{X}}_{\mathrm{k}-1}+{\mathit{W}}_{\mathrm{k}-1}\hfill \\ \multicolumn{1}{c}{{\mathit{Z}}_{\mathrm{k}}={\mathit{H}}_{\mathrm{k}}{\mathit{X}}_{\mathrm{k}}+{\mathit{V}}_{\mathrm{k}}}\end{array}$$

(27)

where ${\mathit{\Phi }}_{\mathrm{k},\mathrm{k}-1}$ is the state transition matrix; ${\mathit{W}}_{\mathrm{k}-1}$ and ${\mathit{V}}_{\mathrm{k}}$ are the independent Gaussian white noise with zero mean: ${\mathit{W}}_{\mathrm{k}}~\mathit{N}(0,\mathit{Q})$ ; ${\mathit{V}}_{\mathrm{k}}~\mathit{N}(0,\mathit{R})$ .

Figure 1.

Structure of the MIMU/WSS/SAS.

In the conventional Kalman filter, the measurement noise covariance matrix R and process noise covariance matrix Q are fixed and known a priori. However, actual circumstances may be complex and beyond predictable. In this article, an adaptive algorithm is proposed to improve R considering that the dynamic accelerations of land vehicles mainly affect the measurement procedures. The proposed algorithm uses the innovation sequence to estimate R online. The innovation sequence is calculated as

$${\mathit{\epsilon }}_{\mathrm{k}}={\mathit{Z}}_{\mathrm{k}}-{\mathit{H}}_{\mathrm{k}}{\hat{\mathit{X}}}_{\frac{\mathrm{k}}{\mathrm{k}-1}}$$

(28)

where ${\hat{\mathit{X}}}_{\mathrm{k}/\mathrm{k}-1}$ is the state vector, which is determined by the one-step algorithm.

The estimation error and its corresponding covariance accumulate during the propagation stage. The propagated covariance matrix ${\mathit{P}}_{\mathrm{k}/\mathrm{k}-1}$ is

$${\mathit{P}}_{\frac{\mathrm{k}}{\mathrm{k}-1}}={\mathit{\Phi }}_{\mathrm{k},\mathrm{k}-1}{\mathit{P}}_{k-1}{\mathit{\Phi }}_{\mathrm{k},\mathrm{k}-1}^{\mathrm{T}}+{\mathit{Q}}_{\mathrm{k}-1}$$

(29)

where ${\mathit{Q}}_{\mathrm{k}-1}$ is the process noise covariance.

According to the Kalman filter theory, ${\mathit{P}}_{\mathrm{k}/\mathrm{k}-1}$ can also be estimated by the innovation sequence as follows

$${\mathit{\epsilon }}_{\mathrm{k}}{\mathit{\epsilon }}_{\mathrm{k}}^{\mathrm{T}}={\mathit{H}}_{\mathrm{k}}{\mathit{P}}_{\frac{\mathrm{k}}{\mathrm{k}-1}}{\mathit{H}}_{\mathrm{k}}^{\mathrm{T}}+{\mathit{R}}_{\mathrm{k}}$$

(30)

where $R_{\mathrm{k}}$ is the measurement noise covariance.

Using the above equations, the measurement noise matrix at time k is

$${\mathit{R}}_{\mathrm{k}}=\frac{1}{N}\sum _{\mathrm{k}-\mathrm{N}+1}^{\mathrm{k}}({\mathit{\epsilon }}_{\mathrm{k}}{\mathit{\epsilon }}_{\mathrm{k}}^{\mathrm{T}}-{\mathit{H}}_{\mathrm{k}}{\mathit{P}}_{\mathrm{k}/\mathrm{k}-1}{\mathit{H}}_{\mathrm{k}}^{\mathrm{T}})$$

(31)

where N is the number of samples.

Then, the measurement noise covariance matrix can be estimated online, and the unexpected bias on the odometer and electric compass can be detected.

In the MIMU/WSS/SAS-integrated navigation system, the MIMU measures the acceleration and angular velocity in the body frame at the time of k and updates the attitude, velocity, and position after switching to the navigation frame. Simultaneously, the car ESP system reads the WSS and SAS output, which are converted to the vehicle speed and front wheel angle. The WSS and SAS provide the following two observations for the combined navigation system: (1) the difference in speed of the WSS output and INS output in the n frame; (2) the difference between the wheel angle and the front wheel angle, which is calculated using the inertial output. The AKF calculates the gain matrix by observing the vector and error model and estimates the error to constantly correct the attitude, velocity, and position during the update process. The recursive principle of AKF is shown in Figure 2.^10,19

Figure 2.

Flow chart of the adaptive Kalman filter.

Experimental testing

To test the accuracy of MIMU/WSS/SAS-integrated navigation system, the actual vehicle experiment was performed. The trajectory of the actual vehicle experiment is shown in Figure 3. A Xiens MTi-30 sensor and Vector CAN bus test equipment were installed on the vehicle, and the installation of each device is shown in Figure 4. MTi-30 is an attitude-heading reference system with an internal integrated tri-axial accelerometer, a tri-axial gyroscope, and a tri-axial magnetometer. The MTi-30 can produce the original data of the accelerometer, gyroscope, and filtered posture information. The MTi-30 performance specifications are shown in Table 1. The Vector bus test equipment includes CANoe, CANcope, and CANStreessDR. In the measurement process, the sampling frequency of MTi is 100 Hz, the sampling frequency of SAS and WSS is 50 Hz, and the frequency of the filter update is 50 Hz.

Figure 3.

Path taken by the vehicle.

Figure 4.

Installation of the equipment in the vehicle.

Table 1.

Specifications of MTi-30. ²⁰

Gyroscopes
In-run bias stability	18°/h
Noise density	0.03°/s/√Hz
Non-linearity	0.03% FS
Accelerometers
In-run bias stability	40 μg
Noise density	80 μg/√Hz
Non-linearity	0.03% of FS
Attitude accuracy (dynamic)
Roll/pitch	<0.4°
Yaw	<1°

The MTi-30 determines the vehicle attitude using an accelerometer, a gyroscope, and a magnetometer and removes the magnetic interference of the vehicle before performing the attitude measurement. The attitude of the vehicle in the quiescent state as the result of the initial alignment, latitude, and longitude of the starting position was known, and the height was set to 0 m. The WSS is mounted on the front wheel, and its final output data take the average of the right and left wheel speeds. The SAS was installed in the steering column below the steering wheel with the steering column coaxial installation, and the output data were the steering wheel angle data. The MTi sensor and CANoe were connected to the same computer by USB. The corresponding PC software programs were MT Manager and Vector CANape, respectively. To ensure the synchronization of the sensor measurements, the system time was added to the measured data as the time axis. After the experimental testing, the data are sent to MATLAB for offline simulation, and the experimental results are shown in Figures 5, 6, and 7. During the testing, the total vehicle driving time was 150 s, and the driving distance was approximately 1 km.

Figure 5.

Attitude estimation results of three different navigation systems.

Figure 6.

Speed estimation results of different navigation systems.

Figure 7.

Navigation results of the integrated navigation system.

Figure 5 shows the results of three different navigation modes to estimate the vehicle attitude. The blue line shows the output curve of MTi. The red line shows the vehicle attitude angle output of the SINS. The black line shows the MIMU/WSS/SAS-integrated navigation system to the vehicle attitude estimation results. Table 2 shows the contraction of the absolute value of maximum attitude estimation error in three navigation methods. The results show that the MIMU/WSS/SAS-integrated navigation system can well estimate the vehicle roll angle and pitch angle. The output error is within 0.5° and 1° compared with MTi and SINS, respectively, and the yaw angle of the estimated error is controlled within 1°. The speed estimation results of different navigation systems are shown in Figure 6. The blue line shows the output curve of the GPS, which serves as a reference curve. The red line represents the vehicle speed output of the SINS. The black line shows the estimation of the vehicle speed using the MIMU/WSS/SAS-integrated navigation system. The speed estimation of the MIMU/WSS/SAS-integrated navigation system is clearly closer to the actual value. Hence, the MIMU/WSS/SAS-integrated navigation system has a higher accuracy in the speed estimation; therefore, the navigation and positioning accuracy are improved.

Table 2.

Absolute value of the maximum attitude estimation error.

Contrast mode	Pitch angle (°)	Roll angle (°)	Yaw angle (°)
The absolute value of maximum attitude estimation error between MIMU/WSS/SAS and MTi (°)	0.43	0.48	0.79
The absolute value of maximum attitude estimation error between MIMU/WSS/SAS and SINS (°)	0.89	0.92	0.95

MIMU: micro-electro-mechanical system; WSS: wheel speed sensor; SAS: steering wheel angle sensor; SINS: strapdown inertial navigation system.

Figure 7 compares the results of the MIMU/WSS/SAS-integrated navigation system data, SINS data, MIMU/WSS-integrated navigation system data, and real path route data. The real path route is measured by the GPS and can be used as a reference route. As shown in Figure 6, the method of using MIMU output data to directly predict the path has better accuracy in the first 20 s. However, because of the accumulated MIMU measurement error over time, rapid divergence appears, which affects its accuracy. Table 3 shows the positioning error using the pure INS and integrated navigation system within 150 s, where the east error rate decreases by 92.25% and 94.67% using the MIMU/WSS-integrated navigation and MIMU/WSS/SAS, respectively, and the error rate increases by 2.42% compared to that without the SAS. Thus, the use of the SAS can effectively improve the positioning accuracy. The north error rate decreases by 98.65% and 98.88% using the MIMU/WSS-integrated navigation and MIMU/WSS/SAS, respectively, compared to that without the SAS; the error rate increases by 0.23%, and the horizon error-reduction rate also increases. Considering the three directions of ENU; the error-reduction rate of MIMU/WSS/SAS is 95.68% higher than SINS, which proves that the MIMU/WSS/SAS can provide more accurate and completely autonomous navigation for vehicles. In addition, the combination of the MIMU/WSS/SAS-integrated navigation mode also improves the positioning accuracy compared to the MIMU/WSS-integrated navigation, which shows that the steering wheel sensor and WSS can assist the INS with better navigation effect.

Table 3.

Positioning error of the MIMU/WSS/SAS.

Navigation method	Direct calculate	MIMU/WSS	Error-reduction rate (%)	MIMU/WSS/SAS	Error-reduction rate (%)
East error (m)	74.55	5.78	92.25	3.96	94.67
North error (m)	537.42	7.26	98.65	6.01	98.88
Horizon error (m)	850.61	18.31	97.85	17.97	97.88

MIMU: micro-electro-mechanical system; WSS: wheel speed sensor; SAS: steering wheel angle sensor.

Conclusion

When the low-cost INS with the MIMU is used for the SINS, considering the positioning accuracy of such INSs and the fact that the gyroscope cannot eliminate the integral error in the process of using the gyroscope, an MIMU/WSS/SAS-integrated navigation system is proposed in this article. Based on the new observations provided by the sensor, the system error-state model and observation equation are deduced, and the effect of the complex situation on the measurement noise in the actual driving process is considered. The AKF algorithm is designed to integrate the navigation system. This filtering method can estimate the measurement noise in real time, so it can correct the data in the navigation process in real time and realize online estimation. The real vehicle experiment shows that the MIMU/WSS/SAS-integrated navigation system can provide higher positioning navigation accuracy than the INS and the integrated navigation method of odometer and INS. The WSS and SAS are the necessary sensors for the vehicle ESP and other systems compared to the GPS, which has the disadvantages that the signals are vulnerable to block and shield. Such sensor information is completely independent of the information source; therefore, the proposed method of the integrated navigation system can provide better vehicle status estimation and navigation information for the vehicle autonomous navigation, unmanned systems, anti-collision systems, and so on.