Anderson Acceleration for on-Manifold Iterated Error State Kalman Filters

Abstract

Iterated Extended Kalman Filter is a promising and widely-used estimator for real-time localization applications. It iterates the observation equation to find a better linearization point and, simultaneously, only maintains the state estimation in a single time to save the computation resources. Inspired by the recent development of the iterative closest point algorithm, this letter investigates an acceleration approach to the iterations in iterative error state Kalman filters (IESKFs). We show that the IESKF can be seen as a fixed point problem, and the Anderson acceleration (AA) can be elegantly applied to the iterations of IESKF since the error state naturally lies in the tangent space and does not require additional transforms. However, the tangent space of the current estimation may change during the iterations, so we should switch the tangent space to the starting point to perform the Anderson acceleration. We propose the AA-IEKF and apply it to the lidar-inertial odometry (LIO) systems to estimate the ego motion of a lidar. The experiments show that the Anderson acceleration can efficiently reduce the number of iterations in ESKF and achieve a lower computational cost.