Invariant Kalman Filter Overview

Updated 12 August 2025

Invariant Kalman Filter is a geometric filtering approach that defines state and error on Lie groups for improved nonlinear state estimation.
It leverages invariant error representations and log-linear propagation to achieve state-estimate-independent error dynamics and robust convergence.
Practical applications include SLAM, inertial navigation, and multi-robot coordination, demonstrating enhanced consistency and reduced sensitivity to initialization errors.

The Invariant Kalman Filter (IKF) and its most prominent realization, the Invariant Extended Kalman Filter (IEKF), constitute a geometric adaptation of Kalman filtering to systems whose state evolves on a Lie group. The IEKF leverages the underlying group structure to achieve state-estimate-independent error dynamics, yielding improved consistency, robustness to poor initializations, and superior performance under strong nonlinearities—especially in the context of group-affine dynamics and high-dimensional robotic and navigation systems.

1. Geometric Foundations and Error Structure

The IEKF departs fundamentally from the standard EKF by defining state and error directly on a matrix Lie group G. For a system with state $X \in G$ , the error is formulated in an invariant manner—either left-invariant ( $\eta_L = X^{-1} \hat{X}$ ) or right-invariant ( $\eta_R = \hat{X} X^{-1}$ ). These group-based error definitions ensure that, for group-affine systems (those whose dynamics are of the form $\dot{X} = X \Omega$ for $X \in G$ and $\Omega$ in the associated Lie algebra), the error evolution becomes autonomous; that is, the error propagation and update are independent of the current trajectory of the estimate (Barczyk et al., 2014, Barrau et al., 2014, Phogat et al., 2019).

The key insight is to "lift" the error to the Lie algebra via the logarithm map, e.g., $\eta = \exp(\xi)$ with $\xi \in \mathfrak{g}$ . Linearizing about the identity yields log-linear error propagation: $\frac{d}{dt} \xi = A_u \xi$ where $A_u$ depends solely on system inputs and not the estimated trajectory (Barrau et al., 2014). This property underpins the IEKF's well-defined region of convergence and log-linear stability.

2. Group-Affine Systems and Invariant Filtering

Systems modeled on matrix Lie groups most commonly arise in robotics, navigation, and SLAM, where the state includes pose (SE(3) or SE(2)), velocity, and sometimes additional features (e.g., foot positions in humanoids, feature poses in SLAM). For such systems, group-affinity (the requirement that the system's right action is compatible with group multiplication) ensures that invariant error dynamics decouple from the estimate.

For a typical process model: $\dot{X} = X \Omega$ with $\Omega$ encoding body-frame velocities, the propagation in the IEKF occurs by integrating these dynamics on the group. The IEKF update is then performed using innovations defined in the Lie algebra, with gain matrices computed by propagating Riccati equations whose coefficients depend only on measured inputs (i.e., invariants of the problem), not the current iterate (Barczyk et al., 2014, Barrau et al., 2014, Phogat et al., 2019, Pavlasek et al., 2021).

The measurement model must also be formulated in an invariant manner; for example, for left-invariant systems, as $Y = X b + n$ with $b$ known and $n$ noise. This ensures that measurement Jacobians are state-independent, a property impossible in conventional linearizations.

3. Log-Linear Property and Stability

A central theoretical result is the log-linear propagation property: the error variable $\xi$ (from $\eta = \exp(\xi)$ ) obeys a strictly linear ODE between updates (Barrau et al., 2014). This leads to error evolution equations such as: $\dot{\xi} = A_{u_m} \xi + W + K(V - \xi)$ with $A_{u_m}$ dependent on the measured process input, $W$ process noise, $V$ measurement noise, and $K$ Kalman gain.

At the update step, the innovation is computed by projecting the group error onto the Lie algebra, and the update becomes: $\xi^+ = \xi - K(H\xi + v)$ with $H$ the measurement Jacobian. Provided standard observability criteria are satisfied, the IEKF is locally exponentially stable around any trajectory. In particular, for group-affine systems, the Jacobians $A$ , $B$ , $C$ , $D$ in the Riccati equations depend only on measured invariants, not the state estimate (Barczyk et al., 2014, Barrau et al., 2014, Phogat et al., 2019, Pavlasek et al., 2021).

4. Practical Implementations and Applications

The IEKF has been validated and implemented across a wide spectrum of robotics and navigation scenarios:

Scan Matching SLAM (SE(3)): IEKF directly on the pose group demonstrates robust fusion of odometry and scan-matching measurements. Experimental results with a wheeled robot report RMS errors of 4.5–5.1 cm in position and sub-degree errors in heading, even under poor initializations (Barczyk et al., 2014).
Attitude and Inertial Navigation: For attitude filtering on SO(3) and inertial navigation on SE₂(3), the IEKF provides lower steady-state mean-squared-error than standard EKF, with reduced sensitivity to estimate errors, as shown in 10,000-run Monte Carlo experiments (Phogat et al., 2019).
Visual-Inertial Navigation and Human Motion Estimation: The IEKF framework has been adapted for VINS (notably in the RIEKF-VINS), SLAM with line features, humanoid state estimation (SE₄(3)), and human biomechanics, consistently providing improved consistency (i.e., NEES and RMS error align), rapid convergence (e.g., <0.2 s under large placement errors), and robustness to partial observability (Zhang et al., 2017, Yan et al., 2019, Vedadi et al., 5 Jan 2024, Zhu et al., 2022).
Multi-Robot SLAM and Distributed Filtering: The IEKF is used as a local estimator in distributed architectures, with covariance intersection for fusing cross-correlated estimates in multi-robot SLAM (Li et al., 14 Sep 2024).
Handling Partial/Noisy Measurements: Methods are developed for integrating partial orientation (e.g., roll/pitch only via horizon sensors) and noise-free pseudo-measurements (constraints), using analytical infinite covariance updates and iterative IEKF procedures to ensure compatibility and fast constraint satisfaction (Goffin et al., 16 Apr 2024, Goffin et al., 16 Apr 2024, Benham et al., 12 Jun 2025).

5. Algorithmic Features and Derivations

The essential steps in an IEKF cycle are:

Prediction (Group Propagation):
- Integrate group-affine dynamics on the Lie group.
- Propagate the error covariance via the group-invariant linearized equation:
$\dot{P} = A P + P A^\top + B Q B^\top$

with $A, B$ computed from measured inputs (not the estimated state).
Measurement Update:
- Compute group-based innovation (e.g., $z = \log(X^{-1} \hat{X})^\vee$ ).
- Linearize measurement model w.r.t. group error.
- Apply the Kalman gain and update state and covariance.
Reset Step (if needed):
- After update, re-center error at zero by shifting the estimate (especially when adopting handedness conventions, see below).

This structure allows implementation using conventional EKF codebases with modifications to embrace group operations, logarithm/exponential maps, and appropriate Jacobians.

6. Equivalence of Left and Right Invariant Formulations

A commonly discussed aspect is the choice between left- and right-invariant IEKF. Theoretical and empirical findings rigorously establish that, once a proper reset step is implemented after each measurement update, the left and right IEKF algorithms are mathematically equivalent (Ge et al., 6 Jul 2025). This reset step recenters the concentrated Gaussian after each update, with the following transformation: $X_{\text{new}} = X^- \exp(\mu^+)$ and

$\Sigma_{\text{new}} = J \Sigma^+ J^\top,$

where $J$ is the Jacobian of the exponential map evaluated at the update. The equivalence is formalized as

$\mu_r = \operatorname{Ad}_{x_l} \mu_l, \quad \Sigma_r = \operatorname{Ad}_{x_l} \Sigma_l (\operatorname{Ad}_{x_l})^\top,$

where $\operatorname{Ad}_{x_l}$ is the adjoint of $x_l$ .

The implication is that the supposed benefits of matching the filter's handedness to the measurement model vanish when the full correct procedure is obeyed. Both left and right IEKF variants yield identical filtered distribution and asymptotic performance (Ge et al., 6 Jul 2025).

7. Implications, Extensions, and Outlook

The invariant Kalman filter paradigm marks a qualitative advance for nonlinear filtering in geometric domains:

Consistency: Error covariance matches estimation error statistics even under large deviations, especially due to the log-linear property and autonomy of error dynamics (Barrau et al., 2014, Zhang et al., 2017, Vedadi et al., 5 Jan 2024).
Robustness: Reduced risk of filter divergence from poor initializations or ill-tuned linearizations, as Jacobians and gains are trajectory- and estimate-independent (Barczyk et al., 2014, Pavlasek et al., 2021, Benham et al., 12 Jun 2025).
Flexibility: Extension to iterated variants (e.g., IterIEKF) restores linear KF-like properties in the low-noise or deterministic measurement regime, ensuring direct satisfaction of constraints and minimization of covariance in measured directions (Goffin et al., 16 Apr 2024).
Hybrid Methods: Neural-augmented IEKF variants exploit the invariant structure as a prior and correct for strong unmodeled nonlinearities using learned corrections in the Lie algebra, preserving group-theoretic consistency (Lee et al., 1 Mar 2025).

The principles underlying invariant Kalman filtering offer a theoretically rigorous and computationally efficient foundation for estimation problems in robotics, navigation, and human motion, especially where state naturally evolves on manifolds. The equivalence of left/right IEKF, strategies for partial and noise-free measurements, and extensions to distributed scenarios or hybrid learning-filtering systems continue to expand the applicability and impact of this methodology.