A Quick Guide for the Iterated Extended Kalman Filter on Manifolds

Abstract

The extended Kalman filter (EKF) is a common state estimation method for discrete nonlinear systems. It recursively executes the propagation step as time goes by and the update step when a set of measurements arrives. In the update step, the EKF linearizes the measurement function only once. In contrast, the iterated EKF (IEKF) refines the state in the update step by iteratively solving a least squares problem. The IEKF has been extended to work with state variables on manifolds which have differentiable $\boxplus$ and $\boxminus$ operators, including Lie groups. However, existing descriptions are often long, deep, and even with errors. This note provides a quick reference for the IEKF on manifolds, using freshman-level matrix calculus. Besides the bare-bone equations, we highlight the key steps in deriving them.