Linear systems, LQR, and feedback — the EE refresher

Somewhere in your EE degree there was a semester of state-space control — pole placement, a Riccati equation stated without ceremony, then years of never touching it. This lesson reconnects that material to the arm on your desk, because the previous lesson left you holding a time-parameterized joint trajectory and an unanswered question: what actually pushes six joints along it when gravity, friction, and a 200 g payload refuse to match the plan? The answer is feedback, and the sharpest lens on feedback is LQR — the one control problem that, like linear least squares in estimation, has an exact closed-form solution. The vocabulary built here — state, cost-to-go, horizon, gain, phase, replanning rate — is the vocabulary Phase 05 uses to reason about action-chunking VLA policies.

State space and linearization: the five-minute reconnect

A state is the minimal vector that makes the future conditionally independent of the past — the Markov property you demand of any recurrent model's hidden state. For a WidowX AI the physics-level state is joint positions and velocities, $x = (q, \dot q) \in \mathbb{R}^{12}$, and the input is the six joint torques $u = \tau \in \mathbb{R}^{6}$. The dynamics come from Lagrangian mechanics and have a fixed, famous shape:

This is nonlinear — $M$, $C$, and $g$ all depend on configuration — but control theory's oldest trick applies: pick an operating point and Taylor-expand. Choose a hover $(q^{*}, \dot q^{*}=0)$ with feedforward torque $\tau^{*} = g(q^{*})$ canceling gravity there, and work in deviations $\delta x = x - x^{*}$, $\delta u = u - \tau^{*}$:

Two simplifications produce this lesson's workhorse. Apply the gravity feedforward so the constant term vanishes, then look at one joint in isolation and command acceleration rather than torque. What remains is the double integrator: position integrates velocity, velocity integrates the command. Discretize with a zero-order hold at your 100 Hz host control rate — $\Delta t = 10$ ms; the arm's UDP interface would accept references up to its 500 Hz cycle, so this number is your host software's choice, not a transport limit — and the discretization is exact, not approximate:

Before optimizing anything, ask whether control is possible at all. Controllability: from any state, can some input sequence reach any other state in finite time? The Kalman rank test answers with linear algebra — stack the directions the input can push the state over successive ticks and check that they span:

For a healthy torque-controlled arm this test passes trivially — every joint has its own actuator. It fails in ways you will actually meet: an actuator in fault mode makes its joint an uncontrollable free-spinning state. And you already met the task-space cousin: at the Jacobian singularities of Lesson 2, no joint velocity produces motion in the lost Cartesian direction — a controllability collapse in the workspace even while the joint-space test still passes.

LQR: a quadratic bill for error and effort

Now pose control as optimization: penalize state deviation and control effort quadratically over a horizon of $N$ ticks, and demand the input sequence minimizing the total bill:

$Q$ and $R$ are the currency-conversion table between kinds of badness: how many rad² of position error equals one (rad/s²)² of command. Solve by dynamic programming: define the cost-to-go $V_k(x)$ — the cheapest remaining bill from state $x$ at tick $k$ — and hypothesize it stays quadratic, $V_k(x) = x^{\top}P_k x$, seeded with $P_N = Q_f$. The Bellman backup says each stage pays its own cost plus the best achievable future:

The bracket is convex quadratic in $u$ (because $R \succ 0$), so the minimum is where the gradient vanishes. Differentiate, set to zero, solve:

Substitute $u^{*}$ back and collect terms: the result is again a pure quadratic in $x$ — the quadratic family is closed under the backup, which is what makes the problem exactly solvable. Reading off the matrix gives the discrete-time Riccati recursion, running backward from the horizon:

For regulation — holding a setpoint indefinitely — you want the infinite-horizon fixed point: drop the subscripts and the recursion becomes the algebraic equation that scipy.linalg.solve_discrete_are solves directly. The steady-state gain $K$ is computed once, offline, and applied forever as one matrix-vector product per tick. For the full treatment — continuous time, stochastic variants, proofs — see the LQR chapter of Underactuated Robotics (opens in a new tab).

Foundations: Convergence, Algebra, and Units

Before deriving the gain, we must clarify the conditions under which the Riccati iteration is valid and the physical meaning of the cost weights. The lesson states that the iteration converges for "stabilizable, detectable" systems. Stabilizability means that every unstable mode of the plant is controllable; if an unstable mode is uncontrollable, no feedback can stabilize it, and the cost diverges. Detectability means that every unstable mode is observable; if an unstable mode is unobservable, the estimator cannot see it, and the state estimate diverges. For the double integrator, both conditions hold trivially because the system is fully controllable and observable.

The jump from the Bellman backup to the Riccati equation involves substituting the optimal control $u^*$ back into the value function. The backup is $V_k(x) = x^T Q x + u^{*T} R u^* + (Ax+Bu^*)^T P_{k+1} (Ax+Bu^*)$. Substituting $u^* = -K_k x$ and expanding the quadratic form reveals that the result is a pure quadratic in $x$. The coefficient matrix $P_k$ is obtained by grouping terms: the $x^T Q x$ term contributes $Q$, the $u^{*T} R u^*$ term contributes $K_k^T R K_k$, and the cross terms from the next state contribute $A^T P_{k+1} A - A^T P_{k+1} B K_k - K_k^T B^T P_{k+1} A + K_k^T B^T P_{k+1} B K_k$. Using the optimality condition $K_k = (R + B^T P_{k+1} B)^{-1} B^T P_{k+1} A$, these terms simplify to the standard Riccati recursion.

A common source of confusion is the units of the cost weights. The cost function sums $x^T Q x$ and $u^T R u$. For a joint, $x_1$ is position in radians and $x_2$ is velocity in rad/s. Thus, $Q_{11}$ has units of $1/rad^2$ and $Q_{22}$ has units of $1/(rad/s)^2$. These are different physical units. Bryson's rule ensures that each term contributes equally to the cost at the tolerance limit, but it does not make the units identical. The sum is valid because the cost is a dimensionless scalar, but the weights must be scaled appropriately to reflect the relative importance of position error versus velocity error. If you change the units of the state (e.g., from radians to degrees), you must scale $Q$ by the square of the conversion factor to keep the cost invariant.

The distinction between discretization error and transport delay is critical. Discretization error arises from approximating the continuous-time dynamics with a discrete-time model. For the double integrator, the zero-order hold (ZOH) discretization is exact because the input is constant over the sampling interval. For a general nonlinear system, ZOH is an approximation, and the error grows with the sampling time $\Delta t$. Transport delay arises from the time it takes for a command to reach the actuator. This delay introduces phase lag, which can destabilize the loop even if the gain is low. Discretization error affects accuracy, while transport delay affects stability. They are distinct phenomena and must be addressed separately.

Worked Example: Discrete Poles and Continuous Frequency

We now compute the closed-loop poles for the Bryson-tuned double integrator and convert them to continuous-time equivalent frequencies. The discrete closed-loop matrix is $A_{cl} = A - BK$. Given $A = \begin{bmatrix} 1 & 0.01 \\ 0 & 1 \end{bmatrix}$, $B = \begin{bmatrix} 5\times 10^{-5} \\ 0.01 \end{bmatrix}$, and $K = [145.1, 18.5]$, we compute $BK = \begin{bmatrix} 0.007255 & 0.000925 \\ 1.451 & 0.185 \end{bmatrix}$. Thus, $A_{cl} = \begin{bmatrix} 0.992745 & 0.009075 \\ -1.451 & 0.815 \end{bmatrix}$.

Solving the quadratic equation, we find the eigenvalues $\lambda = 0.90387 \pm j0.0725$. To convert to continuous-time equivalent frequencies, we use the relation $\lambda \approx e^{s\Delta t}$, so $s \approx \frac{\ln(\lambda)}{\Delta t}$. For $\Delta t = 0.01$ s, $\ln(0.90387 + j0.0725) \approx -0.1012 + j0.0802$. Thus, $s \approx -10.12 + j8.02$ rad/s. The natural frequency is $\omega_n \approx \sqrt{10.12^2 + 8.02^2} \approx 12.9$ rad/s, which is consistent with the lesson's claim of 12.6 rad/s (the minor difference is due to rounding in $K$).

That leaves the actual engineering: choosing $Q$ and $R$. Bryson's rule is the standard starting point — set each diagonal weight to the inverse square of the largest deviation you will tolerate, so every cost term is order-one at its tolerance limit. For one WidowX-class joint, tolerate 0.05 rad (~2.9°) of position error, 1 rad/s of velocity, and 8 rad/s² of commanded acceleration: $Q = \mathrm{diag}(400, 1)$ and $R = 1/64$. The recursion returns $K = [145.1,\ 18.5]$, and the closed-loop poles land at 12.6 rad/s (about 2.0 Hz) with damping ratio 0.77 — a well-damped loop that settles a 0.05 rad step to 1% in 0.52 s with a peak command of 7.3 rad/s², just inside the budget. Bryson landed in the right decade on the first try; the sweep below is the real tuning.

import numpy as np
from scipy.linalg import solve_discrete_are

DT = 0.01                                 # 100 Hz host control loop
A = np.array([[1.0, DT], [0.0, 1.0]])     # exact ZOH double integrator
B = np.array([[0.5 * DT * DT], [DT]])

def dlqr(A, B, Q, R):
    """Steady-state discrete LQR gain via the algebraic Riccati equation."""
    P = solve_discrete_are(A, B, Q, R)
    K = np.linalg.solve(R + B.T @ P @ B, B.T @ P @ A)
    return K, P

def simulate(K, x0, steps=300):
    """Regulate from x0 toward the origin; return state and command histories."""
    x = np.array(x0, dtype=float)
    xs, us = [x.copy()], []
    for _ in range(steps):
        u = -(K @ x)
        x = A @ x + (B @ u).ravel()
        xs.append(x.copy())
        us.append(float(u[0]))
    return np.array(xs), np.array(us)

def settling_time(xs, x0_mag, frac=0.01):
    """First time after which |position| stays within frac of initial error."""
    pos = np.abs(xs[:, 0])
    for i in range(len(pos)):
        if np.all(pos[i:] < frac * x0_mag):
            return i * DT
    return float("inf")

# Bryson's rule: tolerate 0.05 rad error, 1 rad/s velocity, 8 rad/s^2 command
Q = np.diag([1.0 / 0.05**2, 1.0 / 1.0**2])       # diag(400, 1)

for r in (1.0 / 2.0**2, 1.0 / 8.0**2, 1.0 / 32.0**2):
    K, P = dlqr(A, B, Q, np.array([[r]]))
    xs, us = simulate(K, [0.05, 0.0])
    ts = settling_time(xs, 0.05)
    row = "R = {:9.6f}  K = [{:6.1f}, {:5.1f}]  settle = {:4.2f} s  peak u = {:5.1f}"
    print(row.format(r, K[0, 0], K[0, 1], ts, float(np.max(np.abs(us)))))

Read the last row skeptically: 25.2 rad/s² is three times the 8 rad/s² actuator budget. A real actuator clips that command, and once it clips, the closed loop is no longer the linear system your Riccati equation analyzed — the guarantees are fiction. LQR has no vocabulary for constraints; handling them honestly is the biggest reason MPC exists. And hold on to these three gain sets: they return in the sampling section with dramatically different tolerance to latency.

PID versus LQR: what the arm's controller already does

Time for honesty about the robot you own. The WidowX AI's iNerve controller runs the real-time loop at 500 Hz, and each integrated BLDC joint closes its own FOC current loop beneath that; in the stock position mode you never command torque yourself — you stream joint position targets over UDP, and the vendor's tracking loops (with their configured gains, effort corrections, and friction parameters) do the tracking. When Lesson 7's trajectory gets executed, that is the controller executing it. Unlike a hobby servo arm, the libtrossen_arm driver also exposes velocity, effort, and gravity-compensated external-effort modes — torque-level control is there when you want it — but the stock position mode is where the classical oracle lives.

So is LQR a different species of controller? For this plant, no — and seeing that demystifies both. The Bryson gain $u = -145.1\,q - 18.5\,\dot q$ is exactly a PD controller with $k_p = 145.1$, $k_d = 18.5$. For a double integrator, LQR is PD; what changed is where the numbers came from — a model plus an explicit cost, instead of knob-twiddling until it stops oscillating. The differences appear at scale: for the coupled 12-state arm, LQR returns one 6×12 gain matrix whose off-diagonal entries know that shoulder acceleration disturbs the elbow through the mass matrix, while six independent PID loops treat that coupling as a disturbance to fight after the fact. And when payload changes $M(q)$, re-deriving gains is a recomputation, not a re-tuning session.

The honest verdict for the Phase 02 oracle: at quasi-static pick-and-place speeds — 0.1 to 0.3 m/s with well time-parameterized trajectories — the vendor's stock position-mode tracking is genuinely sufficient, and tracking error will be dominated by trajectory quality, calibration bias, and perception error, not gains. LQR earns its keep differently here: it supplies the cost language Lesson 7's trajectory optimizer already used, a sanity check on any gain you meet, and the debugging question this phase exists to ask — when a pick fails, is this a reference problem or a tracking problem? That is the perception/planning/execution split, posed at the control level.

Where optimal control actually lives in this course

You will rarely deploy vanilla LQR in this course, but you will meet its skeleton constantly, in three escalating disguises. First, trajectory tracking: linearize about a reference trajectory instead of a fixed point, and the same machinery yields a time-varying gain schedule — feedforward carries you along the plan, feedback fights deviations from it:

Second, MPC — the mental model that matters most. At every tick, solve a finite-horizon optimal control problem from the current state with constraints included, execute only the first input, shift the horizon, re-solve. LQR is the degenerate case — infinite horizon, no constraints, linear plant — where the receding-horizon solution collapses to the constant gain you already computed and re-solving is pointless. The moment torque or joint limits bind (your 25.2 rad/s² row), the closed form dies and the per-tick numerical solve earns its milliseconds.

Third, the seed for Phase 05. A π₀-class policy on the OpenPI stack consumes an image and proprioception and emits a chunk of roughly 50 actions at 50 Hz — one second of committed motion — while inference on your RTX workstation takes on the order of 100 ms. Squint with this lesson's eyes: that is MPC with a 1 s horizon, a replanning period of one chunk, no explicit model, and a transformer forward pass as the solver. Every classical question transfers verbatim — how stale can the plan get before it is wrong, what rejects disturbances between replans (nothing but the controller cascade), how should a new chunk blend with the one mid-execution? Real-time action chunking (opens in a new tab) is the controls literature restated in policy-learning language, and your capstone lives in exactly this row of the comparison table.

Sampling, aliasing, and the latency ceiling on feedback

Everything so far pretended $\Delta t$ was free. It is not — it is the resource your latency budget rations. The rule of thumb: sample 10–20× faster than your closed-loop bandwidth. The Bryson design closed a 2 Hz loop, so 100 Hz sampling is a comfortable 50×; the firmware's kilohertz loop similarly oversamples the current dynamics it owns. Run slower and two distinct problems appear — one about information, one about time.

The information problem is aliasing, exactly as in your signals courses — except the disturbance now has mass. Any content above $f_s/2$ folds back into band: an 18 Hz structural vibration of the arm, observed through a RealSense-class camera at 30 fps, appears in the image stream as a clean 12 Hz oscillation ($|30 - 18| = 12$). A controller closing a visual loop will dutifully fight the phantom 12 Hz signal, injecting real torque that can pump energy into the real 18 Hz resonance. The classical rule survives unchanged: anti-alias before sampling — stiffen, damp, or low-pass above the disturbance — because after sampling, an alias is mathematically indistinguishable from truth.

The time problem is delay, and it connects directly back to your Phase 01 instrumentation. A pure delay $\tau$ multiplies the loop transfer function by $e^{-s\tau}$: gain exactly one at every frequency, phase loss growing linearly with frequency. Delay never attenuates — it makes corrections arrive late, which at crossover is indistinguishable from pushing in the wrong direction:

Simulation agrees with the Bode arithmetic. Inject a pure input delay into the double-integrator loop and the three gain sets from the sweep table fail at sharply different points: the gentle set ($R = 1/4$) diverges at 120 ms, the Bryson set at 60 ms, the aggressive set at 30 ms. Each 4× increase in aggressiveness costs roughly half the delay margin. Bandwidth and latency tolerance are the same budget spent two ways, and no gain tuning buys both.

One more classical tool, chosen deliberately for where you are headed. If the delay is known and roughly constant — yours is, to within the jitter you measured — you can do better than detuning: augment the state with the commands currently in flight, $z_k = (x_k, u_{k-d}, \ldots, u_{k-1})$, and run LQR on the augmented system. In-flight commands are facts to condition on, not noise to be robust against, and the augmented design is exactly optimal for the delayed plant. Hold that thought: Phase 05's real-time chunking treats actions committed while inference runs in precisely this way — as constraints on the new chunk, not decisions to remake. You implement the classical version in the exercise below.

Where this goes next: Motion planning: RRT, optimization, and time produced the reference trajectories; this lesson closed the loop around them and, with it, closed Phase 02 — the classical oracle now spans kinematics, cameras, pose, grasps, plans, and control. Next comes Behavioral cloning and distribution shift, the first lesson of the learning phase, and it leans on today's vocabulary immediately: a cloned policy is a feedback controller trained without a model or a stability story, and the compounding-error problem that defines behavioral cloning is what this lesson would call an unstable closed loop — small policy errors push the state off the training distribution, where the errors grow, pushing it further off. You now own the classical baseline that failure gets measured against.