Invariant EKF: Robust Lie Group Estimation

• Invariant Extended Kalman Filter is a nonlinear state estimation method that leverages Lie group symmetries to achieve log-linear error propagation.
• It exploits group-affine dynamics, offering robust performance in legged robot kinematics and human biomechanics even under sensor misalignment.
• The filter ensures state-independent covariance evolution and efficient real-time performance, outperforming traditional EKFs in convergence and stability.

The Invariant Extended Kalman Filter (InEKF) is a specialization of nonlinear state estimation leveraging matrix Lie group symmetries, with error dynamics and linearizations formulated to be independent of the trajectory in the ambient state space. InEKF offers substantial advantages in systems whose processes are group-affine, such as legged robot kinematics and human biomechanics with IMU-based sensor fusion. By constructing error definitions using invariant operations, typically either left- or right-invariant group multiplication, the filter achieves log-linear error propagation, state-independent covariance evolution, and greatly enhanced robustness under initialization errors and imperfect sensor placement.

1. Mathematical Foundations and Group-Affine Systems

The state space of InEKF is formulated on a matrix Lie group $G$, such as $SE(3)$ for rigid-body pose or its block-augmented extensions (e.g., $SE_2(3)$, $SE_{N+2}(3)$) that encode body orientation, velocity, position, and, when necessary, sensor placement or contact point states (Hartley et al., 2018). A generic element $X \in G$ may combine submatrices representing orientation ($R$), velocity ($v$), position ($p$), contact offsets, and possible IMU placement errors as separate $SO(3)$ and $\mathbb{R}^3$ blocks.

For autonomous error dynamics, the process model must satisfy the group-affine property: $$f_u(X_1 X_2) = f_u(X_1) X_2 + X_1 f_u(X_2) - X_1 f_u(I) X_2$$. This ensures that the evolution of an invariant error (left: $\eta^L = X^{-1}\hat{X}$ or right: $\eta^R = \hat{X} X^{-1}$) is independent of the state trajectory. Linearization via the logarithm map yields $\xi = \log(\eta)$ in the Lie algebra, for which the error propagation is precisely linear: $\dot{\xi} = A_u \xi + B_u w$, with $A_u$ and $B_u$ constant matrices determined only by the system inputs and noise (Barrau et al., 2014).

2. Filter Structure: Propagation and Update

The filter alternates between propagation and update steps:

Propagation:

Given IMU measurements ($\omega_m$, $a_m$) and group-affine system dynamics (e.g., $\dot{X} = f_u(X) - X w$, where $w$ stacks additive noise terms), the filter propagates the nominal state by integrating the group-affine evolution (e.g., $X_{k+1} = X_k\,\exp(\Omega_k \Delta t)$). The invariant error covariance $P$ is propagated through the Riccati equation using the linearized $A_u$, which remains independent of $\hat{X}$ due to the group-affine structure.

Update:

Measurements are encoded as invariant observations (e.g., right-invariant: $y = X^{-1} b + v$) with associated group-linear residuals and constant measurement Jacobians $H$. The Kalman gain $K$ is computed from $K = P H^T (H P H^T + N)^{-1}$, and the state is corrected by left- or right-multiplication of the group exponential $\exp(K \nu)$ onto $\hat{X}$ (Hartley et al., 2018, Phogat et al., 2019). For complex states, such as those including IMU misalignment parameters, the filter state is augmented with additional blocks (e.g., $R_\Delta$, $p_\Delta$), and the update equations extend naturally (Zhu et al., 2022, Zhu et al., 2022).

3. Observability and Invariant Measurement Modeling

Observability in the InEKF framework is characterized analytically using the log-linear structure of error dynamics. For systems such as legged robots with IMU and contact sensors, observable modes typically exclude global position and yaw (rotation about gravity), as verified by examining the nullspace of the linearized observability matrix $\mathcal{O}$ built from $H$ and discrete propagation matrices $\Phi = \exp(A \Delta t)$ (Hartley et al., 2018, Hartley et al., 2019). When group-affine conditions are met for both process and measurement models, no nonlinear observability analysis is needed: the unobservable directions follow directly from the group structure and measurement modality.

For human motion estimation, where IMU placement errors are present, the group is augmented to jointly estimate body state and sensor misalignment. Observability then depends on excitation: e.g., $\Delta p$ and $\Delta R$ (placement offset) become observable only if there is sufficient rotational motion (Zhu et al., 2022, Zhu et al., 2022). Measurements based on forward kinematics or contact constraints are formulated to preserve as much invariance as possible, but when the observation model is imperfect (non-invariant), linearization must occur around the current estimate, which slightly degrades the global consistency guarantees but retains robust convergence properties.

4. Practical Implementation, Robustness, and Performance

InEKF implementations leverage the analytical propagation of error and covariance, resulting in filters where Jacobians are constant and unaffected by the current estimate. This property is critical for handling large initialization errors and sensor misalignment. Experimental studies on bipedal robots (Cassie-series) and human subjects demonstrate that InEKF adapts rapidly—velocity and orientation estimates typically converge within $0.2$–$0.6$ seconds, even under significant IMU misplacement and poor initial state estimates (Zhu et al., 2022, Zhu et al., 2022, Hartley et al., 2018).

Comparative results consistently show that InEKF outperforms conventional EKF and quaternion-based EKF in terms of convergence rate, robustness to noise, and final estimation error. In particular, the InEKF is less sensitive to detuned process noise covariances, requiring less manual tuning and maintaining consistent performance across a range of noise scales (Zhang et al., 15 Aug 2025). Computational efficiency remains practical, as real-time filtering is achieved with typical evaluation times per filter update on the order of $0.6\,ms$ (Zhu et al., 2022).

5. Advanced Extensions and Theoretical Features

InEKF admits a number of advanced extensions while maintaining its symmetry-adapted error properties. These include:

• Bias Augmentation: Online estimation of IMU bias by extending the state and linearizing jointly (covariance and update are performed on the augmented error block), with only mild relaxation of invariance (Hartley et al., 2018).
• Sensor Misalignment: Explicit estimation of unknown rotation and translation offsets between the IMU frame and the physical body/measurement frame, incorporated as state blocks in the filter group (Zhu et al., 2022, Zhu et al., 2022).
• Contact Switching: Adding/removing contact points (e.g., during legged locomotion) via dynamic augmentation and marginalization in the filter state and covariance (Hartley et al., 2019).
• Hybrid Systems: InEKF error remains continuous through discrete mode transitions (e.g., foot-strike), facilitating estimation in walking and manipulation over changing contact configurations (Gao et al., 2021).
• Observability Enhancement: Integration with orientation-based measurement constraints to render additional states (e.g., base yaw) observable in dynamic environments (Gao et al., 2021).

From an analytical perspective, the log-linear autonomous error propagation ensures local exponential convergence of the estimation error under standard Kalman filter conditions, with guarantees that extend globally due to invariance. The region of attraction and convergence is uniform across the entire group—there is no practical limitation in the initialization or estimate alignment as long as process observability is present (Barrau et al., 2014, Phogat et al., 2019).

6. Comparative Analysis and Applicability

InEKF has been demonstrated in a range of contexts: scan matching SLAM (Barczyk et al., 2014), pedestrian dead reckoning (Zhang et al., 15 Aug 2025), legged robot state estimation (Hartley et al., 2018, Hartley et al., 2019), human biomechanics (Zhu et al., 2022, Zhu et al., 2022), and sensor networks with partial measurements (Benham et al., 12 Jun 2025, Pavlasek et al., 2021). Its use is generally recommended whenever:

• The system's state naturally evolves on a Lie group (e.g., pose, twist, contact ensemble).
• The process model is group-affine, enabling invariant error propagation.
• Measurement models can be formulated in invariant form, or at least are compatible with group error linearization.

Key advantages include analytical Riccati equations, exact or near-exact linearization without first-order bias, easier tuning, and robustness against large initialization errors and imperfect sensor placement. InEKF uniquely couples deep geometric consistency with practical stability, facilitating real-time application and delivering reliable state estimation for complex, highly nonlinear robotic and biomechanical systems.