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

Seokju Lee, Hyun-Bin Kim, Kyung-Soo Kim
Mechatronics, Systems and Control Lab, Korea Advanced Institute of Science and Technology (KAIST)
Accepted to 2025 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2025)
Paper arXiv

Supplementary Video of the Paper

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.

System Overview

System Overview Diagram
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).

SE2(3) Group Generation Network (SEGGN)

SEGGN Architecture Diagram
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).

Training Process & SEGGN Result

Training Process
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.
SEGGN Result
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 \textbf{R}, both shown as histograms.

Position Estimation Results

Position Result
Position estimation results across four terrains (stairs, slope, uneven terrain, discrete obstacle) over a time interval of 0 - 140 s. The solid black line represents the ground truth, the blue dashed line represents InEKF, and the red dashed line represents the InNKF (proposed method).

Absoulte Trajectory Error (ATE) Results

Position Result
Comparison of Absolute Trajectory Error (ATE) in state estimation among the three baselines and InNKF (proposed method) across different terrains.

Raisim Result 1

Raisim Result 1
State estimation results in Raisim for two laps around a 20 m $\times$ 20 m square path on field terrain. The black line represents the ground truth, the blue line represents InEKF, and the red line represents InNKF (proposed). (a): Bird's-eye view, (b): Side view.

Raisim Result 2

Raisim Result 2
Relative Errors (RE) in Raisim across different environments (indoor, field, mountain, hill). In each box, blue represents InEKF, green represents NN, and red represents InNKF (proposed).

Real-World Experiment

Indoor
Real-world experimental setup with gravel, wooden planks, and a cart to create unstructured terrain.
Real Result
ATE and 10 Seconds RE (Mean and Standard Deviation) of state estimation in the real world.

BibTeX

@article{lee2025legged,
  title={Legged Robot State Estimation Using Invariant Neural-Augmented Kalman Filter with a Neural Compensator},
  author={Lee, Seokju and Kim, Hyun-Bin and Kim, Kyung-Soo},
  journal={arXiv preprint arXiv:2503.00344},
  year={2025}
}