Attitude estimation by divided difference filter in quaternion space

Abstract

This article considers the application of Divided Difference Filter (DDF) to the orientation estimation, based on a quaternion-error continuous-discrete time model. DDF is a nonlinear estimator that in contrast to Taylor's expansion of extended Kalman Filter (EKF), exploit the polynomial approximations as a multivariable extension of Stirling's interpolation formula and require no derivatives. The DDF can be based on 1st and 2nd order Stirling's interpolation, which is named the divided difference filter-1st order (DDF1) and the divided difference filter-2nd order (DDF2). The orientation kinematics is defined in a quaternion vector space that unlike the Euler angle representation does not have any singularity problem. The presented nonlinear orientation model is an exact error model and is independent of the rigid body dynamics. The nonlinear process model includes six error-states in which only non-scalar elements of quaternion error vector are included in the error-state equations. The fourth element of quaternion error vector, which obeys unit norm constraint, is removed from system states to alleviate the estimated error covariance matrix divergence. The measurement system is a MARG sensor, which consists of a tri-axial rate gyro, a tri-axial accelerometer and a tri-axial magnetometer. The nonlinear measurement model is obtained based on the principals of magnetometer and accelerometer and the properties of the quaternion vector space. For the presented nonlinear orientation model, the performance of three filters namely DDF, EKF and Unscented Kalman Filter (UKF) is compared for different sampling frequencies in terms of the rms error, the captured area under the error norm curve, the estimated state variance and the computational cost. It is shown that under the same initial angle-error conditions, DDFs and UKF are more robust than EKF. The DDFs perform better than unscented Kalman filter (UKF) although the computational load for UKF is less. Among DDF1 and DDF2, DDF2's performance is slightly better but with more computation load. In the case of no initial angle-error conditions, the performance of the four filters is the same especially when the low noise level condition is considered.