Quaternion Invariant Cubature Kalman Filtering for Attitude Estimation

This paper presents a novel nonlinear filtering method for attitude estimation of rigid bodies, called the invariant cubature Kalman filter. This method combines the theory of invariant observer and cubature Kalman filter. When using the cubature filter formulas, the proposed method can maintain the invariance of system dynamics and calculate the estimated state with the geometrical correction terms obtained from the invariant output errors. According to the left or right Lie group action adopted in the definition of the state estimation errors, two quaternion invariant filters are developed by utilizing unit quaternions to represent attitudes, named left-invariant cubature Kalman filter and right-invariant cubature Kalman filter, respectively. These general filters are implemented to attitude and gyro bias estimation problems of a low-cost multi-sensors system. Simulation results showed that the right-invariant cubature filter outperforms other comparable filters because it inherits the merits of the right nonlinear invariant observer and the cubature Kalman filter and has less dependence on the estimated trajectory.