Model Predictive Control

A linear-quadratic regulator hands you one gain matrix, computed once, that knows nothing about the torque and joint limits your hardware actually has. Model predictive control keeps the same quadratic cost but re-plans from the current state at every step, and that is exactly what lets it respect constraints. It is why MPC runs modern legged locomotion and manipulation controllers.

Re-solve every step

At the measured state $x_k$, MPC solves a finite-horizon optimal control problem over the next $N$ steps,

$$\min_{u_0,\dots,u_{N-1}} \; \sum_{j=0}^{N-1}\!\left( e_j^{\top} Q\, e_j + u_j^{\top} R\, u_j\right) + e_N^{\top} Q_f\, e_N ,$$

subject to the dynamics and the limits,

$$x_{j+1} = A x_j + B u_j, \quad x_0 = x_k, \quad u_j \in \mathcal{U}, \quad x_j \in \mathcal{X},$$

with $e_j = x_j - x_{\text{ref}}$. It applies only the first control $u_0^{\star}$, throws away the rest of the plan, advances to $x_{k+1}$, and solves the whole problem again from there. That re-solving from the fresh state is the receding horizon, and it is what closes the loop: disturbances and model error get absorbed because every step starts from where the system really is.

The unconstrained core is a finite-horizon LQR

Drop the constraints ($\mathcal{U} = \mathbb{R}^m$, $\mathcal{X} = \mathbb{R}^n$) and the inner problem has a closed form. Dynamic programming backward in time gives the discrete Riccati recursion,¹ started from the terminal cost,

$$P_N = Q_f,$$ $$K_j = \left(R + B^{\top} P_{j+1} B\right)^{-1} B^{\top} P_{j+1} A ,$$ $$P_j = Q + A^{\top} P_{j+1} A - A^{\top} P_{j+1} B\, K_j , \qquad j = N-1, \dots, 0 .$$

The optimal plan is the time-varying feedback $u_j = -K_j\, e_j$. MPC applies its first entry $u_0 = -K_0\, e_0$, steps, and reruns the backward pass at the next state.²

One backward pass by hand

Take a double integrator: position and velocity $x = [p,\ v]^{\top}$ driven by an acceleration $u$,

$$p_{k+1} = p_k + v_k\, \Delta t, \qquad v_{k+1} = v_k + u_k\, \Delta t ,$$

so with $\Delta t = 1$,

$$A = \begin{bmatrix} 1 & 1 \\ 0 & 1 \end{bmatrix}, \quad B = \begin{bmatrix} 0 \\ 1 \end{bmatrix}, \quad Q = Q_f = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix}, \quad R = 1 .$$

Run the horizon $N = 2$ backward from $P_2 = Q_f = I$. The last step ($j = 1$):

$$B^{\top} P_2 B = 1, \quad S = R + 1 = 2, \quad B^{\top} P_2 A = \begin{bmatrix} 0 & 1 \end{bmatrix},$$ $$K_1 = \tfrac{1}{2}\begin{bmatrix} 0 & 1 \end{bmatrix} = \begin{bmatrix} 0 & 0.5 \end{bmatrix}, \qquad P_1 = Q + A^{\top} P_2 A - A^{\top} P_2 B\, K_1 = \begin{bmatrix} 2 & 1 \\ 1 & 2.5 \end{bmatrix} .$$

The first step ($j = 0$), now using $P_1$:

$$B^{\top} P_1 B = 2.5, \quad S = 3.5, \quad B^{\top} P_1 A = \begin{bmatrix} 1 & 3.5 \end{bmatrix},$$ $$K_0 = \tfrac{1}{3.5}\begin{bmatrix} 1 & 3.5 \end{bmatrix} = \begin{bmatrix} 0.286 & 1.0 \end{bmatrix} .$$

Two things to read off. The applied gain $K_0 = [0.286,\ 1.0]$ weighs position error, while the one-step-ahead gain $K_1 = [0,\ 0.5]$ sees only velocity: a longer horizon changes the very first move. And MPC uses $K_0$ alone, applies $u_0 = -K_0\, e_0$, then at the next measured state runs $P_2 \to P_1 \to P_0$ again from scratch.

Constraints make each step a QP

Real actuators saturate: $|u| \le u_{\max}$. The moment $\mathcal{U}$ or $\mathcal{X}$ is a bounded set, the backward Riccati recursion no longer applies, because the minimization is now over a constrained quadratic. Stack the controls into one vector $\mathbf{u} = [u_0, \dots, u_{N-1}]^{\top}$, eliminate the states through $x_{j+1} = A x_j + B u_j$, and the horizon cost collapses to a single quadratic program,

$$\min_{\mathbf{u}} \; \tfrac{1}{2}\, \mathbf{u}^{\top} H \mathbf{u} + g^{\top} \mathbf{u} \quad \text{s.t.} \quad \underline{u} \le \mathbf{u} \le \overline{u} ,$$

with $H \succ 0$ assembled from $Q, R, A, B$ and $g$ from the current $x_k$. That QP is what MPC actually solves each step. It is convex, so it solves reliably, and handling the box on $\mathbf{u}$ is exactly the machinery LQR's single gain cannot provide.²

The loop, live

The demo runs the constrained loop on the double integrator. Every control step it runs the full backward pass to get $K_0$, applies $u = \operatorname{clamp}(-K_0\,e,\ \pm u_{\max})$, advances the true system, and re-solves. The faint blue curve is the planned $N$-step trajectory rolled out from the current state, position against prediction step, redrawn every step.

Receding-horizon MPC · double integratorinteractive

position axis (drag to move target) · plan rises with prediction step

PauseReset

horizon N = 10control limit u_max = 2.0

p

-4.00

v

0.00

u applied

+0.00

p − p_ref

-6.500

Re-solving the horizon every step and applying only K_0.

Watch the blue plan lengthen as $N$ grows: a short horizon draws a plan that has not yet started decelerating, so the first control drives hard toward the target and, once clamped, cannot brake in time. A long horizon draws a plan that curves back onto the target, and applying its first step yields the smooth approach. Either way only $K_0$ is ever applied; the rest of the plan is advisory and is recomputed at the next state.

This is the constrained sibling of the finite-horizon LQR, and its inner solve is warm-started iLQR when the dynamics are nonlinear, which is how the same receding loop scales from this cart to a whole-body quadruped.

Footnotes

1. Bertsekas, Dynamic Programming and Optimal Control. ↩

2. Rawlings, Mayne & Diehl, Model Predictive Control: Theory, Computation, and Design (2nd ed.). ↩ ↩²