Invariant EKF Framework for Robust Navigation

• The Invariant EKF framework is a state observer method that exploits matrix Lie groups to decouple error dynamics from the true system state.
• Its log–linear property transforms nonlinear errors into a linear differential form, enabling rigorous convergence and stability using linear observer theory.
• Practical simulations in robotics and inertial navigation highlight the framework’s robustness, outperforming standard EKF techniques under significant nonlinearity.

The Invariant Extended Kalman Filter (IEKF) framework is a class of state observers that leverages the symmetry properties of nonlinear systems evolving on matrix Lie groups to achieve autonomous and log–linear error dynamics. By defining estimation errors using group-invariant constructs (either left- or right-invariant), the IEKF transforms the original nonlinear estimation problem into one where the state error propagation is decoupled from the true trajectory, allowing for the application of linear observer theory in a nonlinear geometric setting. This structural property yields rigorous local stability results and notable robustness advantages over conventional EKF designs, as demonstrated in both theoretical analysis and practical robotics and navigation applications.

1. Geometric Formulation and Invariant Error Variables

The IEKF is constructed by lifting the state-space of a nonlinear system onto a matrix Lie group $\mathcal{G}$ and defining the estimation error in an invariant manner. For a true state $\chi_t$ and estimate $\hat{\chi}_t$, the error is:

• Left-invariant: $\eta_t^L = \chi_t^{-1} \hat{\chi}_t$
• Right-invariant: $\eta_t^R = \hat{\chi}_t \chi_t^{-1}$

The error is then mapped to the Lie algebra $\mathfrak{g}$ via the exponential/logarithmic map: $\eta_t = \exp(\xi_t)$. This construction ensures that the evolution of the error depends only on the error itself and the known input, provided the system dynamics are properly structured.

The key condition on the system dynamics, denoted as “multiplicative” or group-affine, is: $f_u(ab) = a f_u(b) + f_u(a) b - a f_u(I)$ for all $a, b \in \mathcal{G}$. When this is satisfied, both left- and right-invariant errors possess the essential property of autonomy (trajectory-independence) for the error propagation.

2. Error Autonomy and Log–Linear Property

With the above geometric formulation, the invariant estimation error dynamics become: $$\frac{d}{dt} \eta_t = g_u(\eta_t), \quad g_u(\eta) = f_u(\eta) - f_u(I)\eta$$ By taking logarithms, the error mapped to the Lie algebra evolves as: $\frac{d}{dt} \xi_t = A_u\xi_t$ where $A_u$ arises from the first-order expansion of the group dynamics and linearizes $g_u(\exp(\xi))$ about $\xi = 0$. This means the invariant (logarithmic) error evolves exactly according to a linear differential equation, regardless of the underlying system’s nonlinearity—a property referred to as “log–linear” propagation.

This decoupling stands in contrast to the standard EKF, in which the error dynamics always depend (often unfavorably) on the trajectory of the true state, leading to potential divergence in highly nonlinear or poorly initialized scenarios.

3. Local Stability and Convergence Guarantees

The log–linear property allows the convergence and stability of the IEKF to be analyzed with classical linear systems tools. Specifically, using Lyapunov arguments and the Deyst–Price conditions for controllability and observability of the linearized error dynamics, the IEKF is shown to be an asymptotically stable observer around any trajectory.

The Riccati equation governing the Kalman gain computation is: $$\frac{dP_t}{dt} = A_u P_t + P_t A_u^\top + \hat{Q}_t,\quad P_{t_n}^+ = (I-L_n H)P_{t_n}$$ where $$L_n = P_{t_n} H^\top (H P_{t_n} H^\top + \hat{N}_n)^{-1}$$ and $H$ is the observation Jacobian with respect to the invariant error coordinates. The standard quadratic Lyapunov function $V(\xi) = \xi^\top P^{-1} \xi$ decreases monotonically along trajectories, guaranteeing exponential convergence in the neighborhood of the true state, with a convergence radius that is uniform along the trajectory.

4. Algorithmic Structure and Update Equations

The filter alternates between discrete-time propagation (prediction) and measurement update steps, both performed on the Lie group and its algebra:

• Propagation (log–linear phase):

$\frac{d}{dt} \xi_t = A_u\xi_t$

• Measurement update (at discrete time $t_n$):

$$\hat{\chi}_{t_n}^+ = \hat{\chi}_{t_n} \exp[L_n (\text{innov})]$$

The update in Lie algebra coordinates becomes:

$$\xi_{t_n}^+ = \xi_{t_n} + L_n(H\xi_{t_n} + \text{measurement noise})$$

This mirrors, in invariant coordinates, the classic linear Kalman filter update. The essential distinction is that the linearization and update are performed in a geometry-consistent way that respects the underlying manifold.

5. Robustness: Practical Benefits and Simulation Evidence

Practical robustness of the IEKF is demonstrated by simulation studies, particularly in mobile robotics and inertial navigation. In scenarios where standard EKF linearization is inaccurate (e.g., large heading errors, “tight” process noise), the classical EKF frequently diverges while the IEKF—with identical noise tuning—consistently converges.

Example simulation outcomes include:

• In a unicycle car with $45^\circ$ heading error, the EKF diverges, but the IEKF remains stable due to its exact log–linear propagation.
• In a spatial inertial navigation scenario with low process noise trust in inertial sensors, the IEKF exhibits proven local convergence, unlike the EKF which fails due to vanishing gains and poor linearization accuracy.

These results directly attribute the practical performance improvements to the geometry-aware error variable and the autonomy of error propagation.

6. Generalization and Domain of Applicability

The autonomy and log–linear error property are not restricted to canonical left- or right-invariant systems: any system whose dynamics satisfy the group-affine (multiplicative) condition enjoys this structure. This encompasses a wide class relevant for robotics, including:

• Nonholonomic vehicles with left- or right-invariant measurements
• Rigid-body inertial navigation with velocity, orientation, and position coupled on matrix Lie groups such as SE(2), SE(3), and SE₂(3)
• Systems where state estimation error can be naturally defined on a Lie group and mapped via the exponential/logarithm

Consequently, the IEKF methodology generalizes beyond classical invariant observers and covers most practical mobile robotics and navigation estimation problems involving group symmetries.

7. Implications and Theoretical–Practical Synthesis

By reformulating nonlinear observer design on Lie groups and defining the estimation error invariantly, the IEKF enables rigorous convergence guarantees analogous to those in linear theory while yielding practical improvements in robustness and accuracy. The autonomy property circumvents the problematic coupling between trajectory and error typical in standard EKF designs.

The IEKF highlights the value of a careful error variable selection—rooted in system geometry—when designing nonlinear observers, merging geometric control theory and filtering in a unified framework that is of immediate practical use in state estimation for robotics and navigation. Simulation and analysis confirm that exploiting the intrinsic structure of the system yields both rigorous local stability and superior behavior under challenging initial conditions and sensor configurations (Barrau et al., 2014).