MPC: respecting limits

Limits LQR doesn't know about

This motor gives at most 1 N, and the cart has to move 20 cm, from −10 cm to +10 cm, balancing all the way. LQR assumes it gets whatever force it asks for. Its first request is 4.6 N, backwards (to move the cart forwards it first tips the pole forwards); the motor clips that, the pole tips further than 1 N can ever bring back, and the cart chases it into the stop.

Model predictive control (MPC) plans within the limit instead. Every period it chooses the next $N$ forces $U = [u_0, \dots, u_{N-1}]$ that cost least over the next 0.4 s with every |u_k| \le u_\max, applies $u_0$ only, and plans again from the new state.

Predicting the horizon

Work with the error from the target, $\mathbf{e} = \mathbf{x} - \mathbf{x}_\text{ref}$. The target is at rest, so $\mathbf{e}_{k+1} = A\mathbf{e}_k + Bu_k$. Step 1's rollout, written as matrices, stacks the next $N$ errors:

Block row $i$ is $\mathbf{e}_{i+1}$. Force $u_j$ reaches it through $A^{i-j}B$; later forces can't reach back, so $\Gamma$ is zero above the diagonal.

The plan as a QP

The given cost_matrices weighs every predicted error with $Q$, the last one with LQR's $P$, and every force with $R$, and returns $H$ and $F$. The cost of a plan becomes $\tfrac12 U^T H U + g^T U$ plus a constant, with $g = F\mathbf{e}_0$.

Projected gradient

Step downhill, then clip back into the box:

$L$, the largest eigenvalue of $H$, gives the longest step that never overshoots. Start each solve from the last plan moved on one period (its last force repeated): it is nearly right already, so 100 steps are plenty.

Your task

1. predict_horizon(A, B, N): return Phi, Gamma.
2. projected_gradient(H, g, u_max, U0, iters): return $U$.
3. mpc(x): warm-start from the global plan, store the new plan in it, return its first force.

The program runs LQR for 2 s, resets, then runs your MPC for 5 s. The white pole is where the plan expects to be 0.4 s ahead. MPC must reach +10 cm without asking for more than 1 N, leaning past 10° or touching a stop.