From Intrinsic Optimization to Iterated Extended Kalman Filtering on Lie Groups

Abstract

In this paper, we propose a new generic filter called Iterated Extended Kalman Filter on Lie Groups. It allows to perform parameter estimation when the state and the measurements evolve on matrix Lie groups. The contribution of this work is threefold: (1) the proposed filter generalizes the Euclidean Iterated Extended Kalman Filter to the case where both the state and the measurements evolve on Lie groups, (2) this novel filter bridges the gap between the minimization of intrinsic non-linear least squares criteria and filtering on Lie groups, (3) in order to detect and remove outlier measurements, a statistical test on Lie groups is proposed. In order to demonstrate the efficiency of the proposed generic filter, it is applied to the specific problem of relative motion averaging, both on synthetic and real data, for Lie groups $$SE\left( 3\right) $$SE3 (rigid-body motions), $$SL\left( 3\right) $$SL3 (homographies), and $$Sim\left( 3\right) $$Sim3 (3D similarities). Typical applications of these problems are camera network calibration, image mosaicing, and partial 3D reconstruction merging problem. In each of these three applications, our approach significantly outperforms the state-of-the-art algorithms.