Section 7.4: State-space control, LQR

A Careful Control Loop

This section develops the technical contract for State-space control, LQR into a usable mental model. First we define the object of study, then we connect it to the agent loop, then we test it with a compact implementation.

The key question in State-space control, LQR is practical: what must the agent know, what can it observe, what action is available, and what evidence shows that the action worked under the stated conditions?

Theory

For State-space control, LQR, the practical design rule is to make the interface inspectable before optimization begins: inputs, outputs, units, latency, bounds, and failure labels should all be visible in the saved artifact.

State-space control writes the robot near an operating point as $x_{t+1}=Ax_t+Bu_t$, where $x_t$ collects the state variables and $u_t$ collects the commands. LQR adds a quadratic preference: make important state errors small while avoiding commands that are too large or too fast. The matrices $Q$ and $R$ are therefore not arbitrary knobs. They encode which errors are costly, which actuators are precious, and which units must be normalized before comparison.

Worked Example

LQR turns a cost preference into a feedback gain by solving an algebraic Riccati equation. For \(\dot x = Ax + Bu\), the controller minimizes \(\int_0^\infty (x^\top Q x + u^\top R u)\,dt\); the optimal law is \(u=-Kx\) with \(K=R^{-1}B^\top P\), where \(P\) solves the continuous-time ARE. Code Fragment 7.4.1 stabilizes a cart-pole linearized about the upright equilibrium. The state is \([\text{pos},\text{vel},\text{angle},\text{ang.\,vel}]\); the \(Q\) matrix makes the pole angle ten times as expensive as cart position.

import numpy as np
from scipy.linalg import solve_continuous_are

# Cart-pole linearized about upright. State x = [pos, vel, angle, ang_vel].
g, M, m, l = 9.81, 1.0, 0.1, 0.5
A = np.array([[0, 1, 0,              0],
              [0, 0, (m * g) / M,    0],
              [0, 0, 0,              1],
              [0, 0, (M + m) * g / (M * l), 0]], dtype=float)
B = np.array([[0], [1 / M], [0], [1 / (M * l)]], dtype=float)

Q = np.diag([1.0, 1.0, 10.0, 1.0])   # angle error is 10x as costly as position
R = np.array([[0.1]])                  # control effort is cheap

P = solve_continuous_are(A, B, Q, R)
K = np.linalg.inv(R) @ B.T @ P
print("LQR gain K =", np.round(K.ravel(), 2))
cl_eig = np.linalg.eigvals(A - B @ K)
print("closed-loop eigenvalues =", np.round(cl_eig, 2))
print("stable:", bool(np.all(cl_eig.real < 0)))

# 5 s rollout from a 0.2 rad tilt, Euler integration.
dt, x = 0.01, np.array([0, 0, 0.2, 0.0])
for _ in range(500):
    u = -K @ x
    x = x + dt * (A @ x + (B @ u).ravel())
print(f"angle 0.20 rad -> {x[2]:+.4f} rad after 5 s (regulated to upright)")

Practical Recipe

1. Write the observation, action, and success metric before choosing a model.
2. Build a baseline that is simple enough to debug by inspection.
3. Add the library implementation only after the baseline behavior is understood.
4. Record failures as structured cases: perception error, state error, planning error, control error, or evaluation error.
5. Run at least one perturbation test before trusting the result.

Production Pattern

State-space control, LQR sits inside the Part II robotics contract: geometry defines where things are, kinematics defines what motion is possible, dynamics defines what motion costs, control defines how errors are corrected, and sensing defines what the agent can know on time.

Connect LQR costs to units and behavior, since the matrix entries are design choices rather than unexplained constants. This makes the section useful to students, builders, and researchers at the same time: the idea has an intuitive role, a formal interface, a runnable check, and a failure mode that can be reproduced.

Use this recipe when turning State-space control, LQR into code, a simulator experiment, or a robot diagnostic. The point is not to use every library. The point is to keep the hand-built baseline and the maintained-tool path comparable.

1. Write the control objective, measured state, actuator command, update rate, and saturation policy.
2. Run a step-response test before adding learning, with overshoot, settling time, and steady-state error logged.
3. Compare the hand controller with python-control, CasADi, Drake, do-mpc, or ROS 2 control on the same plant model.
4. Record latency, missed deadlines, saturation events, constraint violations, and recovery actions.
5. Only compare controllers and policies when they share sensors, action limits, disturbance tests, and safety checks.

A learned policy can hide a poor LQR linearization until the robot leaves the neighborhood where $A$ and $B$ were valid. Check equilibrium choice, state ordering, units, controllability, sample time, saturation, and the closed-loop eigenvalues before scaling training. For this section, first reproduce one tiny state-space case by hand, then rerun it through python-control or Drake. If the two disagree, inspect matrix convention, discrete versus continuous time, and cost scaling before changing the model.