Invariant Extended Kalman Filter (IEKF)

• Invariant Extended Kalman Filter (IEKF) is a nonlinear state estimation framework on Lie groups that leverages invariant error linearization based on group-affine dynamics.
• It alternates between propagation and update steps using a Riccati equation and invariant innovation derived from the system's symmetry properties.
• IEKF offers uniform convergence, improved consistency, and reduced computational complexity, outperforming conventional EKF in challenging scenarios.

The Invariant Extended Kalman Filter (IEKF) is a nonlinear state estimation framework specialized for systems whose states evolve on Lie groups and whose dynamics possess a group-affine structure. Developed as a principled generalization of the classical Extended Kalman Filter (EKF), the IEKF leverages the geometric invariants and symmetry properties inherent to many robotic, navigation, and estimation systems. Its defining characteristic is the linearization of estimation errors in an invariant manner on the group, resulting in error dynamics that are independent—or only dependent on measured system inputs—rather than on the evolving state estimate. This yields uniform convergence properties, improved consistency, and robustness to poor initialization or unobservable directions, surpassing capabilities of the standard EKF (Barczyk et al., 2014, Phogat et al., 2019, Barrau et al., 2014).

1. Theoretical Foundations and Error Structure

The IEKF formulates state estimates on a matrix Lie group $G$ (e.g., $SE(2)$, $SE(3)$, $SE_2(3)$) and constructs the estimation error by exploiting the group operation. For a process evolving as $\dot{X} = X \Omega(u)$ on $G$, where $u$ is a control or input and $\Omega$ maps $u$ into the Lie algebra, the invariant error can be defined as either left-invariant (e.g., $\eta = X^{-1}\hat{X}$) or right-invariant (e.g., $\eta = \hat{X}^{-1}X$) depending on group and measurement structure (Barczyk et al., 2014). The hallmark of the IEKF is that, under the group-affine condition,

$f_u(XY) = f_u(X)Y + X f_u(Y) - X f_u(I) Y,$

the error dynamics “close” on $\eta$ and become autonomous, meaning they do not explicitly depend on the current (unknown) system trajectory—a property not shared by the standard EKF (Barrau et al., 2014, Phogat et al., 2019, Ge et al., 6 Jul 2025).

Linearizing around the identity yields a first-order error equation on the Lie algebra:

$\dot{\xi} = [\xi, \Omega_m] + W + K(V - \xi),$

where $\xi$ is the log-coordinates of the error, $W$ collects process noise, $K$ is the Kalman gain, and $V$ represents measurement noise (Barczyk et al., 2014). The form of $A$ and $C$ matrices in the associated Riccati equation are constants or functions of measured inputs, but crucially do not depend on the evolving state estimate. This leads to time-varying but state-independent filter gains, in contrast to conventional EKF implementations.

2. Recursive Algorithm and Riccati Equations

The IEKF alternates between propagation (using group-affine dynamics) and measurement update steps defined on the Lie group, with each step leveraging the invariant error form:

• Prediction: Propagate the state estimate and covariance on $G$ using the nominal input and group-affine model, e.g.,

$\dot{\hat{X}} = \hat{X} \Omega(u_m),$

alongside the Riccati equation for covariance $P$:

$$\dot{P} = A P + P A^T - P C^T (DRD^T)^{-1} C P + B Q B^T.$$

Here $A$, $B$, $C$, $D$ are computed from the group structure and input signals (Barczyk et al., 2014, Phogat et al., 2019).

• Update: At measurement times, compute the group-invariant innovation, e.g.,

$$E = \pi(\hat{X}^{-1} Y_{\text{m}}) \approx \hat{X}^{-1} Y_{\text{m}} - I,$$

apply the gain $K$, and update the state along the group exponential:

$\hat{X}^{+} = \hat{X} \exp\left(K E \right).$

The covariance is similarly updated using the linearized observation model.

This invariant structure remains valid under both continuous and discrete time implementations, with closed-form propagators available for key Lie groups such as $SO(3)$, $SE(3)$, and $SE_2(3)$ (Phogat et al., 2019, Li et al., 2022). The measurement model can handle both full-rank and degenerate (partial) observations and is compatible with noise-free pseudo-measurements via suitable limiting procedures (Goffin et al., 16 Apr 2024).

3. Advantages over Conventional EKF

The IEKF differs fundamentally from the standard EKF in its error linearization and propagation, yielding several advantages:

• Trajectory-Independent Linearization: The Jacobians in the IEKF do not depend on the trajectory or the evolving state estimate, but only on measured inputs or fixed structure, eliminating linearization bias that can cause divergence in the conventional EKF when large errors are present (Barczyk et al., 2014, Barrau et al., 2014, Barczyk et al., 2015).
• Uniform Basin of Convergence: The region of convergence is constant over the state space and does not shrink with challenging initializations (Phogat et al., 2019, Barrau et al., 2014).
• Consistency in Unobservable Directions: The IEKF invariant error ensures that the unobservable subspace remains in the null space of the observability matrix, preserving correct filter uncertainty (e.g., correct null space for global translation/yaw in VINS) (Zhang et al., 2017).
• Reduced Computational Complexity: When applied on Lie groups of moderate dimension ($m \ll n^2$ in $G \subset \mathbb{R}^{n \times n}$), the filter operates on $m \times m$ matrices, reducing per-step cost and Jacobian computation relative to a generic EKF (Phogat et al., 2019).
• Noise-Free and Constrained Measurements: IEKF updates can directly encode deterministic constraints as zero-noise pseudo-measurements with well-defined Kalman gain limits, maintaining statistical consistency along the constraint manifold (Goffin et al., 16 Apr 2024).

4. Application Domains and Experimental Validation

The IEKF framework has been instantiated and experimentally validated across various robotics and estimation platforms:

• Scan-Matching SLAM: IEKF-based scan matching delivers robust localization and mapping, outperforming EKF and avoiding divergence in degenerate scenarios (e.g., poor initial alignment, featureless corridors) (Barczyk et al., 2014, Barczyk et al., 2015).
• Visual-Inertial Navigation: Right-invariant EKF (RIEKF) variants integrated into multi-state constraint Kalman filters (MSCKF) demonstrate up to 50% improved RMS accuracy and restored consistency on both Monte Carlo and real-world datasets (Zhang et al., 2017, Li et al., 2022).
• Nocturnal and Low-Texture Localization: InEKF fusing IMU, camera, and odometer data achieves robust localization under low-visibility, with empirical error reductions in challenging environments (Gao et al., 1 Feb 2024).
• Piezoelectric Actuated Wheel Odometry: Partial IEKF (PIEKF) implementations, embedding only rotation-velocity into the Lie group, excel in accuracy, consistency, and outlier rejection in high-speed odometry scenarios (Hua et al., 2023).
• Human Motion and Biomechanics: Augmented IEKF on $SE_2(3) \times SE(3)$ addresses sensor misalignment and personalized kinematic chain estimation for improved accuracy in human trunk and limb motion (Zhu et al., 2022).
• Constraints and Pseudo-measurements: IEKF with noise-free updates supports efficient enforcement of equality constraints (cable lengths, planar motions) with consistency guarantees (Goffin et al., 16 Apr 2024, Goffin et al., 16 Apr 2024).
• Autonomous Surface Vessels with Partial Orientation: Direct integration of partial attitude (roll, pitch) measurements using infinite-covariance in IEKF measurement step preserves estimator consistency under horizon-based constraints (Benham et al., 12 Jun 2025).

5. Comparison: Left vs. Right Invariant Forms and Implementation Guidance

There has been prior debate regarding the equivalence and selection of left-invariant vs. right-invariant IEKF formulations. Rigorous analysis and simulation demonstrate that, when a proper "reset" step (aligning the mean and covariance after multiplicative correction) is implemented, there is no difference in performance or state estimation outcomes between left- and right-invariant versions for group-affine systems (Ge et al., 6 Jul 2025). Handedness can therefore be chosen for notational or software convenience, provided the reset step is always included. All practical and high-performance IEKF implementations—including GNSS-aided inertial navigation—should incorporate this step to avoid loss of invariance and asymptotic inconsistency.

Practical recommendations include:

• Select a group structure (e.g., $SE(2)$, $SE(3)$, $SE_2(3)$) matching the system's topology.
• Implement process and measurement noise models in the group-affine input form (body-frame noise injection).
• Initialize covariance and process/measurement noise based on sensor specification or empirical logs.
• Propagate the Riccati and gain updates at the sensor (typically IMU) rate; process measurement updates at their native rates.
• For deterministic or hard constraints, apply iterated IEKF (IterIEKF) or direct zero-noise update for explicit constraint enforcement (Goffin et al., 16 Apr 2024, Goffin et al., 16 Apr 2024).

6. Convergence, Consistency, and Limitations

Local asymptotic stability for the IEKF is provable under standard reachability and uniform observability conditions, mirroring the classical linear time-varying Kalman filter (Deyst–Price result). The IEKF maintains consistency (correct covariance quantification) even under challenging initializations or prolonged loss of observability, as the invariant error dynamics do not “move” with the state estimate (Phogat et al., 2019, Barrau et al., 2014). In systems with partial observability or degenerate information (e.g., only roll/pitch measured, yaw unobserved), judicious assignment of infinite measurement covariance on unobservable directions prevents spurious updates (Benham et al., 12 Jun 2025).

Notable limitations include increased modeling and computational complexity for nontrivial Lie groups (necessitating careful handling of group exponentials/logarithms), and the assumption of group-affine dynamics, which may not cover all systems of interest.

7. Outlook and Extensions

The IEKF paradigm generalizes to systems with equality constraints (via zero-noise updates or IterIEKF), hybrid continuous-discrete measurements, and sample-based ensemble variants for nonlinear or non-Gaussian error dynamics (Goffin et al., 16 Apr 2024, Barrau et al., 2013). Ongoing research encompasses integration with modern nonlinear observers, further reductions in algebraic complexity, and applications in new robotic domains such as multi-agent SLAM, marine autonomy, and biomechanics.

The theoretical and empirical consensus across independent research projects establishes the IEKF as a foundational framework for consistent, robust, and efficient state estimation on nonlinear manifolds (Barczyk et al., 2014, Phogat et al., 2019, Zhang et al., 2017, Ge et al., 6 Jul 2025, Goffin et al., 16 Apr 2024).