💻 computer science

Four Simple Proprioceptive Estimators for Legged Robots

This paper presents four increasingly expressive proprioceptive state estimators for legged robots that mitigate IMU drift by leveraging intermittent foot contacts, ranging from a contact-aided invariant EKF to a fixed-lag smoother, all implemented in GTSAM and ROS2 for reproducibility.

Original authors: Frank Dellaert, Chiyun Noh, Varun Agrawal, Ayoung Kim

Published 2026-05-25

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Frank Dellaert, Chiyun Noh, Varun Agrawal, Ayoung Kim

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine a legged robot (like a four-legged dog) trying to walk through a dark room without bumping into things. To know where it is, the robot relies on two main sources of information, but both have flaws:

The "Gyroscope" (IMU): This is a sensor that spins and shakes to tell the robot how it's moving. It's very fast and works all the time. However, it's like a drunk person trying to count their steps; over time, it gets slightly confused, and its estimate of where the robot is starts to drift away from reality.
The "Feet": When a foot touches the ground, it stops moving relative to the floor. This is a perfect, solid anchor. But the robot only has this information when a foot is actually planted. When the foot is swinging in the air, this anchor disappears.

The paper "Four Simple Proprioceptive Estimators for Legged Robots" is essentially a recipe book for four different ways to combine these two messy sources of information to keep the robot from getting lost. The authors built four versions of a "calculator" (an estimator) that gets progressively smarter at using the feet to correct the drifting gyroscope.

Here is the breakdown of the four approaches, using simple analogies:

1. The "One-Step-at-a-Time" Calculator (Invariant EKF)

The Analogy: Imagine you are walking in the dark, counting your steps (the gyroscope). Every time you feel your foot hit the ground, you pause, look at your step count, and make a tiny correction to your mental map.
How it works: This is the simplest method. It predicts where the robot is based on the gyroscope, and the moment a foot touches the ground, it instantly fixes the error. It treats the foot as a temporary landmark.
The Catch: It does this correction one foot at a time, in a specific order. If two feet touch the ground at the exact same time, it might process them sequentially, which isn't perfectly efficient.

2. The "Group Think" Calculator (Graph-Update Filter)

The Analogy: Now, imagine that when your feet hit the ground, instead of correcting your map one by one, you gather all the feet that are touching the ground right now and ask them to agree on a single, perfect correction together.
How it works: This version is almost the same as the first one, but when the robot updates its position, it looks at all the active feet simultaneously. It solves a tiny math puzzle to find the best spot that satisfies all the feet at once.
The Result: It's slightly more accurate because it doesn't force an arbitrary order on the corrections, but it's still a "one-step" approach.

3. The "Sliding Window" Calculator (Fixed-Lag Smoother with One Bias)

The Analogy: Imagine you are walking in the dark, and you keep a short "memory tape" of the last few seconds. Instead of just looking at this step, you look at the last few steps and all the footprints you've made recently. You realize, "Wait, if I look at the last 5 seconds of movement, my gyroscope seems to be drifting a little bit to the left."
How it works: This method keeps a small window of history (the last few seconds). It treats every time a foot touched the ground as a specific "event" in history. It uses all the data in that window to smooth out the path. It assumes the gyroscope's error (the "drunk" part) stays roughly the same during that short window.
The Result: This is much better at preventing the robot from drifting over long distances because it can look back and fix mistakes using future information.

4. The "Adaptive" Calculator (Fixed-Lag Smoother with Evolving Bias)

The Analogy: This is the most sophisticated version. It's like the robot realizing, "My gyroscope isn't just slightly off; it's getting more off the longer I walk, maybe because it's getting hot or tired." So, instead of assuming the error is the same for the last 5 seconds, it assumes the error is slowly changing and evolving.
How it works: It keeps the "sliding window" of history, but it allows the gyroscope's error to change over time within that window. It models the error as a journey rather than a static number.
The Result: This is the most flexible and powerful method for long walks or changing environments, as it can adapt to the sensor getting "tired."

What Did They Find?

The authors tested these four calculators on a real robot (Boston Dynamics' Spot) walking through various terrains, including flat floors, stairs, and slopes.

The Good News: All four methods were excellent at keeping the robot from getting lost, especially compared to other existing methods. They proved that you don't need expensive cameras or lasers (which are often used for this) if you have a good math model for the feet and the gyroscope.
The Trade-off:
- The simplest methods (1 & 2) were very stable but didn't correct long-term drift as well.
- The sliding window methods (3 & 4) were the best at keeping the robot on the right path over long distances.
- The Twist: The most complex method (Method 4, the "Adaptive" one) was great at keeping the robot moving forward correctly, but sometimes it got a little confused about height (up and down) on steep stairs. The slightly simpler "Sliding Window" (Method 3) was sometimes better at keeping the vertical height accurate.

The Bottom Line

The paper shows that by changing how a robot "thinks" about its feet and its internal sensors—moving from a simple step-by-step fix to a memory-based, history-smoothing approach—you can make legged robots much more reliable. They didn't just invent a new sensor; they invented a better way to do the math with the sensors they already have.

All four of these "calculators" are now available as open-source code for other researchers to use and improve upon.

Technical Summary: Four Simple Proprioceptive Estimators for Legged Robots

Problem Statement

Legged robots rely on Inertial Measurement Units (IMUs) for high-rate motion estimation. However, consumer-grade IMUs suffer from noise that causes significant integration drift over time. While feet provide intermittent geometric constraints through contact with the environment, these constraints are only available during the stance phase. The core challenge addressed by this paper is how to effectively combine high-rate inertial data with intermittent foot-contact constraints to mitigate drift and achieve robust proprioceptive state estimation (attitude, position, velocity, and IMU biases) without relying on exteroceptive sensors like LiDAR or cameras.

Methodology

The authors propose an incremental development of four state estimators, all leveraging a contact-aided invariant filtering framework on the Lie group $SEK(3)$. The floating-base state $X^N_B$ is defined as $(R^N_B, p^N_B, v^N_B)$ , representing orientation, position, and velocity in the navigation frame.

The four variants are:

Invariant EKF (Inv. EKF):
- Foundation: Based on the contact-aided invariant EKF by Hartley et al. (2020) and the invariant filtering paradigm of Barrau and Bonnabel (2017).
- State Representation: Uses a semidirect-product Lie group $SE_{k+2}(3)$ where the state includes the base pose/velocity and $k$ footholds stored as navigation-frame points. Perturbations are defined in the body frame.
- Prediction: Uses a left-linear Lie-group prediction model driven by IMU samples, decomposing motion into gravity, autonomous drift, and body-frame IMU increments.
- Update: Sequential EKF correction. Contact updates are event-driven (triggered by touchdown or a periodic timeout) rather than running at the full joint-angle rate.
- Contact Handling: Treats stance feet as temporary landmarks. When a foot lifts off, its state is marginalized out of the fixed-size state vector.
Graph-Update Filter (Inv. IEKF):
- Modification: Retains the same prediction model and invariant state representation as the EKF.
- Update Mechanism: Replaces the sequential EKF measurement update with a small, local nonlinear factor graph solved via Levenberg-Marquardt optimization.
- Benefit: Simultaneous contacts are solved jointly rather than sequentially, allowing for symmetric handling of multiple stance feet at a single time step.
Fixed-Lag Smoother with Single Bias (FL-Single):
- Modification: Extends the local graph concept to a short time window (fixed-lag).
- State Representation: Moves from a fixed-size state to explicit navigation-frame landmark variables for "contact episodes" (distinguishing between different contact instances of the same foot).
- Inertial Modeling: Uses preintegrated IMU factors (Forster et al., 2017) to accumulate high-rate IMU data between contact events.
- Bias: Assumes a single, persistent IMU bias variable shared across the entire window.
Combined Fixed-Lag Smoother with Evolving Bias (FL-Combined):
- Modification: Relaxes the constant bias assumption.
- State Representation: Models the bias as an evolving trajectory (Markov chain/random walk) where each contact event has its own pose, velocity, and bias variables.
- Inertial Modeling: Uses extended preintegrated factors that connect neighboring base states and neighboring bias states, modeling bias evolution.

Implementation Details:

The estimators do not use joint angles as measurements; they assume a front-end provides 3D foot points in the body frame.
Contact updates are triggered by touchdown events or a maximum time interval ( $\Delta t_{max} = 100$ ms).
All variants are implemented in GTSAM and provided as a ROS2-compatible package.

Key Contributions

Incremental Framework: A systematic progression from a standard invariant EKF to a fixed-lag smoother with evolving bias, isolating the impact of graph-based updates and bias modeling.
Simplicity and Reproducibility: The authors deliberately avoid complex kinematic-chain factor graphs or joint-angle measurements, focusing on a clean, reproducible implementation available in GTSAM.
Event-Driven Updates: The use of touchdown-triggered updates (rather than high-rate joint updates) is shown to be beneficial for reducing drift.
Open Source: Full implementation of all four variants is provided to facilitate further research in proprioceptive legged odometry.

Experimental Results

The estimators were evaluated on the GaRLILEO dataset (using a Boston Dynamics Spot platform) across indoor and outdoor sequences, including flat loops, stairs, slopes, and tunnels. The evaluation compared the four GTSAM variants against existing baselines: Pronto, MUSE, and Holistic Fusion.

Performance Highlights:

Overall Accuracy: The GTSAM variants generally achieved lower accumulated errors (Absolute Pose Error - APE) than the baselines, particularly in translational and vertical components.
2D Sequences (Flat): The fixed-lag variants (FL-Single and FL-Combined) consistently reduced translational and vertical drift compared to filtering variants and baselines.
3D Sequences (Elevation Change):
- FL-Combined often achieved the best relative pose error (RPE) and absolute pose error (APE) in horizontal dimensions, particularly on complex sequences like "Overpass."
- Trade-off in Vertical Accuracy: While FL-Combined improved horizontal accuracy, it sometimes exhibited higher vertical drift (APEz) on stair and slope sequences compared to FL-Single or the filtering variants. The authors suggest that the additional degrees of freedom in the evolving bias model may allow the optimizer to absorb transient contact-induced vertical impacts into the bias estimate, which is suboptimal for vertical accuracy in these specific scenarios.
- FL-Single emerged as a stable middle ground, improving accumulated errors over filters while maintaining more robust vertical accuracy than FL-Combined on certain elevation-change sequences.

Significance and Claims

The paper claims that these "simple" estimators provide a competitive baseline for proprioceptive legged odometry without requiring exteroceptive sensors. The significance lies in demonstrating that:

Invariant Filtering: The $SEK(3)$-based invariant filtering representation provides a strong foundation with state-independent prediction Jacobians.
Smoothing Benefits: Fixed-lag smoothing effectively suppresses accumulated translational and vertical drift by retaining recent contact episodes and inertial constraints within a sliding window.
Practical Trade-offs: There is a distinct trade-off between estimator expressiveness and vertical accuracy. While evolving bias models (FL-Combined) improve relative and horizontal accuracy, they may not be uniformly beneficial for vertical drift across all terrains. The authors suggest that more careful IMU bias modeling could mitigate this issue.

The work serves as a reproducible benchmark and a modular foundation for future research in legged robot state estimation.