Invariant/Geometric EKF on Lie Groups

• Invariant/Geometric EKF (IEKF/GEKF) is a nonlinear estimator defined on matrix Lie groups that exploits intrinsic error definitions to maintain system symmetries.
• It propagates states on the group manifold and linearizes error dynamics independently of current estimates, ensuring robust convergence and consistency.
• Widely used in robotics, navigation, and SLAM, the filter demonstrates superior accuracy, easier tuning, and enhanced performance over classical EKF methods.

An Invariant/Geometric Extended Kalman Filter (IEKF/GEKF) is a nonlinear observer and stochastic estimator for systems whose state evolves on a matrix Lie group. These filters exploit the Lie group’s geometry to preserve system symmetries and ensure robust, consistent state estimation, especially for robotic, navigation, and SLAM problems where the Euclidean-state assumption is fundamentally inadequate. The core premise is to define and propagate a state-space error intrinsically on the Lie group, achieving error dynamics and linearizations that are independent of the current estimate. The resulting filters exhibit increased consistency, simpler tuning, and superior theoretical convergence properties when compared to classical EKF or multiplicative variants.

1. Mathematical Foundations and System Model

The IEKF/GEKF operates on systems where the state $X_t$ belongs to a matrix Lie group $G\subset\mathbb{R}^{n\times n}$ and evolves according to a control-driven, stochastic process. In the continuous-time Stratonovich form, the system dynamics are written as

$$\mathrm{d}X_t = X_t \Bigl[a(X_t, u_t)\ \mathrm{d}t + B(X_t, u_t)\circ\mathrm{d}W_t\Bigr]$$

where $a:G\times\mathbb{R}^l\to\mathfrak{g}$ encapsulates the drift on the Lie algebra, $$B:G\times\mathbb{R}^l\to \mathfrak{g}\otimes\mathbb{R}^s$$ maps process noise, and $W_t$ is a Brownian motion with covariance $Q$. This formulation, using the Stratonovich integral, guarantees that $X_t$ remains on the group manifold. It is critical in navigation, SLAM, and inertial estimation, as exemplified in $SE_2(3)$-based inertial navigation, visual-inertial odometry, and scan-matching SLAM frameworks (Maurer et al., 2 Jun 2025, Zhang et al., 15 Aug 2025, Barczyk et al., 2015).

2. Invariant Error Definitions and Linearization

The IEKF departs from traditional EKFs by defining the estimation error on the group via either:

• Left-invariant error: $\eta^L = X_n^{-1} X$
• Right-invariant error: $\eta^R = X X_n^{-1}$

Here, $X_n(t)$ is the noise-free nominal trajectory. These are mapped to the Lie algebra by $\xi = \log(\eta^L)$ (for left) or $\bar\xi = \log(\eta^R)$ (for right). Owing to the group-affine property of the dynamics—$f(\eta X)=\eta f(X)+f(\eta) - \eta f(I)$—the error evolution on the algebra is

$$\dot{\xi} = A(X_n, u) \xi + B(X_n,u)\mu + O(\|\xi\|^2, \|\mu\|^2)$$

with $A$ and $B$ determined by group-affine derivatives. Crucially, in typical geometric/invariant navigation problems (e.g., on $SE_2(3)$), the linearized error Jacobians $A$ and measurement Jacobians $H$ are independent of the current estimate (Maurer et al., 2 Jun 2025, Barrau et al., 2022, Phogat et al., 2019).

3. Filter Propagation, Measurement Update, and Covariance Reset

Propagation: The mean is propagated on the group as: $\dot{\hat X} = \hat X\, a(\hat X, u)$ and the error covariance on the algebra is advanced using

$$\dot{P} = A(\hat X, u) P + P A(\hat X, u)^{\top} + B(\hat X, u) Q B(\hat X, u)^{\top}$$

For discrete systems, the exponentials of $A$ are employed to generate state transitions (Maurer et al., 2 Jun 2025, Li et al., 2022).

Measurement update: For a left-invariant output $y = c(X, u) + D(X,u)\eta$, the Kalman gain is: $$K = P^{-} C(\hat X^-,u)^\top \bigl(C(\hat X^-,u)\,P^{-}\,C(\hat X^-,u)^{\top} + D(\hat X^-,u)\,N\,D(\hat X^-,u)^{\top}\bigr)^{-1}$$ The innovation drives a Lie algebraic increment $\zeta$ that is mapped to the group via the exponential map, yielding the state update: $\hat X_k^+ = \hat X_k^- \exp(\zeta)$ Full-order covariance reset: Post-update, the distribution must be re-centered about the new mean. The reset is performed with the group’s right Jacobian $J_\zeta$: $P^{+} = J_{\zeta} P^{\oplus} J_{\zeta}^\top$ where $P^{\oplus}$ is the post-update covariance before reset. This procedure is essential; without full-order reset, invariance is broken and performance deteriorates, as validated by Monte Carlo studies (Maurer et al., 2 Jun 2025, Ge et al., 6 Jul 2025).

4. Left–Right Equivalence and Filter Consistency

A central theoretical result is the equivalence of left- and right-invariant IEKF/GEKF provided the full-order covariance reset is applied. Analyses show that, under proper reset, the left- and right-invariant filters yield identical trajectories, innovations, and covariances up to adjoint transformations. The state propagation, gain computation, and update steps mirror one another; all observable quantities and estimation statistics coincide (Maurer et al., 2 Jun 2025, Ge et al., 6 Jul 2025).

Additionally, the autonomous error-dynamics—independence from the estimation path—lead to provable consistency, globally uniform convergence radii, and robustness to initial error, a marked contrast to classical EKF whose behavior may critically depend on current trajectory and linearization point (Barrau et al., 2014, Phogat et al., 2019).

5. Geometric EKF and Intrinsic Filtering on General Manifolds

The Geometric Extended Kalman Filter (GEKF) (Ge et al., 6 Jun 2025) generalizes the invariant filtering approach to arbitrary smooth manifolds equipped with an affine connection, giving a natural framework for defining Gaussian-like distributions, parallel transport, and invariant error. The geometric approach prescribes explicit propagation and update operations that respect manifold geometry, employing the exponential map, curvature, and parallel transport for error and covariance management. In the special case of Lie groups (where parallel transport and the exponential map admit closed-form expressions), the GEKF reduces to the classical IEKF/GEKF recipe, and propagates the error covariance via adjoint (right or left) actions (Ge et al., 6 Jun 2025, Maurer et al., 2 Jun 2025).

6. Application Domains and Empirical Insights

The IEKF/GEKF framework has been successfully applied in numerous domains:

• Pedestrian and bipedal robot dead reckoning on $SE_2(3)$ (Zhang et al., 15 Aug 2025)
• Inertial navigation and pose estimation, including IMU-GNSS fusion, in $SE_2(3)$ and more general group-structured states (Luo et al., 2021, Barrau et al., 2022, Phogat et al., 2019)
• SLAM and scan-matching-based localization, with experimental deployments validating centimeter-level accuracy and robustness to large pose errors (Barczyk et al., 2015, Barczyk et al., 2014)
• Visual-inertial odometry, particularly with multi-feature (points, lines, vanishing points) integration on right-invariant extended state (Hua et al., 2023)
• Extended Kalman filtering with hard constraints (noise-free pseudo-measurements) (Goffin et al., 2024)
• Recent work demonstrates the efficacy of iterated versions (IterIEKF), Gauss-Newton update refinements, and geometric improvements to reset/propagation, yielding further gains in convergence and accuracy (Goffin et al., 2024, Ge et al., 6 Jun 2025).

Empirical results consistently show that the invariant/geometric variants outperform classical EKF or multiplicative alternatives in terms of accuracy, consistency (ANEES/NEES near one), and ease of tuning under stochastic uncertainties and large initial errors (Maurer et al., 2 Jun 2025, Zhang et al., 15 Aug 2025, Barczyk et al., 2015, Hua et al., 2023).

7. Notional Impact and Theoretical Significance

The IEKF/GEKF delivers a principled, mathematically justified extension of the Kalman filtering paradigm to nonlinear, non-Euclidean state spaces that arise in robotics, autonomous navigation, and SLAM. Its compatibility with system symmetries (“group-affine” property), autonomy of error dynamics, and state-independent filter Jacobians lead to enhanced theoretical guarantees:

• Uniform local stability and convergence (Deyst–Price conditions) (Barrau et al., 2014, Phogat et al., 2019)
• Invariance under coordinate/handedness choice given correct reset (Maurer et al., 2 Jun 2025, Ge et al., 6 Jul 2025)
• Absence of spurious information along unobservable symmetry directions (Brossard et al., 2019, Hua et al., 2023)
• Analytical tractability for observability and consistency analysis in presence of constraints, stochasticity or rich feature sets (Goffin et al., 2024, Li et al., 2022) The geometric perspective further generalizes the filter to arbitrary smooth manifolds with affine connection, using manifold “concentrated Gaussians” and parallel transport for filtering (Ge et al., 6 Jun 2025).

In total, the IEKF/GEKF paradigm unifies a broad class of group-affine, symmetry-exploiting nonlinear estimation problems, establishing it as a foundational methodology for consistent stochastic filtering on manifolds in contemporary research (Maurer et al., 2 Jun 2025, Ge et al., 6 Jun 2025, Barrau et al., 2022).