Legged Robot State Estimation Using Invariant Neural-Augmented Kalman Filter with a Neural Compensator

TL;DR

The paper tackles state estimation for legged robots by augmenting the invariant $InEKF$ with a neural compensator operating on the Lie group $SE_2(3)$ to correct nonlinear error dynamics. It introduces the $SE_2(3)$ Group Generation Network (SEGGN), which processes time-series data to produce Lie-algebra corrections that are exponentiated back to $SE_2(3)$ and applied to the InEKF estimate, yielding the final compensated state. Experimental results across multiple terrains, simulators, and a real-world setup show consistent improvements in rotation, position, and velocity estimation over purely model-based, purely learning-based, and prior hybrid methods, with strong invariance properties and real-time capability. The approach demonstrates a meaningful advance in robust, real-time legged robot state estimation under significant nonlinearities and contact challenges, while acknowledging the need for task- and robot-specific retraining and broader real-world validation.

Abstract

This paper presents an algorithm to improve state estimation for legged robots. Among existing model-based state estimation methods for legged robots, the contact-aided invariant extended Kalman filter defines the state on a Lie group to preserve invariance, thereby significantly accelerating convergence. It achieves more accurate state estimation by leveraging contact information as measurements for the update step. However, when the model exhibits strong nonlinearity, the estimation accuracy decreases. Such nonlinearities can cause initial errors to accumulate and lead to large drifts over time. To address this issue, we propose compensating for errors by augmenting the Kalman filter with an artificial neural network serving as a nonlinear function approximator. Furthermore, we design this neural network to respect the Lie group structure to ensure invariance, resulting in our proposed Invariant Neural-Augmented Kalman Filter (InNKF). The proposed algorithm offers improved state estimation performance by combining the strengths of model-based and learning-based approaches. Project webpage: https://seokju-lee.github.io/innkf_webpage

Figures (10)

• Figure 1: The SE$_2$(3) Group Generation Network (SEGGN) consists of three steps: (1) generating the weights for the elements of $\mathfrak{se}_2$(3), (2) performing a linear combination of the TCN outputs with the elements of $\mathfrak{se}_2$(3), and (3) applying exponential mapping to the results from step (2).
• Figure 2: The overall architecture of the Invariant Neural-Augmented Kalman Filter (InNKF). InNKF involves the predict and update step of InEKF and Neural Compensator (NC) that reduces error values obtained which is designed to SE$_2$(3) Group Generation Network (SEGGN).
• Figure 3: Training process of the Neural Compensator (NC): The dataset is collected in 50 time-step sequences, where state estimates are obtained at each time step using the InEKF. The right-invariant error is computed and labeled only for the final time step. Based on this labeled error, the SEGGN is then trained using the output and a loss function.
• Figure 4: Comparison of addition and multiplication operation time depending on the batch size. This graph represents the efficiency of computation in lie algebra.
• Figure 5: The visual and numerical results of the trained SEGGN. The first row illustrates the transformed coordinates based on the SEGGN output. The second row presents numerical evaluations, including the Frobenius norm error, calculated as $||\textbf{R}^\top\textbf{R} - \textbf{I}||_F$, and the determinant of R, both shown as histograms.