M5-T7: LQR control

Learning objectives

After this lesson, you should be able to:

• State the LQR problem and its optimal feedback law.
• Linearise a quadrotor about hover and build the A and $B$ matrices of a position-attitude subsystem.
• Solve the discrete Riccati equation iteratively and check closed-loop stability.
• Explain how $Q$ and $R$ shape the response and how LQR compares with PID.

Conceptual explanation

The linear-quadratic regulator minimises an infinite-horizon cost that adds up squared state error and squared control effort. For a linear model the solution is a constant state-feedback gain $K$: $u=-K\,x$. The tuning knobs are two weight matrices: $Q$ says how much each state error matters, $R$ says how expensive each input is.

Unlike separate PID loops, LQR uses all states at once: it knows, for instance, that position error should be corrected through a coordinated change of velocity and tilt rather than by each loop independently.

Engineering intuition

Large $Q$ on a state says “keep this near zero, even at the cost of effort”; large $R$ says “be gentle with the actuator”. The ratio sets the speed of response: heavier $Q$ gives a faster, more energetic controller. Because the design is optimal for the model, it is only as good as the model: it inherits guaranteed stability margins for the ideal linear case, but real motor lag, delays and saturation reduce them, so verify in simulation with those effects before flying.

Mathematical formulation

Discrete model $x_{k+1}=A\,x_{k}+B\,u_{k}$ and cost $J=\sum (x_{k}^{\top}\,Q\,x_{k}+u_{k}^{\top}\,R\,u_{k})$ with $Q$ ⪰ 0, $R$ ≻ 0:

$x$

state error relative to the reference (for position control: position, velocity, attitude, rate errors).

$Q$

state weights, diagonal in practice; Bryson’s rule sets $Q_{ii}=1$/(max acceptable $x_{i}$)$^{2}$.

$R$

input weights; Bryson’s rule sets $R_{jj}=1$/(max acceptable $u_{j}$)$^{2}$.

$P$

cost-to-go matrix: $V(x)=x^{\top}\,P\,x$.

Linearised hover (one horizontal axis)

About hover, with small angles and $x$ forward, $\ddot{x}=g\,\theta$, $\ddot{\theta }=\tau /J$. With state $[x,\,\dot{x},\,\theta ,\,\omega ]$ and input the normalised torque $u=\tau /J$:

Four integrator-chain states, one input: the pair ($A$, $B$) is controllable, so an LQR exists. The solution has the cascaded look of PID (position error to velocity to tilt to rate) but with gains chosen jointly.

For reference tracking, define the error relative to the reference trajectory and add feedforward, or augment with an integrator state to reject constant disturbances (LQI).

Architecture and data flow

Design offline: linearise, discretise, choose $Q$ and $R$, solve the Riccati equation, store $K$. Online: compute the state error from the estimator, $u=-K\,e$, pass the torque (or acceleration) command to the next loop. If the estimator’s delay is significant, include it in the model, because LQR assumes the state is known exactly and instantly.

Real-world UAV applications

• Hover and position hold with coordinated multi-state feedback.
• Gain-scheduled controllers across payload or speed.
• Terminal controller and cost-to-go in MPC (the same $P$ matrix).
• Quick controller design for new airframes from CAD-derived inertia.

Implementation example

This runs as shown: it builds the discretised hover model, solves the Riccati equation by iteration (no SciPy required), and simulates the closed loop from a 2 m position error, comparing with an arbitrary hand-picked gain set.

import numpy as np
g, dt = 9.81, 0.01
Ac = np.array([[0, 1, 0, 0], [0, 0, g, 0], [0, 0, 0, 1], [0, 0, 0, 0.0]]); Bc = np.array([[0], [0], [0], [1.0]])
# zero-order-hold discretisation: A = exp(Ac dt), B = (integral of exp(Ac s) ds from 0 to dt) Bc, both by their power series
A = np.eye(4); term = np.eye(4)
for k in range(1, 6):
    term = term @ (Ac * dt) / k; A = A + term
B = np.zeros((4, 1)); Mt = np.eye(4) * dt
for k in range(6):
    B += Mt @ Bc; Mt = Mt @ (Ac * dt) / (k + 2)

def dlqr(A, B, Q, R, iters=20000):
    P = Q.copy()
    for _ in range(iters):
        K = np.linalg.solve(R + B.T @ P @ B, B.T @ P @ A)
        Pn = Q + A.T @ P @ (A - B @ K)
        if np.max(np.abs(Pn - P)) < 1e-11: P = Pn; break
        P = Pn
    return np.linalg.solve(R + B.T @ P @ B, B.T @ P @ A), P

# Bryson's rule: 1 m position, 1 m/s, 0.2 rad, 1 rad/s acceptable; input 5 rad/s^2
Q = np.diag([1 / 1.0**2, 1 / 1.0**2, 1 / 0.2**2, 1 / 1.0**2]); R = np.array([[1 / 5.0**2]])
K, P = dlqr(A, B, Q, R)
print("K =", np.round(K, 3))
rho = max(abs(np.linalg.eigvals(A - B @ K)))
assert rho < 1                                                 # closed loop is stable

def run(K, T=12.0):
    x = np.array([2.0, 0, 0, 0]); out = []
    for _ in range(int(T / dt)):
        u = np.clip(-K @ x, -30, 30)
        x = A @ x + (B @ u).ravel(); out.append(x[0])
    return np.array(out)
traj = run(K)
t_settle = np.argmax(np.abs(traj[::-1]) > 0.1 * 2.0)            # last time |x| exceeds 10% of the step
print(f"closed-loop spectral radius {rho:.4f}; position error back inside 10% after {(len(traj) - t_settle) * dt:.1f} s; final {traj[-1]:.4f} m")
assert abs(traj[-1]) < 0.01 and abs(traj.min()) < 1.0          # converges without wild undershoot

Common engineering mistakes

Failure modes

Debugging approach

1. Check controllability (rank of [$B$ AB $A^{2}B\ldots$]) and the eigenvalues of A − BK.
2. Validate in simulation with the real sensor delay and motor lag before flying.
3. Compare step responses with those of the PID baseline on the same plant.
4. Sweep the Q/R ratio and plot response time against control effort to find a sensible compromise.

Practical exercises

1. Apply Bryson’s rule for a 0.5 m position tolerance and a 0.1 rad tilt limit; compare $K$ with the example.
2. Add an integral of position error as a fifth state (LQI) and test against constant wind.
3. Show that scaling $Q$ and $R$ by the same factor does not change $K$.
4. Compare closed-loop poles of the LQR with those of a PD cascade you tuned in lesson M5-T6.

Interview questions

Write an answer in your own words, then reveal the model answer. Nothing is sent anywhere.

References and further reading

Primary papers, official documentation and standards. Each claim above that is specific to a tool or version points here.

• $B$. $D$. $O$. Anderson, $J$. $B$. Moore, Optimal Control: Linear Quadratic Methods, Prentice-Hall, 1990.
• $K$. $J$. Åström, $R$. $M$. Murray, Feedback Systems, Princeton University Press, 2008, chapter 6.
• R. Mahony, V. Kumar, P. Corke, IEEE RAM 2012

What’s next