Kalman filtering with a class of geometric state equality constraints

In this paper we consider a noise free class of dynamics encompassing left- and right-invariant on a Lie group with noisy partial state measurements. We assume in addition that the covariance matrix of the state is initially rank-deficient. This, combined with the absence of process noise, keeps the system state within a (time-dependent) subset of the state space at all times. We prove mathematically that the invariant extended Kalman filter (IEKF) perfectly respects this kind of state constraints, contrarily to the standard EKF, or the unscented Kalman filter. This is a strong indication that the IEKF is particularly well suited to navigation when motion sensors are highly precise. The theory is applied to a non-holonomic car example on SE(2), and to an attitude estimation example on SO(3).