Discrete Extended Kalman Filter on Lie groups

In this paper, we generalize the Discrete Extended Kalman Filter (D-EKF) to the case where the state and the observations evolve on Lie group manifolds. We propose a new filter called Discrete Extended Kalman Filter on Lie Groups (D-LG-EKF). It assumes that the posterior distribution of the state is a concentrated Gaussian distribution on Lie groups. Our formalism yields closed-form equations for both nonlinear discrete propagation and update of the distribution parameters based on the likelihood. We also show that the D-LG-EKF reduces to the traditional D-EKF if the state evolves on an Euclidean space. Our approach leads to a systematic methodology for the design of filters, which is illustrated by the application to a camera pose estimation problem. Results show that the D-LG-EKF outperforms both a constrained D-EKF and a D-EKF applied on the Lie algebra of the Lie group.