Accurate Attitude Estimation Using ARS under Conditions of Vehicle Movement Based on Disturbance Acceleration Adaptive Estimation and Correction

Abstract

This paper describes a disturbance acceleration adaptive estimate and correction approach for an attitude reference system (ARS) so as to improve the attitude estimate precision under vehicle movement conditions. The proposed approach depends on a Kalman filter, where the attitude error, the gyroscope zero offset error and the disturbance acceleration error are estimated. By switching the filter decay coefficient of the disturbance acceleration model in different acceleration modes, the disturbance acceleration is adaptively estimated and corrected, and then the attitude estimate precision is improved. The filter was tested in three different disturbance acceleration modes (non-acceleration, vibration-acceleration and sustained-acceleration mode, respectively) by digital simulation. Moreover, the proposed approach was tested in a kinematic vehicle experiment as well. Using the designed simulations and kinematic vehicle experiments, it has been shown that the disturbance acceleration of each mode can be accurately estimated and corrected. Moreover, compared with the complementary filter, the experimental results have explicitly demonstrated the proposed approach further improves the attitude estimate precision under vehicle movement conditions.

1. Introduction

An Attitude Reference System (ARS), which is widely applied to estimate the attitude of a vehicle [1,2] or the human body [3], is composed of orthogonally oriented sensors including a 3-axis gyroscope and 3-axis accelerometer. Typically speaking, the attitude is determined by integrating the gyroscope output. However, due to the existence of MEMS gyroscope zero offset error and bias instability, an accurate attitude estimation cannot be obtained by simple calculation using MEMS gyroscopes, which can only be utilized for a short time span to calculate attitude. As MEMS accelerometers are sensitive to the gravitational acceleration and can thus directly provide inclination information [4], the drifted attitude estimate error resulting from MEMS gyroscope data integration can be effectively corrected by fusing the output of a MEMS gyroscope with that of a MEMS accelerometer. Thus, a series of algorithms, which can be divided into three kinds, are studied for vehicle attitude estimation. One of the kinds are those algorithms based on vector observations [5,6,7,8,9], such as the TRIAD algorithm [6], QUEST algorithm [7], FOAM algorithm [8] and SVD algorithm [9]. These algorithms are deduced according to some specific physical or geometric significance. When these algorithms are used in ARS under vehicle movement conditions, the attitude estimate accuracy would be influenced by the MEMS accelerometer measurement errors [10]. In practice, multiple applications use complementary filters by fusing the output of a MEMS gyroscope and a MEMS accelerometer [11,12,13,14,15,16]. In earlier research, the attitude estimation system for an autonomous helicopter used a complementary filter obtained by fusing the output of a gyroscope and an inclinometer [15]. The attitude measuring principle of the inclinometer is similar to that of an accelerometer. The used of a complementary filter for determining the attitude was also applied in a fixed-wing UAV [16]. In these complementary filters, the output of the accelerometer or the inclinometer approximately equals the gravitational acceleration and does not consider the other component—the disturbance acceleration. As the output of the accelerometer is the sum of gravitational and inertial linear accelerations as mentioned in reference [17], and only the first component—the gravitational acceleration—is useful for ARS to determine the attitude, we name the second component—the inertial linear acceleration—the disturbance acceleration. Thus, when the ARS is stationary or moves with approximately uniform linear motion, the three components of the gravity acceleration are obtained and the disturbance acceleration is approximately zero, and then a ‘weak disturbance acceleration’ assumption is made in the filter. The filter fails, however, when vehicle movements are sufficiently strong that the accelerometer output no longer provides a good estimate of the gravitational direction [18]. When the vector observation algorithm or the complementary filter cannot take advantage of the data of the past and estimate sensor errors, this could be resolved by the state estimation algorithm [19,20,21,22,23,24,25,26]. A Kalman Filter is designed to estimate the orientation of human body segments by fusing gyroscope, accelerometer and magnetometer data [19]. In the state estimation algorithm, it could estimate not only the attitude, but also some uncertain error parameters (e.g., gyroscope offset errors), which reduces the impacts of uncertain error parameters on the attitude estimation. State variables of the state estimation algorithm contain attitude parameters as well as other variables for estimating attitude, and state equations are established in accordance with the kinematic differential equation. Moreover, in terms of the problems of nonlinearity in the ARS, there are a variety of nonlinear filter algorithms [22,23,24,25,26]. The Extended Kalman Filter (EKF) is the most widely-used nonlinear filter used in the ARS [23,24,25].

The state estimation algorithm based on Kalman Filter or nonlinear filters could estimate some uncertain parameters caused by some disturbance or sensor errors and improve the attitude precision to some extent. Nonetheless, how to effectively fuse the output of the gyroscope and accelerometer is still a problem when disturbance acceleration exists. In recent works, the threshold value of the disturbance acceleration to identify the acceleration level is utilized to isolate the disturbance acceleration. Based on the characteristics of gyros and accelerometers, the threshold values are extracted experimentally [14] or designed by fuzzy logic [26]. However, the threshold setting has a tight connection with the MEMS accelerometer performance. If the MEMS accelerometer has a large random noise, the threshold would be set at a bigger value. Thus, the disturbance acceleration could not be detected, and then the attitude and gyroscope zero offset would be estimated incorrectly. However, if the threshold is set as a small value, it would weaken the correcting effect of the accelerometer data on the gyroscope zero offset and the attitude. As a result, the attitude estimate would diverge. In other words, these existing attitude estimate algorithms directly use the gravity vector superposed the disturbance acceleration as the aid information for estimating the attitude, so their attitude estimate precision would be greatly influenced by the set threshold and the vehicle movement.

To cope with the abovementioned problems, this paper adds the disturbance acceleration error into the state vectors, and puts forward a disturbance acceleration adaptive estimate and correction approach based on a nine state vector Kalman filter that yields robust attitude estimate performance, even when the vehicle is accelerated in different modes—non-acceleration, vibration-acceleration and sustained-acceleration. In the filter, the attitude error, the gyroscope zero offset error and the disturbance acceleration error are all estimated. The disturbance acceleration is modeled as a first-order Markov Process, which is adaptively estimated by switching the filter decay coefficient for the disturbance acceleration Markov process. By estimating and correcting the disturbance acceleration, the observation accuracy of the gravity vector estimated by the accelerometer is effectively improved through deducting the disturbance acceleration from the accelerometer output, thus improving the roll and pitch estimate precision. Hence, the proposed approach produces robust estimates of roll and pitch. To verify the performance of the proposed approach, it was tested in three different acceleration modes (non-acceleration, vibration-acceleration and sustained-acceleration mode) by digital simulation. Kinematic vehicle experiments were also designed, and the estimation results were compared with the outputs of the on-board MEMS ARS (SBG Ellipse-N). By utilizing digital simulations and the kinematic vehicle experiments under conditions where disturbance acceleration exists, a significantly more accurate attitude estimate is obtained using the proposed approach compared with the complementary filter [14]. The comparison explicitly demonstrates that more accurate roll and pitch estimation performance can be achieved through the proposed disturbance acceleration adaptive estimate and correction approach based on a Kalman filter.

2. Attitude Estimation Based on the Disturbance Acceleration Adaptive Estimate and Correction

Figure 1 refers to the attitude estimation framework based on the disturbance acceleration adaptive estimate and correction.

It includes four parts:

• Calculation of the measurement error vector, that is the difference of gravity vector estimated by the gyroscope and accelerometer, respectively;
• Estimation of attitude error, gyroscope zero offset error as well as the disturbance acceleration error;
• Correction the estimate of the disturbance acceleration, the gyroscope zero offset and the attitude;
• Adaptive adjustment of the filter decay coefficient of the disturbance acceleration model.

2.1. Model of the Error Process

In this paper, the state vector of the Kalman filter is designed as ${\mathit{x}}_{\epsilon }=\left[{\gamma }_{\epsilon }{p}_{\epsilon }{\phi }_{\epsilon }{\epsilon }_{b_{\epsilon }x}{\epsilon }_{b_{\epsilon }y}{\epsilon }_{b_{\epsilon }z}{a}_{\epsilon x}^B{a}_{\epsilon y}^B{a}_{\epsilon z}^B\right],$ where the first three entries represent Euler angle error θ_ε including roll error γ_ε, pitch error p_ε and yaw error ${\phi }_{\epsilon }$. Additionally, ${\epsilon }_{b_{\epsilon }}={\left[{\epsilon }_{b_{\epsilon }x},{\epsilon }_{b_{\epsilon }y},{\epsilon }_{b_{\epsilon }z}\right]}^T$ represents the gyroscope zero offset error and $a_{\epsilon }^B={\left[a_{\epsilon x}^B,a_{\epsilon y}^B,a_{\epsilon z}^B\right]}^T$ represents the disturbance acceleration estimate error on body frame. In the indirect Kalman filter utilized for sensor fusion, the error process x_ε updates recursively in Equation (1). Matrix A_k and the noise component w_k describe the propagation of the a priori error state vector. In this filter structure, the knowledge concerning previous errors is incorporated in the current state estimate. Consequently, there is no correlation between the a priori estimated errors between two time steps, meaning that the a priori errors do not depend on previous error states. Thus, the matrix A_k is expressed as A_k = 0_9×9. Moreover, the a priori estimate of the state vector ${\widehat{\mathit{x}}}_{\epsilon ,k/k-1}$ is abbreviated as Equation (2), while the a posteriori estimate of the state vector ${\widehat{\mathit{x}}}_{\epsilon ,k}$ is abbreviated as Equation (3), where Z_ε,k refers to the measurement error:

$${\mathit{x}}_{\epsilon ,k}={\mathit{A}}_k{\mathit{x}}_{\epsilon ,k-1}+{\mathit{w}}_k$$

(1)

$${\widehat{\mathit{x}}}_{\epsilon ,k/k-1}=\mathbf{0}$$

(2)

$${\widehat{\mathit{x}}}_{\epsilon ,k}={\widehat{\mathit{x}}}_{\epsilon ,k/k-1}+{\mathit{K}}_k\left({\mathit{Z}}_{\epsilon ,k}-{\mathit{H}}_k{\widehat{\mathit{x}}}_{\epsilon ,k/k-1}\right)={\mathit{K}}_k{\mathit{Z}}_{\epsilon ,k}$$

(3)

2.2. Model of the Measurement Error Process

To calculate the a posteriori estimate of the state vector in Equation (3), the model of the measurement error process is defined. Meanwhile, the measurement vector is selected as the difference of the gravity vector estimate on the body frame (front-right-down) from the accelerometer and gyroscope, which is a 3 × 3 vector shown in Equation (4):

$${\mathit{Z}}_k={\widehat{\mathit{g}}}_{A,k/k-1}^B-{\widehat{\mathit{g}}}_{G,k/k-1}^B$$

(4)

$${\widehat{\mathit{g}}}_{A,k/k-1}^B=-{\mathit{f}}_k^B+{\widehat{\mathit{a}}}_{k/k-1}^B$$

(5)

$${\mathit{a}}_k^N=c_a{\mathit{a}}_{k-1}^N+{\mathit{w}}_{a,k}$$

(6)

$${\widehat{\mathit{a}}}_{k/k-1}^N=c_a{\widehat{\mathit{a}}}_{k-1}^N$$

(7)

$${\widehat{\mathit{a}}}_{k/k-1}^B={\widehat{\mathit{C}}}_{N,k/k-1}^B{\widehat{\mathit{a}}}_{k/k-1}^N=c_a\mathsf{\Delta }{\mathit{C}}_N^B\left({\widehat{\mathit{\omega }}}_{k/k-1}T\right){\widehat{\mathit{C}}}_{N,k-1}^B{\widehat{\mathit{a}}}_{k-1}^N\approx c_a\mathsf{\Delta }{\mathit{C}}_N^B\left({\widehat{\mathit{\omega }}}_{k/k-1}T\right){\widehat{\mathit{a}}}_{k-1}^B\approx c_a{\widehat{\mathit{a}}}_{k-1}^B$$

(8)

$${\widehat{\mathit{a}}}_k^B={\widehat{\mathit{a}}}_{k-1}^B-{\widehat{\mathit{a}}}_{\epsilon ,k}^B$$

(9)

In Equation (4), ${\widehat{\mathit{g}}}_{A,k/k-1}^B$ refers to the gravity vector estimate on body frame calculated from the accelerometer. The calculation process is shown in Equation (5), where ${\mathit{f}}_k^B$ denotes the output of the accelerometer at t_k and ${\widehat{\mathit{a}}}_{k/k-1}^B$ denotes the a priori estimate of the disturbance acceleration at t_k. To isolate the disturbance acceleration and estimate the disturbance acceleration, the disturbance acceleration in the N (north-east-down) navigation frame is modelled as a first-order Markov process according to Equation (6) [27], where c_a is the filter decay coefficient for the disturbance acceleration Markov process and w_a,k is a zero mean white Gaussian noise process. Hence, the relationship of the a priori estimate of the disturbance acceleration (in the navigation frame N) and the a posteriori estimate (in the navigation frame N) of the previous iteration is shown in Equation (7). Then, the relationship of ${\widehat{\mathit{a}}}_{k/k-1}^B$ and ${\widehat{\mathit{a}}}_{k-1}^B$ is displayed in Equation (8) and ${\widehat{\mathit{a}}}_{k-1}^B$ is calculated by Equation (9), which is the a posteriori estimate of the disturbance acceleration on body frame at t_k−1. In practice, the incremental orientation cosine matrix approximates $\mathsf{\Delta }{C}_N^B\left({\widehat{\mathit{\omega }}}_{k/k-1}T\right)\approx {\mathit{I}}_{3\times 3}$. ${\widehat{\mathit{\omega }}}_{k/k-1}$ refers to the a priori angular velocity estimate; T refers to the Kalman Filter interval; ${\widehat{\mathit{C}}}_{N,k/k-1}^B$ refers to the a priori estimate of the orientation cosine matrix at t_k; ${\widehat{\mathit{C}}}_{N,k-1}^B$ refers to the a posteriori estimate of the orientation cosine matrix at t_k−1; ${\widehat{a}}_{\epsilon }^B={\left[{\widehat{a}}_{\epsilon x}^B,{\widehat{a}}_{\epsilon y}^B,{\widehat{a}}_{\epsilon z}^B\right]}^T$ refers to the a posteriori disturbance acceleration estimate error in the body frame. Substituting the calculated ${\widehat{\mathit{a}}}_{k/k-1}^B$ into Equation (5), ${\widehat{\mathit{g}}}_{A.k/k-1}^B$ is calculated.

$${\widehat{\mathit{g}}}_{G,k/k-1}^B={\widehat{\mathit{C}}}_{N,k/k-1}^B{\mathit{g}}^N$$

(10)

In Equation (4), ${\widehat{\mathit{g}}}_{G,k/k-1}^B$ represents the gravity vector estimate on the body frame calculated from the gyroscope. The calculation process can be shown in Equation (10), where g^N is the gravity vector in the navigation frame N. ${\widehat{\mathit{C}}}_{N,k/k-1}^B$ is calculated by the a priori estimate of the quaternion ${\widehat{q}}_{k/k-1}$.

$${\mathit{Z}}_{\epsilon ,k}={\widehat{\mathit{g}}}_{A,k/k-1}^B-{\widehat{\mathit{g}}}_{G,k/k-1}^B={\mathit{H}}_k{\widehat{\mathit{x}}}_{\epsilon ,k/k-1}+{\mathit{v}}_k={\mathit{H}}_k\left(\begin{array}{c}{\mathit{\theta }}_{\epsilon }\\ {\mathit{\epsilon }}_{b_{\epsilon }}\\ {\mathit{a}}_{\epsilon }^B\end{array}\right)+{\mathit{v}}_k$$

(11)

$${\mathit{H}}_k=\left(\begin{array}{ccccccccc}0& {\widehat{g}}_{Gz,k/k-1}^B& -{\widehat{g}}_{Gy,k/k-1}^B& 0& -{\widehat{g}}_{Gz,k/k-1}^{B}T& {\widehat{g}}_{Gy,k/k-1}^{B}T& 1& 0& 0\\ -{\widehat{g}}_{Gz,k/k-1}^B& 0& {\widehat{g}}_{Gx,k/k-1}^B& {\widehat{g}}_{Gz,k/k-1}^{B}T& 0& -{\widehat{g}}_{Gx,k/k-1}^{B}T& 0& 1& 0\\ {\widehat{g}}_{Gy,k/k-1}^B& -{\widehat{g}}_{Gx,k/k-1}^B& 0& -{\widehat{g}}_{Gy,k/k-1}^{B}T& {\widehat{g}}_{Gx,k/k-1}^{B}T& 0& 0& 0& 1\end{array}\right)$$

(12)

Equation (11) gives the relationship between the measurement vector and the state vector, and the measurement matrix H_k can be expressed as Equation (12).

2.3. State Errors Estimate and Correction Process

Combining Equations (2) and (11), Equation (13) gives the estimate process of the attitude error, the gyroscope zero offset error and the disturbance acceleration error on body frame. As shown in (13), the a priori 9 × 9 error covariance matrix P_k/k−1 is equal to the state noise matrix Q_k in the filter model, which is different from the conventional Kalman filter. In Equation (13), expanding Q_k = P_k/k−1 gives Equation (14). Through further deduction, we can find that Q_k could be updated for the next iteration as a function of the a posteriori covariance matrix P_k−1 shown in Equation (15), in which $Q_{w{\epsilon }_b}{\mathit{I}}_{3\times 3}$ denotes the noise matrix of the gyroscope zero offset white noise, Q_wbI_3×3 denotes the noise matrix of the gyroscope white noise and Q_waI_3×3 denotes the noise matrix of the disturbance acceleration white noise shown in Equation (16). From Equation (14), we can see that the relationship among the attitude error, the gyroscope zero offset error and the disturbance acceleration error is reflected in the updated process of Q_k, so A_k = 0_9×9 is reasonable, although it is different from the conventional Kalman filter. Equation (17) gives the calculation process of the measurement noise matrix R_k, where Q_vAI_3×3 refers to the noise matrix of the accelerometer white noise.

$$\{\begin{array}{l}{\widehat{\mathit{x}}}_{\epsilon ,k/k-1}=\mathbf{0}\\ {\mathit{P}}_{k/k-1}={\mathit{A}}_k{\mathit{P}}_{k-1}{\mathit{A}}_k^T+{\mathit{Q}}_k={\mathit{Q}}_k\\ {\mathit{K}}_k={\mathit{P}}_{k/k-1}{\mathit{H}}_k^T{\left({\mathit{H}}_k{\mathit{P}}_{k/k-1}{\mathit{H}}_k^T+{\mathit{R}}_k\right)}^{-1}={\mathit{Q}}_k{\mathit{H}}_k^T{\left({\mathit{H}}_k{\mathit{Q}}_k{\mathit{H}}_k^T+{\mathit{R}}_k\right)}^{-1}\\ {\widehat{\mathit{x}}}_{\epsilon ,k}={\mathit{K}}_k{\mathit{Z}}_{\epsilon ,k}\\ {\mathit{P}}_k=\left(\mathit{I}-{\mathit{K}}_k{\mathit{H}}_k\right){\mathit{P}}_{k/k-1}=\left(\mathit{I}-{\mathit{K}}_k{\mathit{H}}_k\right){\mathit{Q}}_k\end{array}$$

(13)

$${\mathit{Q}}_k={\mathit{P}}_{k/k-1}=E\left[\begin{array}{ccc}{\widehat{\mathit{\theta }}}_{\epsilon ,k/k-1}{\left({\widehat{\mathit{\theta }}}_{\epsilon ,k/k-1}\right)}^T& {\widehat{\mathit{\theta }}}_{\epsilon ,k/k-1}{\left({\widehat{\mathit{\epsilon }}}_{b_{\epsilon },k/k-1}\right)}^T& {\widehat{\mathit{\theta }}}_{\epsilon ,k/k-1}{\left({\mathit{a}}_{\epsilon ,k/k-1}^B\right)}^T\\ {\widehat{\mathit{\epsilon }}}_{b_{\epsilon },k/k-1}{\left({\widehat{\mathit{\theta }}}_{\epsilon ,k/k-1}\right)}^T& {\widehat{\mathit{\epsilon }}}_{b_{\epsilon },k/k-1}{\left({\widehat{\mathit{\epsilon }}}_{b_{\epsilon },k/k-1}\right)}^T& {\widehat{\mathit{\epsilon }}}_{b_{\epsilon },k/k-1}{\left({\mathit{a}}_{\epsilon ,k/k-1}^B\right)}^T\\ {\mathit{a}}_{\epsilon ,k/k-1}^B{\left({\widehat{\mathit{\theta }}}_{\epsilon ,k/k-1}\right)}^T& {\mathit{a}}_{\epsilon ,k/k-1}^B{\left({\widehat{\mathit{\epsilon }}}_{b_{\epsilon },k/k-1}\right)}^T& {\mathit{a}}_{\epsilon ,k/k-1}^B{\left({\mathit{a}}_{\epsilon ,k/k-1}^B\right)}^T\end{array}\right]$$

(14)

$$\begin{array}{l}{\mathit{Q}}_k=f({\mathit{P}}_{k-1})=\left[\begin{array}{ccc}{\mathit{Q}}_{1k}& {\mathit{Q}}_{2k}& {\mathbf{0}}_{3\times 3}\\ {\mathit{Q}}_{2k}& {\mathit{Q}}_{3k}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathit{Q}}_{4k}\end{array}\right]\\ \{\begin{array}{c}\begin{array}{l}{\mathit{Q}}_{1k}={\mathit{P}}_{k-1}[1,3:1,3]+T^2\left[{\mathit{P}}_{k-1}[4,6:4,6]+\left(Q_{w{\epsilon }_b}+Q_{wb}\right){\mathit{I}}_{3\times 3}\right]\\ {\mathit{Q}}_{2k}=-T({\mathit{P}}_{k-1}[1,3:4,6]+Q_{w{\epsilon }_b}{\mathit{I}}_{3\times 3})\\ {\mathit{Q}}_{3k}={\mathit{P}}_{k-1}[4,6:4,6]+Q_{w{\epsilon }_b}{\mathit{I}}_{3\times 3}\\ {\mathit{Q}}_{4k}=c_a{\mathit{P}}_{k-1}[7,9:7,9]+Q_{wa}{\mathit{I}}_{3\times 3}\end{array}\end{array}\end{array}$$

(15)

$$\{\begin{array}{c}\begin{array}{l}{Q}_{w{\epsilon }_b}{\mathit{I}}_{3\times 3}=E\left[{\mathit{w}}_{{\epsilon }_{b,k}}{\left({\mathit{w}}_{{\epsilon }_{b,k}}\right)}^T\right]\\ Q_{wb}{\mathit{I}}_{3\times 3}=E\left[{\mathit{w}}_{b,k}{\left({\mathit{w}}_{b,k}\right)}^T\right]\\ Q_{vA}{\mathit{I}}_{3\times 3}=E\left[{\mathit{v}}_{A,k}{\left({\mathit{v}}_{A,k}\right)}^T\right]\\ Q_{wa}{\mathit{I}}_{3\times 3}=E\left[{\mathit{w}}_{a,k}{\left({\mathit{w}}_{a,k}\right)}^T\right]\end{array}\end{array}$$

(16)

$${\mathit{R}}_k=\left[Q_{vA}+Q_{wa}+T^2\left(Q_{w{\epsilon }_b}+Q_{wb}\right)\right]{\mathit{I}}_{3\times 3}$$

(17)

By estimating state errors, states in relation to attitude estimate are corrected further, so as to improve the attitude estimate precision in ARS. State corrections include the attitude correction (the orientation cosine matrix correction or the quaternion correction), the output of gyroscope correction as well as the disturbance acceleration correction on body frame. The state correction process is given by Equations (18) and (19). In MEMS ARS, the velocity and the position cannot be calculated. Beyond that, the a priori MEMS gyroscope correction estimate ${\widehat{\mathit{\omega }}}_{k/k-1}^B\gg {\widehat{\mathit{C}}}_{N,k/k-1}^B({\widehat{\mathit{\omega }}}_{IE,k/k-1}^N+{\widehat{\mathit{\omega }}}_{EN,k/k-1}^N)$, where ${\widehat{\mathit{\omega }}}_{IE,k/k-1}^N$ denotes the a priori estimate of the earth’s rotation angular in the navigation frame and ${\widehat{\mathit{\omega }}}_{EN,k/k-1}^N$ denotes the a priori estimate of rotation angular from the navigation frame to the geocentric Earth frame. Thus, the a priori angular velocity estimate ${\widehat{\mathit{\omega }}}_{k/k-1}$ approximately equals the a priori MEMS gyroscope correction estimate ${\widehat{\mathit{\omega }}}_{k/k-1}^B$.

$$\begin{array}{l}{\widehat{q}}_k={\widehat{q}}_{k/k-1}\mathsf{\Delta }q\left(-{\widehat{\mathit{\varphi }}}_{\epsilon ,k}\right)\\ {\widehat{q}}_{k/k-1}={\widehat{q}}_{k-1}\mathsf{\Delta }q\left({\widehat{\mathit{\omega }}}_{k/k-1}T\right)\\ {\widehat{\mathit{\omega }}}_{k/k-1}={\widehat{\mathit{\omega }}}_{k/k-1}^B-{\widehat{\mathit{C}}}_{N,k/k-1}^B({\widehat{\omega }}_{IE,k/k-1}^N+{\widehat{\omega }}_{EN,k/k-1}^N)\approx {\widehat{\mathit{\omega }}}_{k/k-1}^B\\ {\widehat{\mathit{\omega }}}_{k/k-1}^B={\mathit{\omega }}_k^B-{\widehat{\mathit{\epsilon }}}_{b,k/k-1}\\ {\widehat{\mathit{\epsilon }}}_{b,k/k-1}={\widehat{\mathit{\epsilon }}}_{b,k-1}\\ {\widehat{\mathit{\epsilon }}}_{b,k}={\widehat{\mathit{\epsilon }}}_{b,k/k-1}-{\widehat{\mathit{\epsilon }}}_{b_{\epsilon },k}={\widehat{\mathit{\epsilon }}}_{b,k-1}-{\widehat{\mathit{\epsilon }}}_{b_{\epsilon },k}\end{array}$$

(18)

Equation (18) gives the process of the attitude correction and the gyroscope output correction, where ${\widehat{q}}_{k/k-1}$ is the a priori estimate of the attitude quaternion, while ${\widehat{q}}_k$ is the a posteriori estimate of the attitude quaternion. ${\widehat{\mathit{\omega }}}_k^B$ represents the MEMS gyroscope output, ${\widehat{\mathit{\epsilon }}}_{b.k}$ represents the a posteriori estimate of the gyroscope zero offset, and ${\widehat{\mathit{\epsilon }}}_{b,k/k-1}$ represents the a priori estimate of the gyroscope zero offset.

$$\begin{array}{l}{\widehat{\mathit{a}}}_k^B={\widehat{\mathit{a}}}_{k/k-1}^B-{\widehat{\mathit{a}}}_{\epsilon ,k}^B\\ {\widehat{\mathit{a}}}_{k/k-1}^B=c_a{\widehat{\mathit{a}}}_{k-1}^B\end{array}$$

(19)

Equation (19) gives the process of the disturbance acceleration correction on body frame. The measurement error at t_k is computed by respectively substituting Equations (18) and (19) into Equations (5) and (10). Then, utilizing the computed measurement error and corrected state errors, the attitude estimate could be corrected by the filter given in Equation (13).

2.4. Adaptive Estimation of Disturbance Acceleration

If the disturbance acceleration was not estimated, the gravity vector estimate on body frame ${\widehat{\mathit{g}}}_{A,k/k-1}^B$ would equal the accelerometer output as shown in Equation (5), which could affect the observation accuracy of the gravity vector. The observation error would be distributed to the attitude error as well as gyroscope zero offset error, as shown in Equation (11), and then influence the attitude estimate accuracy. To solve the problem, the complementary filter weakens the disturbance acceleration effect by tuning the proportional and integral coefficients [14]. Nevertheless, the approach could not effectively improve the precision of the attitude estimate. Because it is hard to set the appropriate proportional and integral coefficients to balance disturbance acceleration influence and gyro offset error influence on the attitude estimation under conditions of vehicle movement. In the conventional Kalman filter without the acceleration estimate [23], when there is the disturbance acceleration, the measurement noise matrix R should be increased to reduce the filter gain and decrease the impact of innovation vector (the attitude estimate difference between accelerometer and gyroscope) on the attitude error. The approach is similar to the complementary filter, which could not effectively weaken the disturbance acceleration influence, especially when the vehicle accelerates for an extended period. Since the two filters did not model the disturbance acceleration for estimating, the observation accuracy of the gravity vector could not be effectively improved by deducting the disturbance acceleration from the accelerometer output, and meanwhile the disturbance acceleration would still disturb the attitude estimate.

In the disturbance acceleration estimate model in Equation (19), the decay filter coefficient c_a is proportional to the correlation time of the first-order Markov process as shown in Equation (20) and 0 < c_a < 1. If the disturbance acceleration changes in a low frequency, the correlation time is short and c_a would be set at a larger value. If the disturbance acceleration changes at a high frequency, which is similar to vibration conditions, c_a would be set as a smaller value. By switching c_a, the disturbance acceleration estimate would not only accord with the actual linear acceleration but also improve the observation accuracy of the gravity vector. In this way, it would improve the attitude estimate precision. As the measurement of the disturbance acceleration frequency is impractical, c_a is adaptively switched according to the norm of the disturbance acceleration a_k in the actual vehicle movement:

$${\mathit{a}}_k^N=c_a{\mathit{a}}_{k-1}^N+{\mathit{w}}_{a,k}=e^{-\frac{1}{\tau }}{\mathit{a}}_{k-1}^N+{\mathit{w}}_{a,k}$$

(20)

In the proposed approach, the value of the filter decay coefficient c_a is switched in accordance with the acceleration modes (non-acceleration, vibration-acceleration and sustained-acceleration mode) determined by the norm of the disturbance acceleration a_k, defined as Equation (21). In Equation (21) g refers to the norm of the gravity vector. Based on the characteristics of accelerometers, the threshold value a_threshold to identify the acceleration mode is extracted experimentally. In the proposed approach, a_threshold refers to the accelerometer noise mean-square deviation when the vehicle is static. What’s more, the computation process is shown in Equation (22), in which t_{total_static} signifies the total static time, while T_a signifies the sample period of the accelerometer:

$$\{\begin{array}{c}{a}_k=\left|\sqrt{f_x^B{(k)}^2+f_y^B{(k)}^2+f_z^B{(k)}^2}-g\right|\\ g=\sqrt{{\left({\mathit{g}}_x^N\right)}^2+{\left({\mathit{g}}_y^N\right)}^2+{\left({\mathit{g}}_z^N\right)}^2}\end{array}$$

(21)

$$\{\begin{array}{c}{a}_{threshold}=\sqrt{\frac{{\displaystyle \sum _{k=1}^n{\left(\sqrt{f_x^B{(k)}^2+f_y^B{(k)}^2+f_z^B{(k)}^2}-g\right)}^2}}{n}}\\ n=\frac{t_{total\_static}}{T_a}\end{array}$$

(22)

The switching logic for the filter decay coefficient c_a has the following scenarios:

• Non-acceleration mode: In this mode, the paper assumes that a_k < 3a_threshold. Owing to the little amplitude of the disturbance acceleration, and the acceleration estimate should decay in a short period in the model, namely τ in Equation (20), should be set as a small value. Thus, the filter decay coefficient is set as 0 < c_a ≤ 0.5.
• Vibration-acceleration mode: In this mode, the paper assumes that 3a_threshold ≤ a_k < 50a_threshold. With respect to the changes of the disturbance acceleration, the decay period τ of the acceleration estimate should be set longer than for mode 1. The filter decay coefficient is set as $c_a=0.5\times \frac{a_k-3a_{threshold}}{50a_{threshold}-3a_{threshold}}+\mathrm{0.5.}$ As shown in the formula, c_a is adaptively adjusted with respect to a_k.
• Sustained-acceleration mode: In this mode, the paper assumes that a_k ≥ 50a_threshold. As the vehicle is subject to strong and sustained acceleration, the filter decay coefficient c_a is set as 1.0.

The adaptive switch process of filter decay coefficient c_a is shown in Figure 2.

3. Experiments and Results

3.1. Digital Simulation Experiments and Results

The proposed disturbance acceleration adaptive estimate and correction approach based on switching the filter decay coefficient c_a in three different disturbance acceleration modes have been respectively tested by digital simulation and compared with the conventional complementary filter proposed in [14]. As switching proportional and integral coefficients in the complementary filter is similar to adjusting the noise matrix R explained in Section 2.4, the comparison is mainly between the proposed filter (Proposed) and the complementary filter (CF) in the digital and kinematic vehicle experiments. In the digital simulation experiments, the gyroscope and accelerometer error parameters can be set as shown in

Table 1. MEMS gyroscope and accelerometer error parameters for the digital simulation.

Error Parameters		Gyroscope	Accelerometer
First-order Gauss-Markov Process	Correlation Time	100 s	-
	White Noise	10 °/h	-
White Noise		0.1 °/s	5 mg

. The vehicle accelerates in the X axis on the body frame (front-right-down), which mainly influences the pitch estimate precision. In Figure 3a, Figure 4a and Figure 5a, the disturbance acceleration estimate is compared with the ideal disturbance acceleration in the X axis on the body frame and the time response for adaptively switching c_a is displayed in Figure 3b, Figure 4b and Figure 5b. It has been shown that the X axis disturbance acceleration on the body frame is estimated exactly by switching c_a adaptively. The time response for the pitch estimate comparison between the proposed filter (Proposed) and the complementary filter (CF) is plotted in Figure 3c, Figure 4c and Figure 5c. Moreover, the pitch estimate maximum error as well as root-mean-square (RMS) error are compared in

Table 2. Max and RMS pitch error in three acceleration modes.

Pitch Error (°)	Non-Acceleration		Vibration-Acceleration		Sustained-Acceleration
	Proposed	CF	Proposed	CF	Proposed	CF
MAX	1.65 × 10−2	5.95 × 10−2	4.63 × 10−1	1.28	8.44 × 10−1	2.98
RMS	3.18 × 10−2	1.26 × 10−1	1.95 × 10−1	6.12 × 10−1	5.32 × 10−1	2.12

. As revealed by simulations, the proposed method is better than the complementary filter in non-, vibration- and sustained-acceleration mode, especially in sustained-acceleration mode.

3.2. Kinematic Vehicle Experiments and Results

In this section, the kinematic vehicle experiment was designed and the outputs of the on-board MEMS ARS (SBG Ellipse-N shown in Figure 6) are set as standard comparison results, so as to effectively evaluate the proposed algorithm. The roll and pitch precision in the SBG Ellipse-N are both 0.2°, and MEMS gyroscope and accelerometer performance indexes can be seen in

Table 3. MEMS gyroscope and accelerometer performance index of the SBG Ellipse-N.

Performance Index	Gyroscope	Acceleration
Bias Instability	8 °/h	10 mg
Angle Random Walk	$0.18°/\sqrt{\mathrm{h}}$	-
Speed Random Walk	-	$200°\mathsf{\mu }\mathrm{g}/\sqrt{\mathrm{h}}(\mathrm{X},\mathrm{Y})$ $300°\mathsf{\mu }\mathrm{g}/\sqrt{\mathrm{h}}\left(\mathrm{Z}\right)$

. In the dynamic kinematic vehicle experiment, we also utilize the outputs of the MEMS gyroscopes and accelerometers from the SBG Ellipse-N as sensor data sources to validate the advantages of the proposed algorithm. In the approximately 400 s-long experiment, the vehicle uninterruptedly accelerated and decelerated as seen from Figure 7a–c and the filter decay coefficient c_a is adaptively switched as shown in Figure 8. Figure 7a–c shows that the disturbance acceleration could be estimated effectively and adaptively. Additionally, the roll and pitch estimate precision based on the proposed approach is higher than that of the complementary filter shown in Figure 9 and

Table 4. Max and RMS of roll and pitch error in the kinematic vehicle experiment.

	Roll Error (°)		Pitch Error (°)
	Proposed	CF	Proposed	CF
MAX	3.24 × 10−1	2.96	2.89 × 10−1	2.65
RMS	1.14 × 10−1	8.09 × 10−1	1.31 × 10−1	9.12 × 10−1

. Compared with the outputs of SBG Ellipse-N, Table 4 shows that the maximum estimate error of roll and pitch is approximately 0.3° and the RMS error of roll and pitch is approximately 0.1° using the proposed approach.

4. Conclusions

This paper has considered the problem of the influence of the disturbance acceleration on the roll and pitch estimate in MEMS ARS and put forward a disturbance acceleration adaptive estimate and correction approach. The proposed approach estimates the disturbance acceleration error in the nine state vectors Kalman filter, and adaptively estimates and corrects the disturbance acceleration combining with switching the filter decay coefficient. Even though the rule of the threshold value for selecting the ranges to identify acceleration mode is obtained experimentally based on the characteristics of accelerometers, it is still possible to make further performance improvements by tuning the ranges and adjusting the filter decay coefficient c_a in accordance with different vehicles and different acceleration characteristics. In this paper, the digital simulation and the kinematic vehicle experiment have demonstrated the good estimation characteristics of the disturbance acceleration. In the kinematic vehicle experiments with uninterruptedly accelerated and decelerated conditions, the algorithm gave a maximum error of 0.3° and RMS error of 0.1° in roll and pitch compared to the outputs of a high-precision SBG Ellipse-N, which is far better than the attitude estimation performance based on the complementary filter. The presented approach offers several advantages over other implementations:

• A disturbance acceleration estimation model based on a first-order Markov process is set up and the disturbance acceleration estimation error is estimated in the Kalman filter.
• In the acceleration estimate model, it analyses the relationship of the filter decay coefficient c_a and the correlation time τ of a first-order Markov process, which is the theoretical reason for switching c_a.
• The switching logic for c_a is designed in accordance with different disturbance acceleration modes.

Owing to these advantages, the disturbance acceleration is estimated and corrected accurately and adaptively. Thus, the disturbance acceleration influence on the attitude estimate is isolated effectively, which thereby improves the attitude estimate precision in ARS. Since the estimation results of digital simulations and the kinematic vehicle experiment are encouraging, a possible idea for future research is that the next experimental work will be tested in other vehicles (e.g., UAVs) so as to further verify the advantages of the proposed approach.