The Geometry of Extended Kalman Filters on Manifolds with Affine Connection

The extended Kalman filter (EKF) has been the industry standard for state estimation problems over the past sixty years. The classical formulation of the EKF is posed for nonlinear systems defined on global Euclidean spaces. The design methodology is regularly applied to systems on smooth manifolds by choosing local coordinates, however, it is well known that this approach is not intrinsic to the manifold and performance depends heavily on choosing 'good' coordinates. In this paper, we propose an extended Kalman filter that is adapted to the specific geometry of the manifold in question. We show that an affine connection and the concepts of parallel transport, torsion, and curvature are the key geometric structures that allow the formulation of a suitable family of intrinsic Gaussian-like distributions and provide the tools to understand how to propagate state estimates and fuse measurements. This leads us to propose novel geometric modifications to the propagation and update steps of the EKF and revisit recent work on the geometry of the reset step. The relative performance of the proposed geometric modifications are benchmarked against classical EKF and iterated EKF algorithms on a simplified inertial navigation system with direct pose measurements and no bias.