Model predictive control

What it computes

LQR assumes the motor delivers whatever it asks for. This motor gives at most 0.8 N; LQR's first request on this 15 cm move is 3.5 N. Model predictive control puts the limit into the problem: every period it plans the next $N$ inputs $U = [u_0, \dots, u_{N-1}]$ subject to |u_k| \le u_\max, applies $u_0$, and plans again.

Condensing

Work with the error $e = x - x_\text{ref}$. The target is an equilibrium, so $e_{k+1} = A e_k + B u_k$, and stacking the next $N$ errors gives $E = \Phi e_0 + \Gamma U$:

Let $\bar Q = \operatorname{diag}(Q, \dots, Q, P)$, where the terminal weight $P$ is LQR's cost-to-go. The cost $\sum_{k=1}^{N-1} e_k^T Q e_k + e_N^T P e_N + R \sum_{k=0}^{N-1} u_k^2$ is then twice $\tfrac12 U^T H U + g^T U$, plus a constant, with

Projected gradient

Clipping projects onto the box, so: step $1/L$ down the gradient ($L$ is $H$'s largest eigenvalue), clip, repeat.

def solve_box_qp(H, g, u_max, U0, iters):
    U = U0
    repeat iters times:
        U = clip(U - (H @ U + g) / L, -u_max, u_max)

every period:                                   # mpc(x, x_ref)
    g = F @ (x - x_ref)
    plan = solve_box_qp(H, g, u_max, last plan shifted one period, 100)
    apply plan[0]

The last plan, shifted one period, is nearly optimal already, so 100 iterations per period are enough.

Tools

np.linalg.matrix_power, np.kron(np.eye(N), Q), np.linalg.eigvalsh(H)[-1] (the largest eigenvalue) and np.clip.

The program

LQR drives the cart for 2 s, then the cart is reset and your mpc runs for 5 s. H, F = condense(A, B, Q, R, P, N) is built once, and the global plan (zeros at first) holds mpc's latest plan.

Your task

Implement condense, solve_box_qp and mpc. The grader compares the first two with the reference, then checks that MPC reaches the target without the pole leaning past 10°, touching a stop or asking for more than 0.8 N.