M9-T5 — Model Predictive Control as Guidance and as Control

Learning objectives

After this lesson, you should be able to:

• Write a receding-horizon problem with input and state constraints as a quadratic program in the control sequence.
• State the role of the terminal cost and the terminal set in stability and feasibility.
• Compare MPC with saturated LQR on a command that drives the plant into its limits.
• Estimate the cost of a solve and budget it against the loop rate.

The intuition

UAV M5-T8: Model Predictive Control introduces MPC for a multirotor as a tracking controller, and Navigation M5-T6: LQR and MPC for Path Tracking uses it for a path. This lesson does what those do not: set out what the problem has to contain so that the closed loop is stable and stays feasible, and what it costs to solve.

Look ahead, act, repeat

At time $k$ the controller measures the state, predicts it forward over $N$ steps for a candidate sequence of controls, and picks the sequence that minimizes a cost (state errors and control effort) while satisfying the limits on gimbal angle and pitch. It applies the first control, and at the next step repeats with a fresh measurement. Because the model, the cost and the constraints are linear, quadratic and linear, each solve is a quadratic program of the kind of the last two lessons.

What it does that LQR cannot

LQR knows nothing of limits: its gain is computed for an unconstrained plant, and the gimbal limit is applied afterwards by clipping. For a small command that works. For a 15 m step on Hopper's lateral channel it does not: the clipped gimbal cannot catch the pitch, which swings past 1.2 rad and the loop is lost (the plant is unstable). MPC sees the limit in its prediction. With the pitch limited to 0.25 rad it rises to the limit in 0.5 s, holds there for 1.6 s while the vehicle accelerates sideways, and eases back, reaching 15.00 m. Both controllers use the same model and weights.

The terminal cost

A horizon of finitely many steps ignores what happens after it. A short horizon without a terminal cost is shortsighted: with a horizon of 0.3 s the vehicle settles at 1.82 m for a 2 m command. Adding a terminal cost equal to the unconstrained LQR cost-to-go, $\mathbf{x}_N^{\mathsf{T}}\mathbf{P}\mathbf{x}_N$, makes the finite-horizon problem equal to the infinite-horizon one whenever the constraints are not active after the horizon: the settled position is 2.00 m. With constraints active the proof of stability also needs a terminal set in which the LQR law respects the limits and which the system cannot leave.

The cost of looking ahead

One solve of the 20-step problem takes 8 barrier stages. Counting about eight Newton steps per stage, a Newton step costing a third of $n^3$ for the solve of the linear system plus the assembly of the Hessian, the solve is about 2 million floating-point operations: trivial at 10 Hz, and 0.22 Gflop/s at 100 Hz. A horizon of 100 steps costs 277 Mflop per solve, 28 Gflop/s at 100 Hz: beyond an embedded processor without a structured solver.

MPC solves a constrained optimal control problem at every step. It beats clipped LQR when the plant is driven into its limits, needs a terminal cost to behave like the infinite-horizon design, and costs a solve per step that has to fit in the loop period.

Definitions

Prediction horizon $N$

The number of steps looked ahead; the horizon time is $N\,\Delta t$.

Receding horizon

The strategy of solving at each step, applying the first control and repeating from the new state with the horizon shifted by one step.

Terminal cost $\mathbf{x}_N^{\mathsf{T}}\mathbf{P}\mathbf{x}_N$

A cost on the state at the end of the horizon standing for everything after it; $\mathbf{P}$ is the solution of the discrete Riccati equation.

Terminal set

A set of states in which the terminal control law satisfies all constraints and which it keeps the state inside.

Recursive feasibility

If the problem is feasible at one step it is feasible at the next.

Derivation: the condensed quadratic program and the terminal cost

Eliminate the states from the prediction.

1. $$\mathbf{x}_k = \mathbf{A}_d^k\mathbf{x}_0 + \sum_{j=0}^{k-1}\mathbf{A}_d^{k-1-j}\mathbf{B}_d\,u_j$$

   Iterate the discrete dynamics: the state at step k is the free response plus the weighted sum of the past controls.

2. $$\mathbf{X} = \boldsymbol\Phi\mathbf{x}_0 + \boldsymbol\Gamma\mathbf{U}$$

   Stack the predicted states: $\boldsymbol\Phi$ holds the powers of $\mathbf{A}_d$ and $\boldsymbol\Gamma$ is block lower triangular with the entries $\mathbf{A}_d^{k-1-j}\mathbf{B}_d$.

3. $$J = \tfrac12\mathbf{U}^{\mathsf{T}}\mathbf{H}\mathbf{U} + \mathbf{q}^{\mathsf{T}}\mathbf{U} + \text{const},\quad \mathbf{H} = 2(\boldsymbol\Gamma^{\mathsf{T}}\bar{\mathbf{Q}}\boldsymbol\Gamma + \bar{\mathbf{R}}),\ \mathbf{q} = 2\boldsymbol\Gamma^{\mathsf{T}}\bar{\mathbf{Q}}(\boldsymbol\Phi\mathbf{x}_0 - \mathbf{X}_r)$$

   Substitute into the cost. $\bar{\mathbf{Q}}$ is block diagonal with $\mathbf{Q}$ and $\mathbf{P}$ in the last block. H is positive definite because R is, so the problem is strictly convex.

4. $$-u_{\max} \le u_k \le u_{\max},\qquad -\theta_{\max} \le [\boldsymbol\Phi\mathbf{x}_0 + \boldsymbol\Gamma\mathbf{U}]_{\theta,k} \le \theta_{\max}$$

   The input limit is a box on U; a state limit is a linear inequality in U through the row of the prediction that gives that state.

Why the terminal cost works

1. $$V_N(\mathbf{x}) = \mathbf{x}^{\mathsf{T}}\mathbf{P}\mathbf{x}\ \text{solves}\ V_{N}(\mathbf{x}) = \min_u\,[\mathbf{x}^{\mathsf{T}}\mathbf{Q}\mathbf{x} + Ru^2 + V_N(\mathbf{A}_d\mathbf{x} + \mathbf{B}_du)]$$

   The Riccati equation is the Bellman equation of the infinite-horizon LQR problem (the dynamic-programming argument of the LQR lesson).

2. $$J_N(\mathbf{x}) = \min\bigl[\textstyle\sum_{k<N}(\cdot) + V_N(\mathbf{x}_N)\bigr] = V_N(\mathbf{x})$$

   If the terminal cost is the infinite-horizon cost-to-go, the finite-horizon cost is the infinite-horizon cost for every horizon, as long as no constraint is active after step N.

3. $$\text{stability}: J_N(\mathbf{x}_{k+1}) \le J_N(\mathbf{x}_k) - \mathbf{x}_k^{\mathsf{T}}\mathbf{Q}\mathbf{x}_k - Ru_k^2$$

   With a terminal cost and a terminal set in which the LQR law is feasible and invariant, the optimal cost is a Lyapunov function of the closed loop (the method of the Lyapunov lesson): it falls at every step.

Model predictive control is a quadratic program in the control sequence. With a terminal cost equal to the LQR cost-to-go it inherits the LQR behaviour near the target, and with a terminal set it is stable and recursively feasible; between the two it handles limits that LQR can only clip.

A worked example

Hopper's lateral channel (position, velocity, pitch, pitch rate; gimbal), discretized at 0.1 s with the exact zero-order hold. Gimbal limit 0.15 rad, pitch limit 0.25 rad, the weights of the LQR lesson scaled by the step.

1. The terminal weight. The discrete Riccati equation gives a gain of (0.090, 0.169, 1.103, 0.295), the discrete counterpart of the continuous gain (0.150, 0.272, 1.658, 0.386): the costs per step and per second differ in weight as well as in rate.
2. Constraints against clipping. For a position step from rest, the saturated LQR holds a 5 m step with a peak pitch of 0.326 rad, above the 0.25 rad limit, and loses control for 15 m and 30 m. MPC with a horizon of 2 s (20 steps) and the limits reaches 5.00, 15.00 and 30.01 m with a peak pitch of 0.250 rad each time.
3. The terminal cost. For a step of 2 m, a horizon of 0.3 s without a terminal cost ends at 1.82 m after 15 s (peak pitch 0.023 rad), and with the terminal cost at 2.00 m (0.133 rad). A horizon of 0.6 s: 1.99 and 2.00 m. A horizon of 2 s without it: 2.00 m.
4. Initial feasibility. With a pitch of 0.20 rad and a limit of 0.25 rad the constrained problem has a solution only for an initial pitch rate up to 0.533 rad/s, the same for horizons of 0.3 s and 1.2 s (by hand, stopping a rate q within the room takes $q^2/(2\cdot2.94)$ of pitch, which allows 0.542 rad/s). Beyond it the QP is infeasible and the controller needs a rule: a relaxed constraint, or a fallback.
5. Solve cost. One 20-step solve takes 8 barrier stages with a gap of 2.8×10⁻⁸. With about eight Newton steps per stage the cost is about 2.2 Mflop; for 50 steps 35 Mflop; for 100 steps 277 Mflop (28 Gflop/s at 100 Hz).

import math
import gnckit as g

gr, lam, b = 9.81, 0.7, 19.62
A = [[0, 1, 0, 0], [0, 0, gr, 0], [0, 0, 0, 1], [0, 0, lam ** 2, 0]]
B = [0.0, gr, 0.0, b]
dt = 0.1
# exact zero-order-hold discretization by the matrix exponential of the augmented matrix
n = 4
M = [[0.0] * 5 for _ in range(5)]
for i in range(4):
    for j in range(4): M[i][j] = A[i][j]
    M[i][4] = B[i]
E = g.mat_expm(M, dt)
Ad = [[E[i][j] for j in range(4)] for i in range(4)]
Bd = [E[i][4] for i in range(4)]
Q = [[dt * v for v in row] for row in [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 25, 0], [0, 0, 0, 4]]]       # the per-step cost is dt times the continuous one
R = dt / 0.15 ** 2
UMAX, THMAX = 0.15, 0.25

def dare_P(iters=3000):
    P = [row[:] for row in Q]
    for _ in range(iters):
        BtP = [sum(Bd[i] * P[i][j] for i in range(4)) for j in range(4)]
        S = R + sum(BtP[j] * Bd[j] for j in range(4))
        AtPA = g.mat_mul(g.mat_mul(g.mat_T(Ad), P), Ad)
        AtPB = [sum(Ad[i][j] * sum(P[i][k] * Bd[k] for k in range(4)) for i in range(4)) for j in range(4)]
        Pn = [[Q[i][j] + AtPA[i][j] - AtPB[i] * AtPB[j] / S for j in range(4)] for i in range(4)]
        Pn = [[0.5 * (Pn[i][j] + Pn[j][i]) for j in range(4)] for i in range(4)]
        if max(abs(Pn[i][j] - P[i][j]) for i in range(4) for j in range(4)) < 1e-9 * max(abs(Pn[i][j]) for i in range(4) for j in range(4)):
            return Pn
        P = Pn
    return P
Pinf = dare_P()
BtPinf = [sum(Bd[i] * Pinf[i][j] for i in range(4)) for j in range(4)]
Sinf = R + sum(BtPinf[j] * Bd[j] for j in range(4))
AtPB = [sum(Ad[i][j] * sum(Pinf[i][k] * Bd[k] for k in range(4)) for i in range(4)) for j in range(4)]
Kd = [v / Sinf for v in AtPB]
print('discrete LQR gain (dt = 0.1 s):', ', '.join(f'{v:.4f}' for v in Kd), '(the continuous design had 0.150, 0.272, 1.658, 0.386)')

def mpc_qp(x0, xref, Nh, terminal=True, use_state_limit=True):
    """Condensed receding-horizon QP in the controls U = (u_0 ... u_{N-1}). Returns (U, info)."""
    # prediction: x_k = Ad^k x0 + sum_{j<k} Ad^{k-1-j} Bd u_j
    Apow = [g.mat_eye(4)]
    for _ in range(Nh): Apow.append(g.mat_mul(Ad, Apow[-1]))
    Phi = [g.mat_vec(Apow[k], x0) for k in range(Nh + 1)]
    Gam = [[[0.0] * Nh for _ in range(4)] for _ in range(Nh + 1)]
    for k in range(1, Nh + 1):
        for j in range(k):
            col = g.mat_vec(Apow[k - 1 - j], Bd)
            for i in range(4): Gam[k][i][j] = col[i]
    H = [[0.0] * Nh for _ in range(Nh)]
    q = [0.0] * Nh
    for k in range(1, Nh + 1):
        Wk = Q if (k < Nh or not terminal) else Pinf
        e0 = [Phi[k][i] - xref[i] for i in range(4)]
        for a in range(Nh):
            Ga = [Gam[k][i][a] for i in range(4)]
            WGa = g.mat_vec(Wk, Ga)
            q[a] += 2 * sum(WGa[i] * e0[i] for i in range(4))
            for c in range(Nh):
                H[a][c] += 2 * sum(WGa[i] * Gam[k][i][c] for i in range(4))
    for a in range(Nh): H[a][a] += 2 * R
    Gm, hv = [], []
    for j in range(Nh):
        row = [0.0] * Nh; row[j] = 1.0; Gm.append(row); hv.append(UMAX)
        row = [0.0] * Nh; row[j] = -1.0; Gm.append(row); hv.append(UMAX)
    if use_state_limit:
        for k in range(1, Nh + 1):
            row = [Gam[k][2][j] for j in range(Nh)]
            Gm.append(row); hv.append(THMAX - Phi[k][2])
            Gm.append([-v for v in row]); hv.append(THMAX + Phi[k][2])
    U, info = g.solve_convex(q, P=H, G=Gm, h=hv)
    return U, info

def simulate(controller, x0, xref, steps=80):
    x = list(x0); peak_th = 0.0; hist = []
    for k in range(steps):
        u = controller(x)
        x = [sum(Ad[i][j] * x[j] for j in range(4)) + Bd[i] * u for i in range(4)]
        peak_th = max(peak_th, abs(x[2]))
        hist.append(x[0])
        if abs(x[0]) > 1e4 or abs(x[2]) > 20: return None, None, k
    return x, peak_th, steps

def lqr_sat(xref):
    return lambda x: max(-UMAX, min(UMAX, -sum(Kd[i] * (x[i] - xref[i]) for i in range(4))))
def mpc(xref, Nh, terminal=True, state=True):
    return lambda x: mpc_qp(x, xref, Nh, terminal, state)[0][0]

print('position step from rest, gimbal limited to 0.15 rad, pitch limit 0.25 rad for MPC (horizon 2 s = 20 steps):')
print('  step (m)   saturated LQR: peak pitch, final position      MPC with constraints: peak pitch, final position')
for step in (5.0, 15.0, 30.0):
    xr = [step, 0, 0, 0]
    r1 = simulate(lqr_sat(xr), [0, 0, 0, 0], xr, steps=120)
    r2 = simulate(mpc(xr, 20), [0, 0, 0, 0], xr, steps=120)
    f1 = 'lost control' if r1[0] is None else f'{r1[1]:.3f} rad, {r1[0][0]:7.2f} m'
    f2 = 'lost control' if r2[0] is None else f'{r2[1]:.3f} rad, {r2[0][0]:7.2f} m'
    print(f'  {step:6.1f}     {f1:36s}    {f2}')

print('horizon and terminal cost, unconstrained pitch, gimbal still limited; step of 2 m (inside the region where the gimbal limit is not reached):')
xr = [2.0, 0, 0, 0]
for Nh, term in ((3, False), (3, True), (6, False), (6, True), (20, False)):
    r = simulate(mpc(xr, Nh, terminal=term, state=False), [0, 0, 0, 0], xr, steps=150)
    print(f'  horizon {Nh * dt:3.1f} s, terminal cost {"yes" if term else "no ":3s}: ' + ('closed loop diverges' if r[0] is None else f'final position {r[0][0]:6.2f} m, peak pitch {r[1]:.3f} rad'))

print('initial feasibility: the largest initial pitch rate q0 (pitch 0.20 rad, limit 0.25 rad) for which the constrained problem has a solution, by bisection')
for Nh in (3, 12):
    lo, hi = 0.0, 3.0
    for _ in range(14):
        mid = (lo + hi) / 2
        U, info = mpc_qp([0, 0, 0.20, mid], [0, 0, 0, 0], Nh)
        lo, hi = (mid, hi) if info['status'] == 'optimal' else (lo, mid)
    print(f'  horizon {Nh * dt:3.1f} s: q0 up to {lo:.3f} rad/s; by hand the gimbal can stop a rate q in a pitch of q^2 / (2 * {b * UMAX:.2f}), so the 0.05 rad of room allows q0 = {math.sqrt(2 * b * UMAX * 0.05):.3f} rad/s')
print('beyond that the QP is infeasible: no control sequence respects the pitch limit, and the controller must have a rule for that case')

U, info = mpc_qp([0, 0, 0.1, 0], [10.0, 0, 0, 0], 20)
print(f'one solve, 20 controls: status {info["status"]}, barrier stages {info["iterations"]}, duality gap {info["gap"]:.1e}')
for Nh in (20, 50, 100):
    nv = Nh; cons = 2 * Nh * 2
    newton = info['iterations'] * 8                   # about eight Newton steps at each barrier stage
    flops = newton * (nv ** 3 / 3 + cons * nv * nv)
    print(f'  rough cost estimate, horizon {Nh} steps: {flops / 1e6:7.1f} Mflop per solve; at a 100 Hz loop this is {flops * 100 / 1e9:6.2f} Gflop/s')

discrete LQR gain (dt = 0.1 s): 0.0904, 0.1689, 1.1032, 0.2947 (the continuous design had 0.150, 0.272, 1.658, 0.386)
position step from rest, gimbal limited to 0.15 rad, pitch limit 0.25 rad for MPC (horizon 2 s = 20 steps):
  step (m)   saturated LQR: peak pitch, final position      MPC with constraints: peak pitch, final position
     5.0     0.326 rad,    5.00 m                    0.250 rad,    5.00 m
    15.0     lost control                            0.250 rad,   15.00 m
    30.0     lost control                            0.250 rad,   30.01 m
horizon and terminal cost, unconstrained pitch, gimbal still limited; step of 2 m (inside the region where the gimbal limit is not reached):
  horizon 0.3 s, terminal cost no : final position   1.82 m, peak pitch 0.023 rad
  horizon 0.3 s, terminal cost yes: final position   2.00 m, peak pitch 0.133 rad
  horizon 0.6 s, terminal cost no : final position   1.99 m, peak pitch 0.038 rad
  horizon 0.6 s, terminal cost yes: final position   2.00 m, peak pitch 0.133 rad
  horizon 2.0 s, terminal cost no : final position   2.00 m, peak pitch 0.114 rad
initial feasibility: the largest initial pitch rate q0 (pitch 0.20 rad, limit 0.25 rad) for which the constrained problem has a solution, by bisection
  horizon 0.3 s: q0 up to 0.533 rad/s; by hand the gimbal can stop a rate q in a pitch of q^2 / (2 * 2.94), so the 0.05 rad of room allows q0 = 0.542 rad/s
  horizon 1.2 s: q0 up to 0.533 rad/s; by hand the gimbal can stop a rate q in a pitch of q^2 / (2 * 2.94), so the 0.05 rad of room allows q0 = 0.542 rad/s
beyond that the QP is infeasible: no control sequence respects the pitch limit, and the controller must have a rule for that case
one solve, 20 controls: status optimal, barrier stages 8, duality gap 2.8e-08
  rough cost estimate, horizon 20 steps:     2.2 Mflop per solve; at a 100 Hz loop this is   0.22 Gflop/s
  rough cost estimate, horizon 50 steps:    34.7 Mflop per solve; at a 100 Hz loop this is   3.47 Gflop/s
  rough cost estimate, horizon 100 steps:   277.3 Mflop per solve; at a 100 Hz loop this is  27.73 Gflop/s

In robotics

MPC is how limits and predictions enter the control loop of modern vehicles.

• Constrained attitude and position control. Quadrotor and legged-robot controllers use MPC to respect actuator and state limits (thrust, tilt, joint torques) that a clipped linear controller handles badly, as the 15 m step shows.
• Guidance and control in one. With a long horizon over position and a coarse model MPC is a guidance law (the receding-horizon version of the convex descent of the last lesson); with a short horizon and a fast model it is a controller. The two uses differ in the model, the step and the horizon.
• Budgeting the solve. The solver must finish within the loop period with margin, on the worst case, not the average. Active-set and interior-point solvers with fixed iteration counts are used on board, with warm starts from the previous solution shifted by one step.
• Infeasibility. A real MPC has a rule for the case where the problem has no solution (a disturbance has pushed the state outside the feasible set): soft constraints with a penalty, or a fallback to a safe controller. The terminal set is what makes the problem recursively feasible in the nominal case.
• Explicit alternatives. For small problems the QP can be solved offline as a piecewise-affine function of the state, evaluated by lookup: a fixed, tiny worst-case time, at the price of storage that grows quickly with the horizon.

The lab M9-L2: Solve the Landing as a Convex Program re-solves a landing problem in a receding horizon after a gust, and the capstone M12-L2: Capstone: Land Hopper places a predictive guidance law in the full loop. The unconstrained limit of this problem is the LQR of M4-T4: The Linear-Quadratic Regulator from Dynamic Programming; the solver is that of M9-T3: Convex Optimization for Guidance.

Common misconceptions

Key takeaways

1. MPC solves a finite-horizon quadratic program in the control sequence at every step, applies the first control and repeats.
2. Limits on the input are a box on the controls; limits on the state are linear inequalities through the prediction matrix.
3. A terminal cost equal to the Riccati $\mathbf{P}$ makes MPC equal LQR when no constraint is active after the horizon; with a terminal set it is stable and recursively feasible.
4. For a 15 m step the saturated LQR loses control, while MPC holds the pitch at its 0.25 rad limit and reaches 15.00 m.
5. A horizon of 0.3 s without a terminal cost settles at 1.82 m for a 2 m command; with it, 2.00 m.
6. A 20-step solve is about 2.2 Mflop and a 100-step one 277 Mflop: the solve has to fit in the loop period.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• J. B. Rawlings, D. Q. Mayne and M. M. Diehl, Model Predictive Control: Theory, Computation, and Design, 2nd edition, Nob Hill, 2017, the standard text. Free to read as the authors' PDF; the book is copyrighted.
• D. Q. Mayne, J. B. Rawlings, C. V. Rao and P. O. M. Scokaert, "Constrained model predictive control: stability and optimality", Automatica 36(6), 2000, the terminal-set conditions. Paid (Elsevier).
• MIT 16.323 Principles of Optimal Control (OpenCourseWare) , the lectures on the LQR and its finite-horizon versions. Free; licence CC BY-NC-SA 4.0.
• do-mpc documentation , an open-source toolbox for MPC with worked examples. Free to read; licence LGPL v3.

Knowledge check

Three questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Related content

Nothing below is required to finish this lesson.

• Before
  M9-T4: Powered-Descent Guidance by Convexification

  Formulate fuel-optimal landing with a thrust lower bound and a glide-slope constraint, and explain how lossless convexification makes it a convex problem.

• Lab
  M9-L2: Solve the Landing as a Convex Program

  Pose a planar divert-and-land as a quadratic program with thrust and glide-slope limits, solve it with the course solver, and re-solve it in a receding horizon when a gust displaces the vehicle.

• Lab
  M12-L2: Capstone: Land Hopper

  Write the navigation filter, the guidance law and the controller of Hopper, and pass an acceptance campaign of dispersed flights with wind, sensor errors, mass uncertainty and one beacon fault.

Other courses on this site treat neighbouring topics from their own angle:

• UAV M5-T8: Model Predictive Control
• UAV M5-T9: Trajectory generation
• UAV M5-T12: Kinodynamic planning
• Navigation M5-T6: LQR and MPC for Path Tracking
• Navigation M5-T3: Curvature, Velocity and Acceleration Limits
• Navigation M5-T2: Smoothing: Shortcuts, Splines and Gradient Methods
• Robot Learning M7-T5: Model Predictive Control with Learned Models
• Robot Learning M4-T1: Markov Decision Processes

This course leaves the following to the pages that already teach them:

• Minimum-snap polynomial trajectories and differential flatness of a quadrotor
• Path smoothing and velocity profiles for ground robots
• Sampling-based planning with a learned model (random shooting, CEM)
• Reinforcement learning