Adaptive Invariant Extended Kalman Filter

Updated 26 October 2025

Adaptive Invariant Extended Kalman Filter is a state estimation method that uses invariant error dynamics on matrix Lie groups to ensure local stability under varying conditions.
It exploits group-affine properties by decoupling error evolution from observer trajectories, enabling linear error analysis via Lie algebra mapping.
The adaptive mechanism actively estimates noise covariances, enhancing resilience against high initial errors, dynamic uncertainties, and measurement inconsistencies.

The Adaptive Invariant Extended Kalman Filter (AIEKF) is a class of nonlinear state observers and recursive estimators designed for systems whose state evolution belongs to a matrix Lie group, offering a rigorously stable approach to state estimation in contexts where model structure, measurement quality, or contact conditions can vary over time. By exploiting group-affine system properties and invariance principles, these filters decouple error dynamics from the observer trajectory, which provides strong local stability and consistent estimation even under challenging conditions such as high initial state error, time-varying measurement quality, and slip or contact uncertainty. The adaptive extension introduces online noise covariance estimation or model selection, enabling the filter to respond to dynamic environments or unmodeled phenomena.

1. Group-Affine Invariant Error Definition and Dynamics

The theoretical foundation of the AIEKF lies in constructing an estimation error directly on the underlying matrix Lie group, which yields autonomous error dynamics not tied to the current observer trajectory. In practice, for a left-invariant observer on a Lie group $G$ , the error between estimated $\hat{\chi}_t$ and true $\chi_t$ states is defined as

$\eta_t = \chi_t^{-1} \hat{\chi}_t.$

For group-affine systems where the dynamics $f_{u_t}$ satisfy

$f_{u_t}(ab) = a f_{u_t}(b) + f_{u_t}(a) b - a f_{u_t}(Id) b,$

the error dynamics evolve autonomously:

$\frac{d}{dt} \eta_t = g_{u_t}(\eta_t),$

meaning the error evolution depends only on current error and inputs, not on the system's state trajectory. When mapped into the Lie algebra via the exponential map,

$\eta_t = \exp(\xi_t),$

the log-linear property emerges:

$\frac{d}{dt} \xi_t = A_{u_t} \xi_t,$

where $A_{u_t}$ is obtained from the first-order Taylor expansion. This yields exactly linear error dynamics in the algebra, allowing analysis with standard linear theory.

This invariance is the foundation of strong local convergence and stability: under standard conditions (analogous to the Deyst–Price theorem), the IEKF is provably locally asymptotically stable around any true trajectory. Stability radii are uniform along the trajectory, and rigorous Lyapunov functions (e.g., $V(P, \xi) = \xi^T P^{-1} \xi$ ) can be constructed for analysis.

2. Propagation and Update on Lie Groups

In the continuous/discrete-time AIEKF, propagation follows the true system dynamics on the group, while updates apply group-centered innovations:

Propagation: For left-invariant IEKF,

$\frac{d}{dt}\hat{\chi}_t = f_{u_t}(\hat{\chi}_t),$

where $\hat{\chi}_t \in G$ .

Update: When a group-invariant measurement $Y_{t_n}$ is obtained, the update is

$\hat{\chi}_{t_n}^+ = \hat{\chi}_{t_n} \exp\left(L_n [\hat{\chi}_{t_n}^{-1} Y_{t_n} - d]\right),$

with $L_n$ computed via the Riccati equation in the Lie algebra. In log-coordinates, the update resembles a classic linear Kalman filter:

$\xi_{t_n}^+ = \xi_{t_n} - L_n (H \xi_{t_n} + \text{innovation noise}),$

and the gain is determined by

$L_n = P_t H^T (H P_t H^T + \hat{N}_n)^{-1}.$

Because the error propagation is trajectory-independent and linear in the Lie algebra, classical linear observer theory applies, including conditions for exponential convergence of the (linearized) estimation error.

3. Adaptive Mechanisms: Online Covariance and Model Tuning

Adaptivity in the AIEKF is introduced through online adjustment of process or measurement noise covariances, model parameters, or measurement innovation handling. Representative mechanisms include:

Innovation-based Covariance Estimation: The innovation sequence or measurement residuals are tracked over a short window to estimate the true measurement noise covariance. For a measurement residual $e_i$ ,

$U_i = \frac{1}{m} \sum_{n=0}^{m-1} e_i(k-n) e_i(k-n)^T,$

is an empirical covariance, from which the unknown process noise covariance $Q_{uf_i}$ is isolated. This is essential for coping with time-varying slip noise or disturbances in legged robots and other mobile systems.

EM Algorithm for Covariance Tuning: For attitude estimation, maximization of the likelihood over a batch window (e.g., with the EM algorithm) allows for simultaneous adaptation of process and measurement noise covariances in the observer error dynamics, yielding robust performance in the presence of nonstationary or poorly characterized noise profiles (Pandey et al., 2 Oct 2024).
Moving Window Scaling: The adaptive filter can use the ratio of empirical and nominal covariance components to adjust the filter's sensitivity to phenomena such as ground slip or contact model deviation, ensuring neither too conservative nor too optimistic trust in the measurement model.
"Soft" Versus "Hard" Outlier Handling: Instead of binary slip rejection or measurement discard, adaptively increasing the modeled noise "softens" the filter's response, maintaining observability and numerical stability in the face of partial measurement failure or intermittent faults.

4. Application Case Studies and Performance

Concrete instantiations of the AIEKF and their properties have been demonstrated in:

Application	Adaptivity Principle	Lie Group Structure	Performance Benefit
Legged robot state estimation (Kim et al., 19 Oct 2025)	Adaptive process noise scaling per contact foot	SE₂₊ₙ(3)	Maintains accuracy despite small slips; robust to misdetected contacts or time-varying floor conditions
Attitude estimation under sensor noise (Pandey et al., 2 Oct 2024)	EM-based noise covariance estimation	S³ (unit quaternions) or SO(3)	Rapid convergence and stability under nonstationary noise; robust to gyroscope/magnetometer drift
Visual-inertial navigation (Zhang et al., 2017)	Invariant error representation and efficient marginalization	Composition of SO(3), ℝ³	Greater consistency (NEES), robust in yaw-unobservable scenarios, reduced drift
SLAM/odometry with dense mapping (Li et al., 7 Feb 2024)	Adaptive Gauss–Newton initialization, fast closed-form updates	SE₂(3)	Asymptotically optimal MMSE performance with O(n) update cost
Human motion and sensor misalignment (Zhu et al., 2022)	Augmented state for sensor placement errors	Blend of SE₂(3), SE(3)	Accurate velocity/orientation even with large initial error and uncalibrated offsets

Across these domains, invariance and adaptivity jointly confer local exponential stability, consistent error evolution, and resilience to poor parameterization, even where the standard EKF can diverge or become inconsistent.

5. Comparison with Conventional EKF and Other Nonlinear Observers

Relative to the standard EKF, which linearizes about the current observer state and employs Euclidean error metrics, the AIEKF offers:

Trajectory-Independent Linearization: The Jacobians and gains do not depend on the current state estimate, providing improved handling of large initial errors and unpredictable transients.
Group-Invariant Error Metrics: Stability analysis and performance no longer hinge on the local state coordinate chart, sidestepping parameterization ambiguities.
Robust Adaptation: Online covariance estimation or model adaptation avoids ad hoc heuristics or the need for gain scheduling, as the adaptation is intrinsically aligned with the innovation structure and system geometry.
Consistency Under Partial Observability: In scenarios with unobservable directions (e.g., yaw in inertial navigation), invariant adaptive filters suppress spurious information gain and mitigate covariance underestimation (Zhang et al., 2017).
Soft Adaptation Superior to Hard Thresholding: Adaptive noise inflation more reliably preserves estimator stability than conventional slip rejection or hard residual gating.

6. Stability and Observability Guarantees

The core mathematical result substantiating the AIEKF's reliability is the existence of uniform covariance bounds and strictly decreasing Lyapunov functions under observable and reachable linearization pairs $(A_{u_t}, H)$ . The local stability and convergence properties only require the ambient system to satisfy group-affine conditions and for the corresponding pair to satisfy Deyst–Price-type criteria (i.e., uniform observability and reachability), which hold in most mobile robotics, navigation, and tracking settings of practical interest (Barrau et al., 2014).

Comprehensive nonlinear and discrete-time observability analyses further show that real-world unobservable directions (such as yaw or global position with only velocity measurements) are preserved, and adaptive updates act only in observable subspaces, preventing false confidence or numerical instability (Teng et al., 2021).

7. Broader Implications and Research Directions

The AIEKF’s framework is extensible to a range of advanced estimation problems:

Contact- and Slip-Robust Estimation: Adaptive process noise on foot model states in legged robots (Kim et al., 19 Oct 2025, Teng et al., 2021).
SLAM and Dense Sensing: Adaptive initialization and update with $\sqrt{n}$ -consistent pose from environmental measurements for real-time large-scale mapping (Li et al., 7 Feb 2024).
Human Physiology and Wearable Robotics: On-the-fly calibration of sensor misalignment or offset (Zhu et al., 2022).
Fully Nonlinear Filtering on Manifolds: Use of geometric connections, parallel transport, and coordinate resets to construct fully intrinsic EKF variants (e.g., (Ge et al., 6 Jun 2025, Ge et al., 2023)).
Deep Learning-Enhanced Adaptive Filtering: Integration of learning-based modules (e.g., set transformers or LSTM networks) for process noise or uncertain observation modeling (Cohen et al., 18 Jan 2024, Ye et al., 2023).

The synthesis of invariance and adaptivity represents a convergence of robust geometric observer theory and modern data-driven or innovation-based adaptation, with widespread applicability in robotics, navigation, SLAM, biomechanics, and sensor fusion.

The Adaptive Invariant Extended Kalman Filter unites rigorous group-theoretic observer design with principled, online adaptation of noise and modeling assumptions, yielding a robust, consistent, and locally stable state estimator. Its mathematical foundations and empirical performance across robotics and navigation underscore its relevance for challenging real-world applications requiring resilience to uncertainty, nonlinearity, and time-varying conditions.