Proprioceptive Simultaneous Base and Terrain Elevation Estimation for Legged Robot Locomotion

TL;DR A blind legged robot builds a height map from its own footsteps and feeds it back to its state estimator, like loop closures in SLAM. This stops vertical drift and cuts trajectory error by 22.1% against the best proprioceptive baseline on 296 m of real ANYmal-D data.
What they do
With only an IMU, joint encoders and foot forces, a legged robot cannot observe its global position, and its height estimate drifts. Vertical drift is the dangerous part: swing legs overextend or touch down late. SBEE treats the ground as a landmark. Every footstep is a height measurement that goes into a local elevation map, and stepping on terrain the map already knows corrects the base height, much like a loop closure in SLAM.
How it works
- Filter. A left-invariant EKF on SE2(3) plus IMU biases. Instead of adding one contact state per leg, it uses a single kinematic base-velocity measurement weighted by per-foot contact confidence, so the state size does not depend on the number of legs.
- Contact confidence. Each foot’s vertical ground reaction force, normalized by the average load across all feet, gives a continuous value between 0 and 1.
- Map. An M x M grid of height cells, each a scalar Kalman filter, recentered in O(1) by toroidal indexing. Updates spread to neighbouring cells, using the surface normal for flat feet.
- Coupling. A Schmidt-Kalman partial update lets the map correct only the vertical base position. A bad map cell can bias the height but cannot touch attitude, velocity or biases.
Results
- In MuJoCo, the terrain update cuts vertical RMSE by 96.4% (Go2 on a slope) and 96.8% (Go2 on rough terrain). Final vertical drift on those runs falls from 0.649 m to 0.020 m and from 0.887 m to 0.019 m.
- On the 296 m ANYmal-D GrandTour CYN-1 sequence, ATE is 1.062 m: 22.1%, 24.4% and 53.2% lower than the invariant smoother, IEKF and MUSE.
- Translational RPE over 1 m segments is about 19-21% worse than IEKF and the smoother.
- A cycle takes 0.162 ms on a Raspberry Pi 5.
Limitations
The method assumes static, rigid terrain, and it bounds only the vertical axis: horizontal position and yaw still drift. The force-based contact confidence cannot detect slippage. All evaluation is offline, in simulation or on a recorded dataset, with no closed-loop runs on hardware.
Figures




Abstract
Legged robots in visually degraded environments must rely exclusively on proprioceptive sensing, under which global position and yaw are unobservable. Vertical drift is particularly detrimental, as it corrupts the height reference required by swing-leg trajectories and can destabilize the robot. We treat the terrain itself as a persistent proprioceptive landmark: every footstep provides a height measurement that anchors the vertical axis. We propose Simultaneous Base and Elevation Estimation (SBEE), a real-time open-source framework in which a Left-Invariant Extended Kalman Filter (LI-EKF) for floating-base estimation and a grid-based elevation map are coupled in a bidirectional loop, mirroring SLAM with terrain elevation in place of landmarks. Stable foot contacts insert uncertainty-aware height samples into the map, and previously mapped cells are fed back as uncertainty-weighted vertical pseudo-measurements that act analogously to loop closures. Unlike contact-aided invariant filters, the LI-EKF consumes a kinematic velocity measurement instead of augmenting the state with a contact model per leg, so its state dimension is independent of the number of legs. We evaluate SBEE in MuJoCo on quadrupedal and bipedal platforms and on the ANYmal-D GrandTour dataset over 296 m of locomotion, reducing the absolute trajectory error by 22.1% relative to the best of three state-of-the-art proprioceptive estimators.
Summaries are written automatically from each paper's text. Figures and videos come from the paper and its project page; all credit goes to the authors.
