UAV Systems Engineering · M5-L2 · Guided lab

Implement LQR

Compute K from Q and R yourself, then see what the weights do.

UAV Guidance, Navigation and Control (GNC)
Established engineering
About 45 minutes

What you will do

You will design an LQR for a 1D double integrator (position, velocity) that stands in for one translational axis of a multirotor near hover, using an iterative Riccati solution (no control library needed). Then you will explore the Q/R trade-off and an actuator limit.

Before you start

Lesson M5-T7 (LQR).
Python 3 with NumPy.

Starter

import numpy as np
dt = 0.02
A = np.array([[1, dt], [0, 1]]); B = np.array([[0.5 * dt**2], [dt]])    # discrete double integrator: acceleration command
Q = np.diag([10.0, 1.0]); R = np.array([[1.0]])

def dlqr(A, B, Q, R, iters=2000):
    # TODO 1: iterate the discrete Riccati recursion  P <- Q + A'PA - A'PB (R + B'PB)^-1 B'PA
    # TODO 2: return K = (R + B'PB)^-1 B'PA and P
    raise NotImplementedError

Starter: implement the Riccati iteration and the gain.

Reference solution

Show the solution (try first)

import numpy as np
dt = 0.02
A = np.array([[1, dt], [0, 1]]); B = np.array([[0.5 * dt**2], [dt]])
def dlqr(A, B, Q, R, iters=5000):
    P = Q.copy()
    for _ in range(iters):
        G = np.linalg.solve(R + B.T @ P @ B, B.T @ P @ A)
        Pn = Q + A.T @ P @ A - A.T @ P @ B @ G
        if np.max(np.abs(Pn - P)) < 1e-12: P = Pn; break
        P = Pn
    return np.linalg.solve(R + B.T @ P @ B, B.T @ P @ A), P
def simulate(Q, R, umax=None, T=8.0):
    K, P = dlqr(A, B, Q, R); x = np.array([[5.0], [0.0]]); xs = []; u_peak = 0.0
    for _ in range(int(T / dt)):
        u = -K @ x
        if umax is not None: u = np.clip(u, -umax, umax)
        u_peak = max(u_peak, abs(u.item())); x = A @ x + B @ u; xs.append(x.ravel().copy())
    return K, np.array(xs), u_peak
K, xs, up = simulate(np.diag([10.0, 1.0]), np.array([[1.0]]))
rho = max(abs(np.linalg.eigvals(A - B @ K)))
print(f"K = {K.ravel()}, closed-loop spectral radius {rho:.4f}, peak command {up:.2f} m/s^2, position at 8 s {xs[-1, 0]:.5f}")
assert rho < 1 and abs(xs[-1, 0]) < 1e-3
Kc, xc, uc = simulate(np.diag([10.0, 1.0]), np.array([[100.0]]))          # expensive control: slower, gentler
Ka, xa, ua = simulate(np.diag([10.0, 1.0]), np.array([[0.01]]))          # cheap control: faster, harder
print(f"peak command: R=100 {uc:.2f}, R=1 {up:.2f}, R=0.01 {ua:.2f}")
assert uc < up < ua
Qm = np.diag([10.0, 1.0]); P = dlqr(A, B, Qm, np.array([[1.0]]))[1]; x0 = np.array([5.0, 0.0])
cost = sum(float(x @ Qm @ x + 1.0 * (-(K @ x)).item() ** 2) for x in xs)             # accumulated cost along the closed-loop run (starts at the second state)
cost += float(x0 @ Qm @ x0 + (-(K @ x0)).item() ** 2)
print(f"simulated cost {cost:.1f}  vs  x0' P x0 = {float(x0 @ P @ x0):.1f}")
assert abs(cost - float(x0 @ P @ x0)) / float(x0 @ P @ x0) < 0.02

Solution with Q/R comparison.

Experiments

Sweep $R$ over 0.01, 0.1, 1, 10, 100 and tabulate peak command and 2 % settling time.
Add actuator saturation at $2\,\mathrm{m}/\mathrm{s}^{2}$ and show whether the closed loop remains stable for the large initial offset. What does that tell you about LQR and constraints?
Add a constant acceleration disturbance of $1\,\mathrm{m}/\mathrm{s}^{2}$ and measure the steady error. Then augment the state with an integral of the position error and redesign.
Verify the Riccati solution against the closed-form: compute the cost $\Sigma$ ( $x$ ᵀQx + $u^{\top}$ Ru) over a long simulation and compare with $x_{0}^{\top}\,P\,x_{0}$ .

How to know it worked

The solution runs and its assertions pass.
The closed-loop spectral radius is below 1.
You have a table of peak command and settling time against $R$ .

If it does not work

Troubleshooting
Symptom	Likely cause	What to check
Riccati iteration does not converge	The pair ( $A$ , $B$ ) is not stabilisable or $Q$ is not positive semidefinite	Check controllability and the weights
Closed loop unstable with saturation	LQR ignores actuator limits	Use smaller initial errors, reference shaping or MPC
Steady error with a disturbance	No integral action in the state	Augment with the integral of the error

References

Record your progress

Saved in this browser. This lab runs on your own machine, so the site cannot check it for you.