A Tutorial on the Invariant Filtering Framework

Filtering involves estimating the state of a dynamic system by integrating noisy observations with mathematical models. While the Extended Kalman Filter (EKF) is widely used for nonlinear problems, it often neglects the geometric structure and symmetries inherent in many systems. The Invariant Extended Kalman Filter (IEKF) addresses this limitation by leveraging these geometric properties for improved estimation performance. This work provides a comprehensive introduction to the invariant filtering framework, using the illustrative example of estimating the pose of an IMU constrained to pendular motion. It demonstrates that kinematic constraints can be effectively modeled as noise-free pseudo-measurements within this framework. Additionally, we propose an Iterated IEKF, inspired by the Gauss-Newton algorithm, to ensure that all estimated states adhere to the imposed constraints. These methods offer a pathway to generalizing the invariant framework for estimating extended poses in multibody systems.