Continuous-Discrete Extended Kalman Filter on Matrix Lie Groups Using Concentrated Gaussian Distributions

In this paper we generalize the continuous-discrete extended Kalman filter (CD-EKF) to the case where the state and the observations evolve on connected unimodular matrix Lie groups. We propose a new assumed density filter called continuous-discrete extended Kalman filter on Lie groups (CD-LG-EKF). It is built upon a geometrically meaningful modeling of the concentrated Gaussian distribution on Lie groups. Such a distribution is parametrized by a mean and a covariance matrix defined on the Lie group and in its associated Lie algebra respectively. Our formalism yields tractable equations for both non-linear continuous time propagation and discrete update of the distribution parameters under the assumption that the posterior distribution of the state is a concentrated Gaussian. As a side effect, we contribute to the derivation of the first and second order differential of the matrix Lie group logarithm using left connection. We also show that the CD-LG-EKF reduces to the usual CD-EKF if the state and the observations evolve on Euclidean spaces. Our approach leads to a systematic methodology for the design of filters, which is illustrated by the application to a camera pose filtering problem with observations on Lie group. In this application, the CD-LG-EKF significantly outperforms two constrained non-linear filters (one based on a linearization technique and the other on the unscented transform) applied on the embedding space of the Lie group.