LQR Optimal Control — one cost, one gain

1 · The 5-minute recap

About hover the quad is a linear system \( \dot{\mathbf{x}}=A\mathbf{x}+B\mathbf{u} \), where \( \mathbf{x} \) is the 12-state deviation and \( \mathbf{u}=[\,\delta T,\tau_x,\tau_y,\tau_z\,] \) the wrench deviation. \(A\) is \(12\times12\), \(B\) is \(12\times4\) — exactly the matrices analysis.linearize(p) returned in Week 4.

LQR chooses the input that minimizes a quadratic cost; the answer is linear full-state feedback:

\[ J = \int_0^\infty \big(\mathbf{x}^\top Q\,\mathbf{x} + \mathbf{u}^\top R\,\mathbf{u}\big)\,dt, \qquad \mathbf{u} = -K\mathbf{x}, \qquad K = R^{-1}B^\top P \]

where \( P=P^\top\succeq 0 \) solves the continuous-time algebraic Riccati equation (CARE):

\[ A^\top P + P A - P B R^{-1} B^\top P + Q = 0 \]

The closed loop is \( \dot{\mathbf{x}}=(A-BK)\mathbf{x} \); LQR guarantees every eigenvalue of \(A-BK\) has \( \mathrm{Re}<0 \). \(Q\) penalizes state error (bigger ⇒ faster, more effort), \(R\) penalizes control effort (bigger ⇒ gentler). Only the ratio \(Q/R\) matters.

2 · Setup (2 min)

From the simulator/ directory, in your virtual environment:

# one-time
python -m venv .venv && source .venv/bin/activate   # Windows: .venv\Scripts\activate
pip install -r requirements.txt
# every session — so `import quadsim` works
export PYTHONPATH=.                                # Windows: set PYTHONPATH=.

Everything you need lives in quadsim/analysis.py (linearize, poles, care, lqr) and quadsim/plotting.py (plot_poles). The reference walkthrough is examples/04_analysis.py — open it alongside this sheet.

3 · Design the LQR gain

Work through the steps. You'll write a short script examples/wk7_lqr.py that linearizes the model, designs \(K\), and renders the pole map. The state order is [p(3), v(3), (φ,θ,ψ), ω(3)]; the wrench is [T, τx, τy, τz].

import numpy as np
from quadsim import QuadParams, analysis as an

p = QuadParams()
A, B = an.linearize(p)          # A: 12x12, B: 12x4 — the hover linear model
ol = an.poles(A)                # open-loop eigenvalues
print("open-loop max Re(pole) =", ol.real.max())

Q = np.diag([10, 10, 10,  1, 1, 1,  10, 10, 10,  1, 1, 1.0])  # pos, vel, euler, rates
R = np.diag([1.0, 10, 10, 10])                               # [T, tau_x, tau_y, tau_z]

K, P, cl = an.lqr(A, B, Q, R)   # u = -K x ; cl = closed-loop poles
print("closed-loop max Re(pole) =", cl.real.max())   # ~ -1.74
print("stable:", np.all(cl.real < 0))

from quadsim import plotting as viz
viz.plot_poles([ol, cl], labels=["open loop", "LQR closed loop"],
               save="wk7_poles.png", show=False)
print("saved wk7_poles.png")

for scale in (0.1, 1.0, 10.0):
    _, _, cl_s = an.lqr(A, B, Q, scale * R)
    print(f"R x{scale:>4}: max Re = {cl_s.real.max():7.3f}")

# examples/wk7_lqr.py — Week 7 checkpoint: LQR pole map on the hover model
import numpy as np
from quadsim import QuadParams, analysis as an
from quadsim import plotting as viz

p = QuadParams()
A, B = an.linearize(p)
ol = an.poles(A)

Q = np.diag([10, 10, 10,  1, 1, 1,  10, 10, 10,  1, 1, 1.0])
R = np.diag([1.0, 10, 10, 10])
K, P, cl = an.lqr(A, B, Q, R)

print(f"open-loop   max Re(pole) = {ol.real.max():7.3f}")
print(f"closed-loop max Re(pole) = {cl.real.max():7.3f}  stable={np.all(cl.real < 0)}")

viz.plot_poles([ol, cl], labels=["open loop", "LQR closed loop"],
               save="wk7_poles.png", show=False)
print("saved wk7_poles.png")

4 · Run & verify

Run your script — and the shared reference example, which does the same design and also flies a figure-eight with the cascade controller:

export PYTHONPATH=.
python examples/wk7_lqr.py
python examples/04_analysis.py     # reference: prints the same LQR line, saves analysis_poles.png

Self-check

• Every closed-loop pole has a strictly negative real part (max Re ≈ −1.74).
• Scaling R up moved the rightmost pole toward the origin; scaling it down moved it left.
• I can explain why R must be positive definite and Q only PSD.
• I compared LQR to my Week-5/6 PID on the same step: overshoot, settling, and effort.

5 · Going further (optional)

• PID vs LQR, head to head. From a 20° tilt, run your Week-5 PID and the LQR law on the same start. Compare overshoot, settling time, and control effort \( \int \|\mathbf{u}\|^2\,dt \) — one short paragraph.
• Bryson's rule. Set \(Q_{ii}=1/x_{i,\max}^2\) and \(R_{jj}=1/u_{j,\max}^2\) from your own "acceptable" limits, and see how the pole map shifts.
• Controllability. Call an.controllability(A, B) and confirm rank 12 — the precondition that makes LQR stabilizing in the first place.

6 · Deliverable & submission

Reference: MIT 16.323 (How), Lec 3–4 — ocw.mit.edu/courses/16-323; conventions in simulator/CONVENTIONS.md. Next week → Week 8: model-predictive control — LQR with a horizon and input limits.