Invariant particle filtering with application to localization

The recently introduced Invariant Extended Kalman Filter (IEKF) is an extended Kalman filter designed for systems admitting symmetries, that possesses interesting convergence properties, and a relative independence of the filter behavior with respect to the system´s trajectory. In the present paper, the ideas are extended to a broad class of systems introducing the notion of “conditional invariance”, that is, invariance properties of the system once some of the state variables are known. We exploit this structure by devising an Invariant Rao-Blackwellized Particle Filter: those state variables are sampled, and the rest are marginalized out using IEKFs. The striking property of the obtained particle filter is that the Kalman gains are identical for all particles, leading to a drastic reduction of the computational burden. The strong potential of the method is illustrated by the challenging and realistic problem of localization from noisy inertial sensors and a noisy GPS having a randomly jumping bias.