Iterated Invariant Extended Kalman Filter (IIEKF)

In this paper, we introduce the Iterated Invariant Extended Kalman Filter (IIEKF), which is an invariant extended Kalman filter (IEKF) where the updated state in the light of the latest measurement is defined as a maximum a posteriori (MAP) estimate. Under some compatibility requirements on the output map, we prove strong mathematical guarantees which echo those of the Kalman filter in the linear case. We apply the technique to two problems: solving a system of equations on a Lie group, and a problem of engineering interest, namely ego-localization of the hook of a crane. The latter serves as a benchmarking example, where the IIEKF favorably compares to other filters.