Unified SRBD NMPC + MHE for ANYmal-C

What it does

One reduced-order model, used twice

The single-rigid-body dynamics are written once, symbolically, and instantiated as both the controller and the estimator. The controller decides ground reaction forces over half a second of gait; the estimator reconstructs what the controller needs to know — the base state and the disturbances acting on it — from the sensors a real quadruped actually has.

Controller — nonlinear MPC

50-stage horizon (0.5 s), 100 Hz. Chooses four 3-D contact forces subject to friction pyramids, normal-force limits and attitude bounds, with the gait-scheduled contact sequence baked into the prediction. Feeds the estimated disturbance force forward.

12 states · 12 inputs · 28 constraint rows/stage

Estimator — moving-horizon

20-node window (0.2 s), 100 Hz. Fuses IMU attitude, gyro, and leg odometry from the settled stance feet, with the base state augmented by a 6-D disturbance wrench so the push is observable and can be fed forward.

18 states · 6 disturbance inputs · 21 outputs

Framework

Closed-loop architecture

Every 10 ms the estimator turns the sensors into a state and a disturbance estimate; the planner turns that into references, per-stage parameters and swing targets; the controller returns the ground reaction forces; a 500 Hz leg controller maps them to joint torques.

Shared model

Dynamics, Jacobians, constraints and outputs are emitted once from a SymPy source with common-subexpression elimination, then compiled into both the controller and the estimator — they agree bitwise for identical inputs.

Real-time iteration

One Newton-type iteration per sample, split into a preparation phase that runs before the measurement and a feedback phase that applies within microseconds of it — the controller's first force lands 0.11 µs after the estimate.

Disturbance feed-forward

The estimated force is fed to the controller; the estimated torque (swing-leg reaction at gait frequency) is deliberately not — the ablation shows why.

Results

Closed-loop performance

All plots are from a single 8-second run with sensor noise and no ground-truth feedback. Shaded bands mark the 40 N lateral push (red) and the turn (blue).

Velocity tracking

Forward speed: command, true and estimate

command true estimate

The body tracks the forward command through the push and into the turn.

Attitude stability

roll pitch

Roll stays within ±5° and pitch within ±3.6° across push and turn.

Disturbance estimation

applied push estimated

A 40 N push is recovered at 39.5 N by the augmented disturbance state.

Push recovery — why feed-forward matters

Lateral drift with and without compensation

no compensation force feed-forward

Feeding the estimated force forward cuts the push-induced drift from 0.74 m to 0.15 m.

Estimation accuracy

RMS error over the walking phase — from proprioception only.

Real-time timing vs. horizon

preparation feedback

Cost grows linearly in the horizon; the 50-stage controller uses ~9% of a 10 ms budget.

The full scenario, in numbers

Quantity	Result	Condition
Attitude estimation error (roll/pitch/yaw)	1.4 / 2.6 / 1.7 mrad	rms, walking phase
Velocity estimation error	9 / 10 / 7 mm/s	proprioception only
Base height regulation	0.452 ± 0.002 m	0.45 m commanded
Roll / pitch excursion	≤ 0.089 / 0.063 rad	whole run
40 N push — estimate & drift	39.5 N · 0.15 m	vs 0.74 m uncompensated
Controller cycle (N=50)	0.56 ms + 56 µs	prep + feedback, 100 Hz
Estimator cycle (M=20)	0.24 ms + 37 µs	1.1 µs instantaneous output
Robustness	5 / 5 seeds · to 2× noise	full scenario, no fall

Achievements

What this project demonstrates

A matched controller–estimator pair on one model. The same single-rigid-body dynamics drive both the MPC and the MHE, so the estimated quantities are exactly the ones the controller consumes — verified to agree bitwise between the two instantiations.
Full output feedback from proprioception. The loop runs with no ground-truth state: attitude to a couple of milliradians and velocity to a centimetre per second from IMU and leg odometry alone, with settled-stance gating and contact-point kinematics.
An observable, feed-forwardable disturbance. Carrying the external wrench as a random-walk state (rather than a penalised estimator input) makes a standing push observable within ~0.3 s and turns a 0.74 m knock-down into a 0.15 m sway.
Real-time by a wide margin. The complete estimate-then-control cycle costs ~0.9 ms median and ≤2.1 ms worst case against a 10 ms budget, in plain C on a single shared core.
Robust and reproducible. Five independent noise seeds complete the push-and-turn scenario; every figure and number is regenerated from released data by a single script.
A solver-agnostic framework. The whole pipeline talks to the solvers through a small documented C interface, so any real-time-iteration package can be dropped in behind it.

On the solver. The NMPC and MHE solvers are my own — a constrained real-time-iteration NMPC solver and its moving-horizon-estimation variant, developed as an extension of my master's thesis on SLQP-MPC. That work is ongoing and unpublished, so the solver algorithms are not part of this project; what is public is the modelling, the simulation framework, the architecture and the results shown here.

A unified single-rigid-body NMPC + moving-horizon estimator for ANYmal-C

One reduced-order model, used twice

Controller — nonlinear MPC

Estimator — moving-horizon

Closed-loop architecture

Shared model

Real-time iteration

Disturbance feed-forward

Closed-loop performance

The full scenario, in numbers

What this project demonstrates

Explore the project

📄 Technical report

💻 Source repository

🎬 HD animation

Stack