UAV Systems Engineering · M5-L4 · Guided lab

Compare PID, LQR and MPC

The same plant, the same limit, three controllers: measure what each one costs and where each one breaks.

UAV Guidance, Navigation and Control (GNC)
Established engineering
About 60 minutes

What you will do

A 1D double integrator (one translational axis near hover) must move 10 m with an acceleration limit of ± $2\,\mathrm{m}/\mathrm{s}^{2}$ . You will implement a PD controller, an LQR from the Riccati equation, and a box-constrained MPC solved by projected gradient, run them on the same step, and compare overshoot, settling and effort. This is deliberately a model problem: conclusions about real vehicles need the vehicle’s own model, delays and noise.

Before you start

Lessons M5-T6 (PID), M5-T7 (LQR) and M5-T8 (MPC).
Lab: Implement LQR.
Python 3 with NumPy.

Starter

import numpy as np
dt = 0.05; umax = 2.0; T = 12.0
A = np.array([[1, dt], [0, 1]]); B = np.array([[0.5 * dt**2], [dt]])
# TODO 1: reuse your dlqr(Q, R) from the LQR lab
# TODO 2: build the condensed prediction matrices Phi (stack of A^i) and Gam (block lower-triangular, A^(i-j) B) for horizon N = 40
# TODO 3: write mpc(x, r) that minimises the horizon cost with |u| <= umax, using projected gradient: U <- clip(U - (H U + f)/L, -umax, umax)
# TODO 4: run PD, LQR (clipped) and MPC on a 10 m step and report overshoot, last time more than 0.2 m from the target, and control effort sum(u^2) dt

Starter: follow the TODOs, then compare with the reference solution.

Reference solution

Show the solution (try first)

import numpy as np
dt = 0.05; umax = 2.0; T = 12.0
A = np.array([[1, dt], [0, 1]]); B = np.array([[0.5 * dt**2], [dt]])
def dlqr(Q, R):
    P = Q.copy()
    for _ in range(5000):
        G = np.linalg.solve(R + B.T @ P @ B, B.T @ P @ A); Pn = Q + A.T @ P @ A - A.T @ P @ B @ G
        if np.max(np.abs(Pn - P)) < 1e-12: P = Pn; break
        P = Pn
    return np.linalg.solve(R + B.T @ P @ B, B.T @ P @ A), P
Q = np.diag([20.0, 4.0]); R = np.array([[1.0]]); K, P = dlqr(Q, R)
N = 40
Phi = np.vstack([np.linalg.matrix_power(A, i + 1) for i in range(N)])
Gam = np.zeros((2 * N, N))
for i in range(N):
    for j in range(i + 1):
        Gam[2 * i:2 * i + 2, j:j + 1] = np.linalg.matrix_power(A, i - j) @ B
Qb = np.kron(np.eye(N), Q); Qb[-2:, -2:] = P; Rb = np.kron(np.eye(N), R)
H = Gam.T @ Qb @ Gam + Rb; Lip = np.linalg.eigvalsh(H).max()
def mpc(x, r):
    f = Gam.T @ Qb @ (Phi @ (x - r) ); U = np.zeros(N)
    for _ in range(300):
        U = np.clip(U - (H @ U + f) / Lip, -umax, umax)
    return U[0]
def run(ctrl, r=np.array([10.0, 0.0])):
    x = np.zeros(2); us = []; ps = []
    for _ in range(int(T / dt)):
        u = float(np.clip(ctrl(x, r), -umax, umax)); x = A @ x + B[:, 0] * u; us.append(u); ps.append(x[0])
    ps = np.array(ps); over = max(0.0, ps.max() - r[0])
    ok = np.where(np.abs(ps - r[0]) > 0.2)[0]; ts = (ok[-1] + 1) * dt if len(ok) else 0.0
    return float(over), float(ts), float(np.sum(np.square(us)) * dt)
Kp, Kd = 1.0, 1.4                                   # PD gains: wn = 1 rad/s, zeta = 0.7
def pidc(x, r): return Kp * (r[0] - x[0]) - Kd * x[1]
lq = lambda x, r: (-K @ (x - r)).item()
mp = lambda x, r: mpc(x, r)
res = {n: run(c) for n, c in (('PD', pidc), ('LQR', lq), ('MPC', mp))}
for n, (over, ts, eff) in res.items(): print(f"{n:>3}: overshoot {over:5.2f} m, last time > 0.2 m from target {ts:5.2f} s, control effort {eff:5.1f}")
assert res['MPC'][0] < res['LQR'][0] and res['MPC'][1] < res['PD'][1] and res['MPC'][1] < res['LQR'][1]

PD, clipped LQR and constrained MPC on one actuator-limited step.

Experiments

Change umax to 1, 2, 5 and $20\,\mathrm{m}/\mathrm{s}^{2}$ . At what limit do LQR and MPC become nearly identical, and why?
Add a 0.2 s input delay (apply the command from four steps earlier). Which controller degrades most? Does giving MPC a delay-aware prediction help?
Add velocity measurement noise $(\sigma =0.05\,\mathrm{m}/\mathrm{s})$ and report the change in control effort for each controller.
Add a constant $0.5\,\mathrm{m}/\mathrm{s}^{2}$ disturbance: which controller has a steady error and what is the smallest change that removes it in each case?
Write down, for each controller, one situation where you would choose it for a real vehicle and one where you would not.

How to know it worked

The solution runs and its assertions pass.
You have a table of overshoot, settling time and effort for the three controllers at several limits.
You can explain the ranking from the saturation behaviour alone.

If it does not work

Troubleshooting
Symptom	Likely cause	What to check
MPC command oscillates	Projected gradient not converged	More iterations, or a step of 1/L from the Hessian’s largest eigenvalue
LQR overshoot is large	Saturation of a controller designed without limits	This is the expected result: it is the point of the lab
MPC too slow	Horizon too long for the iteration count	Shorten the horizon or warm-start with the previous solution

References

Record your progress

Saved in this browser. This lab runs on your own machine, so the site cannot check it for you.