Extended Kalman Filtering With Nonlinear Equality Constraints: A Geometric Approach

In this paper, we focus on extended Kalman filtering (EKF), in the difficult case where a function of the state has been perfectly observed, and is thus known with certainty, while the full state still has unobserved degrees of freedom. In the linear case, the Kalman filter seamlessly handles such constraints, which result in the state being in an affine subspace. Yet, in the nonlinear case, the EKF poorly handles such type of constraints. As a remedy, we propose a novel general methodology of EKF based on an (arbitrary) nonlinear error e. And we prove that under compatibility of the error e with the constraints, the EKF based on e seamlessly handles the constraints. Furthermore, when the state space is a Lie group, we prove the EKF based on invariant errors is exactly the invariant EKF (IEKF), and we prove further properties. The theory is applied to the problem of simultaneous localization and mapping, where the IEKF is shown to perfectly handle some partial deterministic information about the map. As a byproduct, the theory is also shown to readily allow devising EKFs on state spaces defined by a class of equality constraints.