Model predictive control

Deep dive

How it works

Parts:

• plant model — A, B (or nonlinear f) that predicts N steps ahead
• state estimator — supplies x0 each tick (Kalman filter)
• cost and limits — weights Q, R, Qf and bounds on inputs and states
• QP solver — warm-started, solves the condensed problem each tick
• plant interface — applies u[0] at a fixed rate

Model x[k+1] = A x[k] + B u[k], cost sum_k (x[k+1]'Q x[k+1] + u[k]'R u[k]) + x[N]'Qf x[N], limits lo <= u[k] <= hi.

1. Estimate x0 (Kalman filter; nonlinear plant: extended Kalman filter).
2. Condense: stack U = (u[0..N-1]). Then X = Sx x0 + Su U with Sx blocks A^(k+1) and Su blocks A^(k-j) B. The cost becomes 1/2 U'HU + g'U with H = Su'Qb Su + Rb and g = F x0. H and F depend only on model and weights: build once. Per tick only g changes.
3. Solve the box-constrained QP. Here: accelerated projected gradient (FISTA), step 1/L with L an upper bound of the largest eigenvalue of H; the projection is a clamp. Production solvers use active-set, interior-point or operator splitting (QP, interior point).
4. Apply u[0]. Shift the solution one step as the next warm start. Repeat at a fixed rate (fixed-rate loop).

 x(t) ─► min cost over N steps s.t. limits ─► u0..u(N-1) ─ apply u0 ─► plant ─┐
   ▲                                                                          │
   └──────────────────────── re-plan next tick ◄──────────────────────────────┘

Stability is not automatic for a short horizon: use the Riccati solution as terminal cost Qf plus a terminal set (Mayne et al. 2000). The code uses a heavy diagonal Qf for brevity: a tuning choice, not a guarantee.

Code

/// Condensed linear MPC, x+ = A x + B u (2 states, 1 input).
/// min sum_k (x_{k+1}' Q x_{k+1} + r u_k^2) + x_N' Qf x_N   s.t. lo <= u_k <= hi
pub struct Mpc {
    n: usize,
    h: Vec<f64>,       // n x n Hessian, row-major
    f: Vec<[f64; 2]>,  // gradient is g = F x0
    l: f64,            // Lipschitz bound of the gradient
    lo: f64,
    hi: f64,
    u: Vec<f64>,       // previous solution, shifted: warm start
}

fn mv(a: &[[f64; 2]; 2], v: [f64; 2]) -> [f64; 2] {
    [a[0][0] * v[0] + a[0][1] * v[1], a[1][0] * v[0] + a[1][1] * v[1]]
}
fn mm(a: &[[f64; 2]; 2], b: &[[f64; 2]; 2]) -> [[f64; 2]; 2] {
    let mut c = [[0.0; 2]; 2];
    for i in 0..2 { for j in 0..2 { c[i][j] = a[i][0] * b[0][j] + a[i][1] * b[1][j]; } }
    c
}

impl Mpc {
    pub fn new(a: [[f64; 2]; 2], b: [f64; 2], q: [f64; 2], qf: [f64; 2], r: f64,
               n: usize, lo: f64, hi: f64) -> Self {
        // pw[k] = A^k; Su(k,j) = A^(k-j) B is the effect of u_j on x_{k+1}
        let mut pw = vec![[[1.0, 0.0], [0.0, 1.0]]];
        for k in 0..=n { let p = mm(&a, &pw[k]); pw.push(p); }
        let su = |k: usize, j: usize| mv(&pw[k - j], b);
        let mut h = vec![0.0; n * n];
        let mut f = vec![[0.0; 2]; n];
        for k in 0..n {
            let w = if k == n - 1 { qf } else { q };          // weight on x_{k+1}
            let sx = &pw[k + 1];
            for i in 0..=k {
                let si = su(k, i);
                for j in 0..=k {
                    let sj = su(k, j);
                    h[i * n + j] += w[0] * si[0] * sj[0] + w[1] * si[1] * sj[1];
                }
                for c in 0..2 { // F[i] += Su(k,i)' W Sx
                    f[i][c] += w[0] * si[0] * sx[0][c] + w[1] * si[1] * sx[1][c];
                }
            }
        }
        for i in 0..n { h[i * n + i] += r; }
        // Gershgorin bound: lambda_max(H) <= max row sum of |H|
        let l = (0..n).map(|i| (0..n).map(|j| h[i * n + j].abs()).sum::<f64>()).fold(0.0, f64::max);
        Mpc { n, h, f, l, lo, hi, u: vec![0.0; n] }
    }

    fn grad(&self, u: &[f64], x0: [f64; 2], g: &mut [f64]) {
        for i in 0..self.n {
            let row = &self.h[i * self.n..(i + 1) * self.n];
            g[i] = row.iter().zip(u).map(|(a, b)| a * b).sum::<f64>()
                + self.f[i][0] * x0[0] + self.f[i][1] * x0[1];
        }
    }

    /// Accelerated projected gradient (FISTA). Returns (first input, iterations used).
    pub fn solve(&mut self, x0: [f64; 2], max_iter: usize, tol: f64) -> (f64, usize) {
        let n = self.n;
        self.u.rotate_left(1); self.u[n - 1] = self.u[n - 2]; // shift warm start
        let (mut y, mut g, mut un) = (self.u.clone(), vec![0.0; n], vec![0.0; n]);
        let mut t = 1.0f64;
        let mut it = 0;
        while it < max_iter {
            it += 1;
            self.grad(&y, x0, &mut g);
            for i in 0..n { un[i] = (y[i] - g[i] / self.l).clamp(self.lo, self.hi); }
                let step: f64 = (0..n).map(|i| (un[i] - y[i]).abs()).fold(0.0, f64::max);
            let tn = 0.5 * (1.0 + (1.0 + 4.0 * t * t).sqrt());
            for i in 0..n { y[i] = un[i] + (t - 1.0) / tn * (un[i] - self.u[i]); }
            self.u.copy_from_slice(&un);
            t = tn;
            if step * self.l < tol { break; }
        }
        (self.u[0], it)
    }
}

pub fn simulate() -> [f64; 2] {
    let dt = 0.1;
    let a = [[1.0, dt], [0.0, 1.0]];           // double integrator
    let b = [0.5 * dt * dt, dt];
    let mut mpc = Mpc::new(a, b, [1.0, 0.1], [10.0, 1.0], 0.01, 20, -1.0, 1.0);
    let mut x = [5.0, 0.0];
    for _ in 0..150 {
        let (u, _iters) = mpc.solve(x, 2000, 1e-6);
        assert!(u.abs() <= 1.0);
        x = [a[0][0] * x[0] + a[0][1] * x[1] + b[0] * u, a[1][0] * x[0] + a[1][1] * x[1] + b[1] * u];
    }
    x
}

Run: from x0 = (5, 0) the loop reaches the origin within 1e-6 and |u| never exceeds 1 (unbounded, the first input would be about -38). KKT conditions checked at the optimum with 10 of 20 inputs at a bound.

Complexity and constants

• Variables N*nu. Condensed H is (N*nu)^2 doubles (3.2 kB for 20 x 20); one gradient costs (N*nu)^2 multiply-adds (400 here).
• Dense Cholesky is O((N*nu)^3). The sparse form keeps states as variables and uses the block-banded structure (Riccati recursion): O(N*(nx+nu)^3). Condense for short horizons and few inputs.
• First-order iteration counts grow with the conditioning of H and the tolerance. Measured (x0 = (0.8, 0)): 102, 1676, 8470 iterations at 1e-3, 1e-6, 1e-10. Closed loop at 1e-6: 110 on average warm, 261 cold, worst tick about 1400.

Efficiency

Cost: one solve of this 20-step problem is of the order of 10 µs on an Apple-silicon Mac (release build, measured): 3.2 kB of H, about 110 iterations x 400 multiply-adds. Bound: solver latency; an overrun acts as dead time. Larger plants: order of 0.1-10 ms with a code-generated solver.

Levers:

• Warm start from the shifted plan — 261 to 110 average iterations, 2.4x (measured here) — always.
• Cap the iterations — bounded latency; every projected iterate respects the bounds — hard real time. Nonlinear version: real-time iteration (one SQP step per tick).
• Precompute H, its bound or factorization, and F — per tick only g = F x0 (2N multiply-adds) is new — fixed model.
• Move blocking (hold u over several late steps) — fewer variables (not measured here) — long horizons.
• Explicit MPC — no solver at run time, only a region lookup (Bemporad et al. 2002) — tiny problems; region count grows quickly with N and nx.

Naive → standard → state of the art:

• Naive: general NLP or dense QP rebuilt and cold-started each tick · O((N*nu)^3) plus model building.
• Standard: condensed QP, H offline, warm start, iteration cap · about 10 µs here.
• State of the art: code-generated, structured solvers (HPIPM inside acados, OSQP) with real-time iteration · O(N*(nx+nu)^3) per iteration (Verschueren et al.).

Implementation notes

• Predict x0 forward by the expected solve time; latency is dead time.
• Scale states and inputs so Q and R entries are similar; a badly conditioned H makes first-order solvers crawl.
• Weight input increments for smooth actuation; for offset-free tracking estimate a constant disturbance state.
• On a failed solve (NaN, residual large at the cap) fall back to the shifted previous plan, then a safe controller.
• Horizon from physics: with |u| <= 1, braking from speed v takes v seconds; N*dt must exceed it.

Pitfalls

• Horizon shorter than the braking or settling time: the controller cannot see limits coming.
• Hard state constraints without slack: infeasible QP at the worst moment.
• Model mismatch without a disturbance estimate: steady-state offset.

Variants and alternatives

In production

Process industry (the first large users; Qin and Badgwell 2003); building HVAC with weather forecasts (Oldewurtel et al. 2012); autonomous-vehicle steering (Falcone et al. 2007); legged-robot locomotion, quadrotor tracking, spacecraft guidance.

Implementations

Sources

• J. B. Rawlings, D. Q. Mayne, M. M. Diehl, Model Predictive Control: Theory, Computation, and Design, 2nd ed., Nob Hill 2017. book site
• F. Borrelli, A. Bemporad, M. Morari, Predictive Control for Linear and Hybrid Systems, Cambridge UP 2017. doi
• D. Q. Mayne, J. B. Rawlings, C. V. Rao, P. O. M. Scokaert, Constrained model predictive control: Stability and optimality, Automatica 36(6), 2000. doi
• A. Bemporad, M. Morari, V. Dua, E. N. Pistikopoulos, The explicit linear quadratic regulator for constrained systems, Automatica 38(1), 2002. doi
• B. Stellato, G. Banjac, P. Goulart, A. Bemporad, S. Boyd, OSQP: an operator splitting solver for quadratic programs, Math. Prog. Comp. 12, 2020. arXiv · code
• R. Verschueren et al., acados: a modular open-source framework for fast embedded optimal control, Math. Prog. Comp. 14, 2022. arXiv · code
• F. Oldewurtel et al., Use of model predictive control and weather forecasts for energy efficient building climate control, Energy and Buildings 45, 2012. doi
• P. Falcone, F. Borrelli, J. Asgari, H. E. Tseng, D. Hrovat, Predictive active steering control for autonomous vehicle systems, IEEE Trans. Control Systems Technology 15(3), 2007. doi
• S. J. Qin, T. A. Badgwell, A survey of industrial model predictive control technology, Control Engineering Practice 11(7), 2003. doi