Invariant Momentum-tracking Kalman Filter for attitude estimation

This paper presents the development, simulation and experimental testing of a non-linear Kalman filter for attitude estimation. This non-linear filter is able to conserve the invariants of the Kalman filter, i.e., the expectations on state estimates and their covariances, by operating in the Lie algebra of SO(3) and along the trajectory of evolving angular momentum. The main feature of this novel discrete-time filter is that the linearization of the Gaussian uncertainty around these permanent trajectories leads to a locally optimal Kalman gain matrix. Results confirm that this Invariant Momentum-tracking Kalman Filter (IMKF) out-performs state-of-the-art approaches such as the Extended Kalman Filter (EKF), and Invariant Extended Kalman Filter (IEKF). At very-low sampling rates, EKFs suffer from divergence as the uncertainty propagation is corrupted by the underlying system approximations. The IMKF suffers no such problems according to the theoretical developments and results reported here.