⚡ electrical engineering

Proprioceptive Invariant State Estimation for Humanoid Robots on Non-Inertial Ground

This paper presents a right-invariant extended Kalman filter that enables real-time, proprioceptive-only state estimation for humanoid robots on non-inertial ground, achieving significantly faster convergence and reduced position errors compared to existing methods.

Original authors: Falak Mandali, Zijian He, Yan Gu

Published 2026-06-19

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Falak Mandali, Zijian He, Yan Gu

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 humanoid robot named "Digit" trying to walk, squat, or stand still. Now, imagine the floor it's standing on isn't a solid, stationary concrete slab, but a moving, swaying, and pitching surface—like a ship in rough seas or a train car that's speeding up and slowing down.

For a robot to stay upright and move correctly, it needs to know exactly where it is and how fast it's moving. This is called state estimation.

The Problem: The "Blind" Robot on a Moving Bus

Most robots today are like passengers on a bus who assume the bus is parked. They use their internal sensors (like a sense of balance and joint angles) to guess their position, assuming the ground beneath them is perfectly still.

But if the bus (the ground) starts moving, the robot's "still ground" assumption breaks. It gets confused. It thinks it's falling when it's actually just the floor moving under it.

To fix this, scientists usually try to put a camera on the bus or a sensor on the floor to tell the robot, "Hey, the floor is moving!" But in the real world, you can't always strap a camera to a moving ship or a train. You need the robot to figure it out using only its own internal sensors (its "proprioception").

The Solution: The "Invariant" Filter

This paper introduces a new mathematical brain (an algorithm called an Invariant Extended Kalman Filter, or InEKF) that lets the robot figure out its motion relative to the moving floor without needing any outside help.

Here is how it works, using a simple analogy:

1. The "Feet as Anchors" Trick
Imagine you are standing on a moving walkway at an airport. You can't see the floor moving, but you can feel your feet. If your feet are planted firmly and not slipping, you know that whatever motion your feet feel is actually the motion of the walkway.

The Paper's Innovation: The robot has special sensors (IMUs) on its feet. The new algorithm uses the fact that when a foot is planted, it acts as a temporary anchor. By feeling the acceleration and rotation of that planted foot, the robot can mathematically calculate how the entire ground is moving, even though it has no sensors on the ground itself.

2. The "Right-Handed" Math (Invariant Filtering)
Standard math for robots often gets messy and inaccurate if the robot starts with a big guess about where it is (e.g., "I think I'm 1 meter to the left, but I'm actually 1 meter to the right").

The Paper's Innovation: The authors used a special type of math called "Invariant" filtering. Think of this like a compass that always points North, no matter how you turn your body. This method ensures that even if the robot starts with a huge error in its guess, it can correct itself very quickly and smoothly, without getting confused by the complex physics of the moving ground.

What They Tested

The researchers tested this on the Digit robot in a lab. They put the robot on a treadmill that could sway side-to-side and pitch up-and-down (simulating a moving ship).

They tested five scenarios:

Standing still.
Squatting down.
Walking.
Doing these while the treadmill was moving in different ways.

The Results: Faster and Smarter

The paper claims three main victories:

Speed: The new method corrected the robot's position 96% faster than older methods. If the robot started with a big mistake (thinking it was 1 meter away when it wasn't), it fixed that error in a fraction of a second.
Accuracy: The robot's guess of its position was 80% more accurate than the old methods. In walking tests, the error was less than 9 centimeters, even when the ground was moving wildly.
No Extra Gear: It achieved all this using only the sensors already on the robot (joint sensors and sensors on the torso and feet). It didn't need cameras or sensors on the floor.

One Extra Trick: Fixing the "Spin"

The paper also noted a small limitation: while the robot could perfectly figure out where it was and how fast it was moving, it sometimes struggled to know exactly which way it was facing (its orientation) if the ground was moving in a very specific, simple way.

To fix this, they tried a simple trick: adding more sensors to the robot's chest. By placing three sensors in a triangle on the robot's torso, the robot could build a "mental map" of its own body and perfectly figure out its direction, just like how you can tell which way a table is facing if you can see three distinct points on it.

Summary

In short, this paper teaches a robot how to walk on a moving ship using only its own sense of balance and touch. It does this by treating its planted foot as a reference point to "feel" the ground's motion, using special math to correct mistakes instantly, and proving it works better and faster than previous methods that relied on the ground being still or having extra sensors attached to the floor.

Technical Summary: Proprioceptive Invariant State Estimation for Humanoid Robots on Non-Inertial Ground

Problem Statement
Accurate, real-time state estimation is fundamental for the model-based planning and control of legged robots. While existing estimators, such as Invariant Extended Kalman Filters (InEKFs), have achieved high performance on static terrain by exploiting group-affine dynamics and invariant observation conditions, they rely on the assumption that the supporting ground is stationary. This assumption fails in non-inertial environments (e.g., moving trains, ships, or aircraft), where ground-induced translational and rotational motions introduce significant nonlinearities into system dynamics and measurement models. Ignoring these motions leads to modeling errors that degrade estimation accuracy.

Current approaches to non-inertial state estimation face practical limitations:

Absolute State Estimation: Methods that estimate the robot's absolute state in an inertial world frame often require direct, accurate knowledge of the ground's absolute pose and velocity, which is rarely available without external instrumentation.
Relative State Estimation: Recent methods that estimate the robot's state relative to a moving ground frame typically require an external IMU physically mounted on the ground to measure its motion. This is often impractical in real-world deployments due to instrumentation constraints, communication delays, and signal loss in unstructured environments.
Learning-Based Methods: While promising, learning-based estimators often require substantial training data and their generalization to non-inertial environments remains unclear.

There is a need for a fully proprioceptive estimator that can accurately track a robot's motion relative to a moving ground without relying on external sensors or explicit ground-motion measurements.

Methodology
The paper proposes a Proprioceptive Invariant Extended Kalman Filter (InEKF) designed to estimate the robot's base position and velocity relative to a dynamic ground frame using only onboard sensors (joint encoders and IMUs mounted on the torso and feet).

State Representation: The filter estimates the robot's relative base orientation ( $R_B^D$ ), position ( $p_t^D$ ), and velocity ( $v_t^D$ ) with respect to the dynamic ground frame $\{D\}$ . The state is embedded in a matrix Lie group $G$ , composed of a double spatial isometry $SE_3(3)$ and the special orthogonal group $SO(3)$.
Process Modeling:
- The filter derives continuous-time dynamics for the relative base orientation, velocity, and position.
- Crucially, it eliminates the dependency on direct ground-motion measurements (which are unavailable) by exploiting kinematic constraints at the stance foot. By utilizing measurements from a foot-mounted IMU, the filter explicitly calculates the ground-induced accelerations and angular velocities required for the process model.
- The model accounts for nonlinear effects induced by ground motion and treats foot slippage as white Gaussian noise.
- Discrete events, such as foot-landing and stance-switching, are handled via a discrete propagation step that updates the stance-foot pose using forward kinematics while maintaining continuity in the base state.
Measurement Modeling:
- The filter uses leg odometry derived from joint encoders as the measurement source.
- The measurement model is formulated to satisfy a right-invariant observation condition. This structure ensures that the error dynamics are linear and trajectory-independent in the deterministic case, promoting robust convergence even under large initial uncertainties.
Observability Analysis:
- A local observability analysis of the linearized log-error dynamics reveals that the relative base velocity is always observable.
- The relative base position is observable under general ground motions, provided the stance-foot IMU detects angular motion on at least two axes.
- The analysis indicates that base orientation is unobservable under a nominal sensing configuration (one torso IMU and one foot IMU). However, it demonstrates that augmenting the sensing configuration with multiple non-collinear IMUs on the torso allows for the reconstruction of the base orientation, thereby achieving full-state observability.

Key Contributions

Fully Proprioceptive InEKF Formulation: A novel InEKF that estimates a humanoid robot's base pose and velocity relative to a non-inertial ground frame using only joint encoders and torso/foot-mounted IMUs, eliminating the need for external ground sensors.
Explicit Nonlinear Modeling: Process and measurement models that explicitly account for ground-induced nonlinearities. The measurement model satisfies the right-invariant observation condition, enabling favorable error dynamics under large initial errors.
Observability Characterization: An analysis establishing conditions under which relative base position and velocity are observable. It further proposes a simple sensor augmentation strategy (multiple torso IMUs) to recover base orientation observability.
Experimental Validation: Hardware experiments on the Digit humanoid robot performing standing, squatting, and walking tasks on a swaying and pitching treadmill.

Experimental Results
Experiments were conducted on the Digit robot under five test cases involving various robot motions (standing, squatting, walking) and ground motions (swaying, pitching). The proposed filter (termed FRS) was compared against two baselines: SRS (a standard InEKF for static ground) and DRS (an InEKF for dynamic ground requiring an external ground-mounted IMU).

Convergence Rate: The proposed FRS achieved a 96% speedup in convergence rate compared to existing InEKFs. Specifically, FRS converged to the ground truth within 0.1 seconds for position estimation, whereas the DRS baseline required 3–5 seconds.
Accuracy:
- FRS demonstrated an 80% reduction in position estimate errors compared to existing InEKFs.
- In steady-state, FRS achieved position errors below 4 cm for standing and squatting tasks (Cases T1–T4), compared to approximately 10 cm for the DRS baseline.
- During walking on a pitching ground (Case T5), FRS maintained an average estimation error of less than 9 cm despite initial errors of up to 1 meter.
Orientation Recovery: Experiments with three torso-mounted IMUs successfully reconstructed the base orientation with mean absolute errors below 0.4° in roll, pitch, and yaw, validating the proposed augmentation strategy.
Real-Time Performance: All filters, including the proposed method, supported real-time implementation with computation times of less than 3 ms per cycle on a standard laptop.

Significance and Claims
The paper claims that the proposed approach enables accurate, real-time state estimation for humanoid robots in non-inertial environments without explicit ground motion sensing. By leveraging foot-mounted IMUs to infer ground dynamics, the method overcomes the practical limitations of requiring external instrumentation. The authors assert that the filter provides a robust solution for planning and control tasks defined relative to a moving environment, achieving superior convergence and accuracy compared to state-of-the-art baselines, even under significant initial uncertainties and large ground motions. The work validates that full-state observability can be achieved through simple sensor augmentation, making the approach viable for deployment in unstructured, dynamic environments.