Iterated Invariant EKF for Quadruped Robot Odometry

2026-04-16

AI Summary

This work addresses the challenge of state estimation for quadrupedal robots operating under nonlinear dynamics and non-Gaussian noise by proposing a proprioceptive odometry approach based on the Iterated Invariant Extended Kalman Filter (IterIEKF). For the first time, IterIEKF is introduced to the domain of legged robotics, leveraging velocity kinematic constraints during foot contact phases to achieve high-precision, environment-independent state estimation using only onboard proprioceptive sensors. By integrating an invariant filtering framework on Lie groups, an iterative update mechanism, and explicit modeling of contact constraints, the proposed method demonstrates substantial improvements in both accuracy and consistency over standard IEKF, SO(3)-based Kalman filters, and their iterated variants across both simulated and real-world datasets, while preserving an update structure compatible with classical Kalman filtering.

Technology Category

• Intelligent Robots: State Estimation
• Machine Learning: Imitation Learning & Inverse Reinforcement Learning
• Planning, Routing, and Scheduling: Replanning and Plan Repair

Application Category

• Graph Algorithms and Modeling for the Web: Efficient manipulation of static and dynamic Web-related graphs
• Systems and Infrastructure for Web, Mobile and WoT: Experiences and lessons learnt from Web-based algorithms and system deployments
• Semantics and Knowledge: Methods to enhance, augment, integrate or synergize semantic models such as knowledge graphs and LLMs

Abstract

Kalman filter-based algorithms are fundamental for mobile robots, as they provide a computationally efficient solution to the challenging problem of state estimation. However, they rely on two main assumptions that are difficult to satisfy in practice: (a) the system dynamics must be linear with Gaussian process noise, and (b) the measurement model must also be linear with Gaussian measurement noise. Previous works have extended assumption (a) to nonlinear spaces through the Invariant Extended Kalman Filter (IEKF), showing that it retains properties similar to those of the classical Kalman filter when the system dynamics are group-affine on a Lie group. More recently, the counterpart of assumption (b) for the same nonlinear setting was addressed in [1]. By means of the proposed Iterated Invariant Extended Kalman Filter (IterIEKF), the authors of that work demonstrated that the update step exhibits several compatibility properties of the classical linear Kalman filter. In this work, we introduce a novel open-source state estimation algorithm for legged robots based on the IterIEKF. The update step of the proposed filter relies solely on proprioceptive measurements, exploiting kinematic constraints on foot velocity during contact and base-frame velocity, making it inherently robust to environmental conditions. Through extensive numerical simulations and evaluation on real-world datasets, we demonstrate that the IterIEKF outperforms the vanilla IEKF, the SO(3)-based Kalman Filter, and its iterated variant in terms of both accuracy and consistency.

Problem

• state estimation
• quadruped robot
• nonlinear systems
• proprioceptive measurements
• Kalman filter

Innovation

• Iterated Invariant EKF
• proprioceptive odometry
• legged robot state estimation
• kinematic constraints
• Lie group filtering