LQR

What it computes

The linear-quadratic regulator is the best linear feedback $u = -Kx$ for a model $x_{k+1} = A x_k + B u_k$. "Best" means it minimises

$Q$ charges for being away from the goal (upright, at the rail centre) and $R$ charges for force. Raise $Q$ and the controller corrects hard and fast; raise $R$ and it pushes gently, so it takes longer and the cart travels further. Only the ratio matters.

The Riccati recursion

The lowest cost from state $x$ is a quadratic, $x^T P x$. One step of dynamic programming (pay for this step, then $P$ from wherever you land) updates it:

Starting from $P = Q$, each update looks one step further ahead. Once $P$ stops changing, it is the infinite-horizon cost and the gain is $K = (R + B^T P B)^{-1} B^T P A$.

P = Q
repeat:
    K = solve(R + B.T @ P @ B, B.T @ P @ A)
    P_next = Q + A.T @ P @ (A - B @ K)     # the same update, written with K
    stop once P_next is (relatively) equal to P
    P = P_next
return K computed from the final P

Use a convergence test, not a fixed count: on the cart-pole, 100 iterations still leave $K$ about 2 % off.

Tools

cartpole.linearize() returns (A, B) for one 20 ms control period. The state is $[x, \dot x, \theta, \dot\theta]$ (m, m/s, rad, rad/s); the input is the force on the cart (N).

The program

The pole starts about 10° from upright, to either side. The program computes $K$ with $Q = \mathrm{diag}(10, 1, 100, 1)$ and $R = 0.1$, then applies $u = -Kx$ at 50 Hz for 3 s.

Your task

Implement lqr(A, B, Q, R), returning $K$ as an $m \times n$ array, for any sizes. The grader compares it with the reference on the cart-pole and three random systems, then checks the pole is within 1° of upright by 2 s, with no request over 10 N and no end-stop hit.