A Robust Indirect Kalman Filter Based on the Gradient Descent Algorithm for Attitude Estimation During Dynamic Conditions

The real-time response and accuracy of the attitude (i.e., roll and pitch) estimation from low-cost inertial measurement unit (IMU) have become the key issues restricting related applications. This paper proposes a robust attitude estimation scheme which can perform well under dynamic conditions. When only accelerometers are used to calculate and correct the attitude, the external acceleration becomes the main source of attitude estimation errors. Moreover, the truncation error in the linearization process of the nonlinear system also affects the attitude estimation. As our first contribution, the external acceleration is modeled as a first-order Gauss Markov model, and its value is calculated under the indirect Kalman filter (IKF) framework. The measurement noise covariance matrix of the IKF is adaptively adjusted to enhance its robustness and reduce the negative impact caused by inaccurate modeling. In the second part of our work, the two-step cascade filter method is used for attitude estimation. The attitude obtained from the gravity field based on the gradient descent (GD) algorithm shows fast response capabilities, and hence, it is embedded as a measurement in the IKF by using the chain-derivation rule. The truncation error introduced into the linearization process of the nonlinear system is effectively avoided. Both simulation and experiments are carried out to verify the feasibility and accuracy of the proposed algorithm. The results show that the approach proposed in this paper can meet the accuracy requirements of consumer products.


I. INTRODUCTION
An IMU consists of a three-axis accelerometer and a threeaxis gyroscope, which senses acceleration and angular velocity information in three orthogonal directions [1], [2]. The estimation of horizontal attitude angles using inertial sensors is an important research topic and has extraordinary application value in different fields such as platform stability control, vehicle maneuvering, Unmanned Aerial Vehicle (UAV), human-machine interaction et al. [3]- [5]. This working mode requires the navigation system to provide high precision horizontal attitude angles of the carrier, while the accuracy requirement for the heading angle is lower. Accelerometers The associate editor coordinating the review of this manuscript and approving it for publication was Lubin Chang . alone cannot sense rotations around the vertical axis, and they are often combined with other sensors, such as camera, light detection and ranging (LiDAR), magnetometers, and global positioning system (GPS), to provide 3D attitude estimation capabilities [6]- [8]. This is the Attitude and Heading Reference System (AHRS), in which the heading implies the yaw [9]. An important prerequisite for the multi-sensor integration with IMU as the core is the calibration of internal and external parameters. Reference [10]- [12] provide corresponding effective calibration work, respectively.
One of the key technologies focused on high-precision and real-time attitude estimation is the data fusion of multiple sensors. As an optimal recursive filter, the Kalman filter (KF) is widely used in linear system estimation [13]. Intuitively, it uses innovation to correct the prior estimate to obtain the VOLUME 8, 2020 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ posterior estimate. However, the process and measurement model of most attitude estimation systems are nonlinear. To this end, extended Kalman filter (EKF) and unscented Kalman filter (UKF) with the core idea of approximation are proposed with the characteristics of different degrees of linearization errors [14], [15]. Among them, EKF exploits the Taylor series approach to linearize the non-linear systems, and UKF uses prior knowledge to approximate samples. IKF scheme which uses the error state instead of the full state is widely welcomed by scholars to avoid the linearization process. Another important guideline for attitude estimation is proportional-integral-derivative (PID) control. Currently, the most commonly used methods are the none-KF approaches of Madgwick et al. [16] and Mahony et al. [17]. These two approaches are simpler to implement than any KFs, and Madgwick applied a gradient descent algorithm to obtain a corrected attitude from the gravity field. Mahony proposed complementary filters based on differential geometry tools on Lie Group SO (3). Although these two approaches show a very high computational efficiency, they are different from the IKF in that they use constant gain for the measurement updates, which may lead to incorrect estimates in the presence of external acceleration.
S. Zihajehzadeh et al. proposed a novel two-step cascade KF. In the first step, accelerometer data and gyroscope data are merged to estimate the tilt attitude, the estimated results are delivered to the second step in which the magnetometer data are combined to calculate the yaw [18]. Unfortunately, this method has the disadvantage of releasing quaternion normalization constraints and leads to large models regarding matrix operations. Another approach is to incorporate the GD algorithm into the KF [19], [20]. Nevertheless, neither the external acceleration interference nor the magnetic disturbance is well addressed in the dynamic environment.
The performance of the KF relies heavily on the prior knowledge of the noise covariance matrix and the measured noise covariance matrix. Using the wrong prior noise covariance matrix can generate estimation error and even cause filter divergence [21]. Huang proposed a series of reliable and accurate adaptive Kalman filtering methods [22]- [25]. However, in general, the performance of gyroscopes and accelerometers does not change much with changes in the environment in a short period of time. It is reasonable to believe that the system noise covariance matrix of the attitude estimation model does not change much. Considering the complexity of the algorithm and the stability of KF, the paper only adaptively adjusts the measurement noise covariance matrix. In [26], [27], the external acceleration is modeled as a first-order Gauss-Markov process, and the observation noise covariance matrix of the IKF is adjusted to adapt to the time-varying dynamic environment. This paper combines the complementarity of the two approaches, both modeling the external acceleration and adjusting the noise covariance matrix.
There are many mathematical approaches to describe the rotation of the coordinate system, such as the quaternion, and the direction cosine matrix (DCM). Compared to the DCM, the quaternion representation is simple and has no singular values. In view of the above problems, this paper proposes a robust IKF based on the GD algorithm in a dynamic environment. This method first uses the idea of two-step cascade filtering, and adds the GD algorithm as the first step of the IKF framework. In this way, the measurement process and the state process are linear, and there is no constraint of quaternion normalization. Second, by modeling the external acceleration and adjusting the measurement noise covariance matrix, the impact of the external acceleration on the attitude estimation system can be reduced complementarily.
The remainder of this paper is organized as follows: Section II introduces the concrete implementation of the proposed robust IKF algorithm. The simulation and experimental results are shown in Section III to verify the effectiveness of the proposed algorithm. A brief summary and outlook are shown in Section IV.

II. METHODOLOGY
The attitude estimation system without considering external acceleration is very simple, and the horizontal attitude is calculated and corrected by the gyroscope and the accelerometer, respectively. Generally, the carrier is required to be relatively gentle in movement most of the time. Refer to [28] for a detailed description of the system definition. In this section, a two-step cascaded IKF containing the GD algorithm is introduced. Some necessary definitions of the matrices, variables and corresponding model parameters used in our implementation are also introduced.

A. FILTER INITIALIZATION
The IKF is a stepwise recursive weighted estimation method. The initialization process is the basis, including the initial alignment ([ roll 0 pitch 0 yaw 0 ] T ) and parameter initialization. Reference [29] provides a reasonable initialization process. For initial alignment, the IMU needs to remain static for a few seconds. Gravity is sensed by a three-axis accelerometer, and the initial attitude is determined by the accelerometer output. Due to the measurement noise, the initial heading cannot be determined from the low-cost gyroscope. However, in static, there are sufficiently conditions to determine the initial heading by effective means such as derivation from known points and optical alignment. The initialization of the parameters of the IKF includes the initial state vector (δx 0 ) and the initial state covariance matrix (P 0 ). For a progressive steady-state filter system, with the iteration of filter, the effect of the initial states will gradually disappear. In our implementation, the initial state is set to δx 0 = 0 6×1 , the initial state covariance matrix reflects the deviation range of the corresponding elements in the initial states, and it is set to P 0 = diag(var[δx 0 ]), where diag(·) is the diagonal matrix, and var[ · ]) is the variance.

B. SYSTEM MODEL
The KF is perfect in theory, is suitable for stationary or nonstationary stochastic processes and is an optimal estimate for linear systems. However, integrated navigation problems are usually nonlinear problems. IKF, which uses error states instead of full states for iterative updates, is a popular filtering method. The 6 × 1 error-state vector is designed as follows: Here, δθ denotes a 3 × 1 error quaternion, and δb g is the error bias of the gyroscope. For the gyroscope bias, the standard additive error definition is used, but the attitude error is expressed by the error quaternion.
Assuming thatq is defined as the expected value of the quaternion q which is defined by q [ q T v q w ] T = [ q x q y q z q w ] T , the error quaternion δq according to [30] is described as follows: where the notation ⊗ denotes quaternion multiplication, and δθ is the vector part of the error quaternion. The gyroscope output (ω m ) and the accelerometer output (a m ) for IKF measurement update are modeled as: where C(q) is the DCM corresponding to the quaternion q, ω denotes the angular rate, g is the gravitational acceleration, n ω and n a are the gyroscope and accelerometer measurement noise vectors, respectively, which are assumed to be zeromean white Gaussian noise processes, and a(t) denotes the external acceleration vector of the body, which is modeled as a first-order Gauss Markov model where α determines the cut-off frequency, which is a dimensionless constant between 0 and 1, and ς (t) is the timevarying error of the acceleration process model. The dynamics of the perturbation are described by a linearized continuous-time state space model, so the deviation model of the attitude estimation system is defined as [8], [18], [30] whereV is the expectation of 3D vector V , and V × is the skew symmetric matrix of V , the above deviation model can be redefined in matrix form as: where F and G are the state transition and system noise drive matrices, respectively, n denotes the system noise vector, and the covariance Q = E[n(t + τ )n T (t)] depends on the system noise vector and can be calculated offline.
When a new measurement is available, the state estimate is propagated using numerical integration, such as the fourth order Runge-Kutta method, taking the state transition matrix F as an example, we evaluate the discrete-time state transition matrix: Select a reasonable measurement model to provide linear correction information for the error-state, and apply IKF theory to obtain its optimal estimate. The next subsection will show the detailed design.

C. MEASUREMENT MODEL
The idea of the two-step cascade filter is used to calculate measurement. The IKF uses the error state instead of the full state, but the measurement model is still a nonlinear system. Applying Taylor series expansion to linearize the processing will introduce linearization errors. The GD algorithm uses the negative gradient direction corresponding to the objective function to update the new direction of each iteration, so that each iteration can gradually reduce the objective function to be optimized. The GD algorithm can obtain attitude information from nonlinear observations, thereby eliminating linear errors in the standard EKF algorithm and improving the accuracy of attitude estimation. As proposed by Madgwick, we also formulate the observation quaternion as an optimization problem.
where a − k = αa + k−1 is the predicted external acceleration of the current time step, and a + k−1 represent a posteriori external acceleration of the previous step. Then, a m,k − a − k can be approximated as a projection of gravity because the external acceleration is subtracted from the accelerometer signal.
Similar to Madgwick, we choose the implementation of the computational effective GD algorithm as a possible solution to this optimization problem where ∇f (q a,k ) ∇f (q a,k ) is the normalization of the gradient, indicating the direction of the gradient, and µ denotes the step size of the gradient descent method. The fixed gradient step can only satisfy the accuracy of the algorithm at a certain speed. When the object is rotating at a high speed, the step size of the algorithm is too small, resulting in the wrong attitude estimation result. Therefore, a gradient descent algorithm with a dynamic step length is proposed. The step calculation formula is as follows: where c > 1. This indicates that the faster the angular velocity of the carrier is, the longer the sampling time is, and the longer the step size of the gradient is. A further mathematical derivation or navigation equations of the filter can be found in [16]. When the attitude quaternion is obtained from the gravitational field, the measurement model can be written as follows where H is the 4 × 6 Jacobian matrix obtained by using the chain rule.
where I n is the n-dimensional identity matrix, and the quaternion term can be derived as follows by using the chain rule again:

D. ADAPTIVE IKF VARIABLE COVARIANCES
It is well known that the measurement noise covariance affects the filter value via the gain matrix. When the measurement information contains large noise, the filter should reduce the trust in the measurement information by increasing R k . Another rule is that the measurement of the noise covariance matrix should be simple and adequately guaranteed to be positively definite. R k can be designed as where M acc is the covariance matrix related to the external acceleration model error and defined as , and R a denote the covariance matrix of the measurement noise, which is mainly introduced from the accelerometer affecting the quaternion estimation in the GD and is defined as R a = E[n a (k)n T a (k)]. Now, assuming that the predicted external acceleration a − k is uncorrelated to true external acceleration a k , and the expectation of a k is zero, M acc is simplified as Once the attitude quaternion q + k−1 is estimated after the k − 1 th IKF update, the external acceleration a + k−1 can be obtained by E. ITERATED IKF UPDATE By integrating the system model, the observation quaternion measurement model, and the adaptive adjustment into the IKF framework, a complete algorithm for attitude estimation system is obtained. The pseudo code is listed in Algorithm 1. The workflow of the proposed algorithm is summarized as follows. First, the error state and state covariance are updated using a simplified INS mechanization and deviation model. Obtain the observation quaternion by GD, and then execute measurement update and adaptively adjust the measurement covariance matrix. After that, the nominal state of the current iteration is updated with the error state and previous moment nominal-state using the appropriate compositions (⊕ denotes standard additive and quaternion products), where x ← f (x, ·) represents an iterative update of type x k+1 = f (x k , ·).The error state and its covariance matrix are then reset, and the reset operation is where M = This expression should produce more precise results, but in most cases in which the IKF is implemented, the error term δθ is neglected, which simply leads to M = I 6 .

A. SIMULATION: COMPARISONS WITH TWO REPRESENTATIVE FILTERS
Simulation analysis plays an important role in the feasibility study of the attitude estimation scheme. Refer to [31] for a more detailed sensor simulation method. The trajectory information of the real world, such as the position, velocity,  attitude, acceleration and time, is used as the input reference of the simulation system. The real-word trajectory information is provided by INS/GPS integrated navigation results, we fixed the IMU and GPS receiver on an aluminum plate, shaking it on the open ground, so that a roll angle and a pitch angle were generated. Unlike Gonzalez, our sensor output model does not consider static bias, as shown in equation (3). For the simulation parameters refer to ADIS-16448. The simulated data of the accelerometer and gyroscope are shown in Fig. 1 In the data analysis, the actual integrated navigation results of inertial navigation and satellite navigation are used as the true reference value. Based on the simulation data, the difference between the attitude calculated by the MEMS inertial navigation solution and the reference attitude is used as the evaluation criterion. The specific data processing solutions for evaluation include the following. 1) The GD Filter (GDF) proposed by Madgwick, with the adjustment parameters of the GDF given by β = 0.1. 2)The EKF method without considering external acceleration, for which the detailed algorithm is descripted in [14] and the EKF parameters are given by δ w = 0.05, δ q = 0.01 and δ a = 0.05. 3) The attitude estimation algorithm proposed in this paper, which considers both the linearization error and the external acceleration. The estimated Euler angles of roll and pitch are shown in Fig. 2.
It can be seen in Fig. 2 that the accuracies of the three estimation methods are different in this experiment.  The difference between the output of the gradient descent filter and the true value seems to be larger in the three estimation methods because it uses a fixed filter gain. To describe this observation objectively, we plotted the attitude errors of the three methods in Fig. 3 and obtained their root mean square error (RMSE) and maximum error in Table 1.
The results show that the proposed filter is more accurate than the two compared methods. In particularly, there are still good filtering effects in several periods when the external acceleration is obvious, such as the periods near 100 s and 300 s. However, the other two representative filters do not perform well in these time periods because they rely on gravity to correct the attitude, which introduces incorrect corrections when there is external acceleration. The proposed algorithm can effectively estimate the value of external acceleration and adjust the measurement noise matrix adaptively, which has good robustness.

B. EXPERIMENT 1: COMPARISONS WITH COMMERCIAL PRODUCTS
In this section, we conduct experiments with real data provided by commercial product MTi-G-710 to verify the relative accuracy of the proposed algorithm. The attitude estimation results of the proposed filter are compared with those provided by MTi-G-710. For the sake of fairness, commercial products only collect acceleration and gyroscope data when collecting data, and the build-in GeneralNoBaro filter is selected. We acquire a classic sample of data, including 10 s of static and 40 s of high dynamics. The recorded raw inertial data are shown in Fig. 4. The proposed IKF parameters in this experiment are given by c = 10, δ w = 0.01, and δ a = 0.05. The comparison with the commercial product is shown in Fig. 5.
The raw data of Fig. 4 show that we did not keep the MTi-G-710 in steady and slow motion for a long time. The accelerometer senses not only gravity, but also VOLUME 8, 2020   external acceleration. Fig. 5 shows that the output of the proposed filter in this experiment has a very high consistency with the static and high dynamic references of mature commercial products. Before evaluating the experimental results, we first defined the method for comparing the performance of the algorithm. We defined the attitude error from the perspective of the Euclidean distance and presented the RMSE relative to the time in Fig. 6.
The RMSE relative to the marching time can well reflect the trend of the accumulation of errors over time. Observing the results in Fig. 6, we find that the RMSE of the proposed algorithm is very small during the static time, which is also well understood. Because there is no interference from external acceleration, gravity plays a strong role in correcting. In dynamics, the RMSE of the proposed algorithm increases over time, but the overall result is within an acceptable range,  which shows that the proposed filter is accurate. The overall roll and pitch RMSE are 1.7775 • and 1.3239 • , respectively. Experiment 2 shows that the relative accuracy of the proposed filter is reliable. However, the absolute accuracy of the proposed algorithm cannot be verified without a high precision reference. This is be shown in the next section.

C. EXPERIMENT 2: TEST RESULTS ON AN OUTDOOR CAR-BORNE EXPERIMENT
To test the absolute accuracy of the attitude estimation of the proposed algorithm in the real world, an outdoor carborne experiment in Fuxin, Liaoning Province, China, was conducted. The raw measurements of the accelerometer and gyroscope were collected by the NAV440 sensor with a sampling rate of 200Hz. The integrated high precision navigation results of INS and GPS RTK are used for reference. The equipment and route are shown in Fig. 8. We selected accelerometer data and gyroscope data in the 60 s for experiment 2 (the trajectory is highlighted in orange in Fig. 7). After initial alignment, the car passes through a relatively gentle road surface (with some gravel), which means an unavoidable intervention with external acceleration.
The parameter c related to the dynamic step is set to 10 and The noise covariances of gyroscope and accelerometer are respectively δ 2 w I, and δ 2 a I. It can be seen from Fig. 8 that the external acceleration signal is very obviously, and its detrimental effect in the attitude estimation cannot be ignored. Marks I, II and III are three typical periods of time with external acceleration. The difference is that the both the x-axis of the gyroscope output at mark II and the y-axis of  the gyroscope output at mark III vary greatly. Generally, such output does not conform to the pavement characteristics of the expressway. Fig. 9 compares the output of the roll and pitch of the proposed filter with those of the reference. It can be seen from the results that the output of the proposed filter fits the reference values well most of the time, except for the roll angle in mark II and the pitch angle in mark III. To verify the calculation results and their variance, the output error of the proposed filter and its 3σ bounds are shown in Fig. 10. The standard deviation vector of the errors from navigation estimates is calculated by only taking the diagonal elements of the covariance matrix.
Combining the raw acceleration signal of Fig. 8 and Fig. 10, it can be seen that under the condition that external acceleration cannot be ignored, the proposed filter effectively reduces its harmful effect on the system, and the antiinterference ability of the filter is well verified. Combining the raw gyroscope signals, it can be seen that the x-axis and y-axis of the gyroscope at marks II and III in Fig. 8 have large fluctuations, respectively, which does not meet the road conditions of the expressway. It can be regarded as the gross error existing when collecting raw data. Further, the roll and pitch errors at the marks II and III in Fig. 10 exceed the 3sigma limit, respectively. Since the proposed algorithm does not consider the elimination and adaptation of gross errors. Such a situation can be avoided by culling gross errors, as proposed by Chang and Qin [21] and Yang et al. [32].
The statistical results show that the RMSE of the roll and pitch angle are 0.2604 • and 0.4423 • , respectively, and their maximum errors are 1.0192 • and 2.5510 • , respectively. The test results show that the proposed algorithm can achieve robust attitude estimation under car-borne environment.

IV. CONCLUSION AND FUTURE WORK
This paper presents the attitude estimation of IMU under dynamic conditions. Using our previous contributions in attitude estimation, the attitude quaternion obtained by the GD is integrated into the IKF framework as a measurement, which makes the state and measurement dynamics are linear. Since we consider the effects of external acceleration under dynamic conditions, the accuracy of the attitude estimation and the robustness of the system have been improved. Throughout comparisons of results with other two representative filters using simulation data, we prove that the accuracy of the proposed algorithm is higher than that of standard EKF and GDF. By using real data to compare the attitude estimation results between commercial AHRS and high-precision GPS RTK/INS, we not only prove that the absolute and relative accuracy of the proposed algorithm is correct, but also show the robustness of the system under dynamic conditions. The orientation estimation from Magnetic Angular Rate Gravity (MARG) sensors is one of future research directions, future work will focus on improving robust orientation estimation and performance compared to other filters during low and high dynamic environments. The estimation of magnetic distortion is the prerequisite for using magnetic sensors, and also need to focus on perform quality analysis on the output of low-cost gyroscopes, although the interference of gyroscope data is not considered in many orientation estimations schemes.
In adaptive Kalman filtering, the estimation of statistical characteristics of system noise is a very difficult problem. It is also one of the future research directions. In response to this problem, based on the feature that state estimation is more relevant to the one-step prediction covariance matrix, Huang's method can be adopted to consider the idea of estimating the one-step prediction error covariance matrix, thereby avoiding direct estimation of the system noise covariance matrix.