LQR: gains from a cost

Say what you care about

Pole placement works, but which poles do you actually want? The linear-quadratic regulator (LQR) asks a different question: what does a bad future cost? It adds up, over every period from now on,

$Q$ is a 4×4 weight on the state. A diagonal $Q = \mathrm{diag}(q_1, q_2, q_3, q_4)$ charges $q_1 x^2 + q_2 \dot x^2 + q_3 \theta^2 + q_4 \dot\theta^2$: how much you mind the cart being off centre, moving, the pole leaning, tipping. $R$ charges for force. The cheapest controller turns out to be linear again, $u = -K\mathbf{x}$.

The Riccati iteration

Let $\mathbf{x}^T P\,\mathbf{x}$ be the least cost you can still get from state $\mathbf{x}$, the cost-to-go. Start with $P = Q$, which only counts the present, and look one more period ahead each time:

Once $P$ stops changing it is the cost over the whole future, and $K$ from it is the optimal gain. Stop on convergence, not after a fixed count: on the cart-pole $P$ takes hundreds of iterations, and sometimes over a thousand.

Tuning

Only the balance between $Q$ and $R$ matters. Make $R$ bigger and force gets dearer: the controller pushes more gently, the pole takes longer to come back, and the cart travels further to catch it. A bigger weight on $x$ holds the cart nearer the centre.

Your task

1. Implement lqr(A, B, Q, R) for any number of states $n$ and inputs $m$. Return K, P: $K$ is $m \times n$.
2. Choose Q_GENTLE and R_GENTLE for a gentle controller that never asks for more than 4 N and keeps the cart within 15 cm of the centre.

The program balances from 10° with the firm weights ($Q = \mathrm{diag}(100, 1, 100, 1)$, $R = 0.1$), resets, and balances again with yours. Each must bring the pole within 1° of upright, by 1.5 s (firm) and 2 s (gentle). The plots put the two runs on top of each other.