How a Robot Stays on the Trajectory

Chapter 1: The Error — What Every Controller Watches

A controller is a machine with one obsession: drive the error to zero. So before anything else, we have to define the error precisely, because every controller in this lesson — P, PID, feedback-linearizing, LQR — is just a different recipe for reacting to it.

Let x(t) be the robot's actual state (for a wheeled robot, its pose — recall Lesson 1's ξ = (x, y, θ)). Let x_d(t) be the desired or reference state — where the plan says the robot should be at time t. The tracking error is simply the difference:

Read it carefully. e is desired minus actual. If the robot is exactly on plan, e = 0. If the robot is behind, e points "forward toward where it should be." The controller's entire job is to make e → 0 and keep it there, despite disturbances.

Feedforward plus feedback

A trajectory tracking controller has two pieces, and naming them keeps the design clean. Stanford writes the control law as:

• u_d(t) is the feedforward term — the open-loop command the planner already computed (Lesson 5). On a perfect, disturbance-free robot this alone would track the trajectory. It does the bulk of the work: "turn the wheels roughly this fast to follow the curve."
• π(x − x_d, t) is the feedback term — a function of the error that corrects for whatever the feedforward got wrong. On a perfect robot it is exactly zero; the moment a disturbance creates error, it wakes up and pushes back.

For most of this lesson we will study the feedback term π on its own, often by setting u_d = 0 and asking the controller to drive the robot to a fixed point (the goal). That is called regulation: hold the state at a setpoint. Tracking a moving reference is regulation with a feedforward term added on top, so everything we learn about regulation transfers directly.

Error dynamics: how the error itself evolves

Here is the conceptual leap that makes control a science instead of guesswork. We do not just watch the error — we write down a differential equation for how the error changes over time, called the error dynamics. If the robot's state obeys some dynamics and we apply a control law, then the error e(t) obeys its own dynamics. Designing a controller means shaping those error dynamics so that e decays to zero.

Take the simplest possible case: a 1-D robot whose position x has velocity directly commanded, ẋ = u. We want to hold it at a fixed setpoint x_d (so ẋ_d = 0). The error is e = x_d − x, so its rate of change is:

Now suppose we choose the feedback law u = k·e (push proportionally to the error — the subject of Chapter 2). Substitute it in:

This is the most important little equation in the lesson. It says the error shrinks at a rate proportional to itself. Its solution is an exponential decay:

If k > 0, the error decays to zero, smoothly, forever — the controller is stable. If k < 0, the error grows exponentially — the robot runs away. The sign and size of the gain determine whether the error dies or explodes. Larger k means faster decay. This single fact — that a control law turns into error dynamics, and stability is a property of those dynamics — is the engine behind everything in this lesson.

Watching the error decay, by hand

Let's verify the exponential claim with actual numbers, so it isn't just a formula on the page. Start the error at e(0) = 8 with gain k = 0.5. In continuous time the prediction is e(t) = 8·e^−0.5t. Sample it at one-second marks:

Every increase of 2 seconds multiplies the error by e⁻¹ ≈ 0.37 — it loses 63% of whatever is left. There is no point where the error suddenly arrives at zero; it approaches asymptotically, halving and halving. The quantity τ = 1/k (here 2 seconds) is the time constant — the time to fall to 37% of the current value. Double the gain and you halve the time constant: the system gets to "close enough" twice as fast. This is the dial every controller in this lesson turns.

Now flip the sign to see runaway. With k = −0.5, the same table reads e(t) = 8·e^+0.5t: at t = 6 the error is 8·e³ ≈ 161, twenty times bigger than it started. A negative gain doesn't merely fail to fix the error — it actively pumps energy into it. Sign errors in feedback are the classic way to turn a stable robot into a violently unstable one, which is why Chapter 1's sign convention is not pedantry.

The lesson of the widget: a controller is a way of choosing the error dynamics. Every controller we build is an answer to the question "what should ė be?" Proportional control makes it −k·e (first-order decay). PID adds an integral and a derivative term to shape it better. LQR computes the gain that makes the error decay optimally. But they all live inside this one idea: shape the error dynamics so e → 0.

One more subtlety worth nailing now: tracking a moving reference and regulating to a fixed point are the same problem in error coordinates. If x_d(t) moves, then ė = ẋ_d − ẋ picks up the reference's own velocity ẋ_d as an extra term. That extra term is exactly what the feedforward u_d is there to cancel: supply u_d so the reference's motion is accounted for, and the feedback term is left to handle only the leftover error e — back to the clean ė = −k·e we just analyzed. So everything we derive for "hold at a setpoint" carries straight over to "follow a trajectory," with feedforward absorbing the reference's motion.

python
# The error dynamics e' = -k e, simulated and checked against the analytic solution.
import numpy as np
k, e0, dt = 0.5, 8.0, 0.01
e = e0
for step in range(601):
    t = step * dt
    if step % 100 == 0:                       # print once per second
        analytic = e0 * np.exp(-k * t)        # the closed-form decay
        print(f"t={t:.0f}  sim={e:.2f}  analytic={analytic:.2f}")
    e = e + dt * (-k * e)                      # one Euler step of e' = -k e
# t=0 sim=8.00 analytic=8.00 ; t=2 sim~2.94 ; t=6 sim~0.40  -- they agree

Chapter 2: Proportional Control — Push Back in Proportion

The simplest feedback law in existence is one line: command an action proportional to the error. If you are far from the target, push hard; if you are close, push gently; if you are there, do nothing. That is proportional control, the P in PID:

Here K_p (the proportional gain) is a single positive number you choose. It is the controller's aggressiveness dial — how hard it reacts per unit of error. A thermostat with a proportional valve, a car's cruise control easing onto the gas as it slows on a hill, a person steering a shopping cart back toward the aisle center: all proportional control.

A worked step response, by hand

Let us actually compute what a P-controller does, step by step, with real numbers — no hand-waving. Take a discrete-time first-order system (we step in ticks of size Δt, which is how a real digital controller runs):

Pick concrete values: setpoint x_d = 10, start at x₀ = 0, timestep Δt = 1, and a gentle gain K_p = 0.5. Substituting Δt·K_p = 0.5, each tick moves the robot halfway-ish toward the target. Let us crank it:

Trace it. At tick 0 the error is the whole 10 units, so the controller commands a big move of 5. Halfway there, the error has halved, so the next command halves too. Each tick closes half the remaining gap. The robot approaches 10 smoothly, never overshooting, slowing as it nears — a clean exponential approach. After 5 ticks it's at 9.84, about 98% of the way. This is the discrete cousin of the e^−kt decay from Chapter 1.

What happens if we get greedy?

Now redo it with an aggressive gain. Set K_p = 1.8 (so Δt·K_p = 1.8). Watch what overshoot looks like in numbers:

Disaster. The big gain commands a move of 18, blowing straight past the target to 18. Now the error is negative 8, so the controller slams it back to 3.6 — overshooting the other way. It oscillates, bouncing around the target with growing-then-shrinking swings (this one happens to decay slowly; push K_p past 2 and the swings grow and the system goes unstable). High gain means fast response but overshoot and ringing. Low gain means smooth but sluggish. That trade-off is the eternal headache of the proportional controller, and it's exactly what you'll feel in the widget below.

Steady-state error: the P-controller's secret flaw

Here is a subtle thing the numbers above hid because that system had no disturbance. Suppose there is a constant load — the robot rolls on a slight downhill incline, so gravity adds a steady push of, say, +2 units every tick that the controller must fight. In equilibrium the robot stops moving only when its command exactly cancels the load: K_p·e + 2 = 0, which means the controller needs a permanent nonzero error just to produce the cancelling command:

The robot settles near the target but never at it — it parks a little downhill of the line forever, because if e were zero the controller would command zero and gravity would pull it back. This leftover gap is the steady-state error. You can shrink it by raising K_p (e_steady = −2/K_p gets smaller), but you can never kill it with proportional control alone, and pushing K_p up brings back the overshoot. To truly eliminate steady-state error you need a new idea: the integral term. That's Chapter 3.

Let's put a number on it. With the load = 2 and K_p = 0.5, the robot settles at e = −2/0.5 = −4 — a full 4 units short of the target of 10, parking at 6 forever. Double the gain to K_p = 1: e = −2/1 = −2, parking at 8 (better, but still off). Push to K_p = 4: e = −0.5, parking at 9.5 (close, but we're now near the overshoot regime). The gain that would make the error truly zero is infinite — impossible. Proportional control trades steady-state accuracy against stability, and can never win both. That fundamental limitation is precisely why the integral term exists.

The reaching law: why a P-controller can't overshoot a first-order system

Notice that the smooth first table (K_p = 0.5) never overshot, while the aggressive one (K_p = 1.8) oscillated. For a pure first-order system, the boundary is sharp: as long as Δt·K_p < 1, each step closes a fraction of the gap and the approach is monotone (no overshoot). At Δt·K_p = 1 it reaches the target in a single step ("deadbeat"). Between 1 and 2 it overshoots and rings but still converges. Above 2 it diverges. We'll see in Chapter 9 that this exact bound — Δt·K_p < 2 for stability — falls straight out of the discrete error dynamics. For now, the takeaway is visceral: more gain is not always more better.

python
# Proportional controller, from scratch. The entire feedback loop.
def p_control_step(x, x_des, Kp):
    e = x_des - x          # tracking error: desired minus actual
    u = Kp * e             # command proportional to the error
    return u

# simulate the smooth case from the table above
x, x_des, Kp, dt = 0.0, 10.0, 0.5, 1.0
for t in range(6):
    u = p_control_step(x, x_des, Kp)
    print(f"t={t}  x={x:.2f}  e={x_des-x:.2f}  u={u:.2f}")
    x = x + dt * u         # integrate one step (Euler)
# t=0 x=0.00 e=10.00 u=5.00  ...  matches the worked table exactly

Chapter 3: PI and PID — Memory and Anticipation

Proportional control reacts to the error right now. That leaves it blind in two directions: it cannot remember that the error has been stubbornly nonzero for a while (so it tolerates steady-state error), and it cannot anticipate that the error is rushing toward zero fast (so it overshoots). PID fixes both by adding two more terms. The full controller is three reactions summed:

Three terms, three time-tenses, three jobs:

• P — the present. K_p·e reacts to the error now. The main workhorse, as in Chapter 2.
• I — the past. K_i·∫e accumulates the error over all past time. If a small error persists, the integral grows and grows until the command is big enough to crush it. This is what kills steady-state error.
• D — the future. K_d·(de/dt) reacts to how fast the error is changing. If the error is plummeting toward zero, D applies the brakes before the robot overshoots. This is what damps oscillation.

Why the integral kills steady-state error — with numbers

Recall the downhill robot from Chapter 2: a constant load of +2 forced the P-controller to settle at e_steady = −2/K_p, never zero. Add an integral term and watch the gap close. The integral term accumulates the error each tick: I_t+1 = I_t + e_t·Δt, and contributes K_i·I to the command. Take K_p = 0.5, K_i = 0.3, load = −2 (a steady pull), x_d = 10, start near the P steady-state at x₀ = 6 (where P alone would stall, since −2/0.5 = −4, settling at 6):

Watch the integral term I do its work. With P alone the robot was frozen at 6 with a permanent error of 4. But the integral keeps accumulating that error — tick after tick the stored I grows, and K_i·I climbs until the command finally overpowers the load and pushes the robot up past 10. At equilibrium the error reaches zero, but the integral term keeps the command at exactly +2 to cancel the load. The integral provides the steady command the load needs, freeing the error to go to zero. P couldn't do that because P's command vanishes when e = 0; I's command lives on a memory of past error, so it survives.

To make windup concrete: imagine the robot is jammed against an obstacle for 10 ticks with a constant error of e = 5 and Δt = 1. The integral grows by 5 each tick — after 10 ticks it holds I = 50. With K_i = 0.3 that's a stored command of 15, far larger than anything the task needs. The instant the obstacle clears, the controller dumps that 15-unit command into the actuator and the robot rockets past the setpoint before the integral slowly bleeds back down. The fix is to simply stop accumulating while the actuator is pinned at its limit, so I never balloons in the first place — that one if is the difference between a smooth recovery and a launch.

Why the derivative damps oscillation

The derivative term watches de/dt — the speed at which the error is changing. Suppose the robot is racing toward the setpoint: the error is positive but shrinking fast, so de/dt is large and negative. The D term, K_d·(de/dt), is therefore negative — it subtracts from the command, easing off the gas before the robot blows past the target. It is exactly the instinct of a driver lifting off the accelerator as the red light approaches, rather than flooring it until the last instant. D adds damping: it converts a bouncy, oscillatory approach into a smooth glide.

Watch it numerically on a small slice. A mass approaches the setpoint; here are two consecutive ticks with K_p = 8, K_d = 4, Δt = 0.1. Say the error was 0.5 last tick and 0.3 this tick (closing fast). The derivative estimate is (0.3 − 0.5)/0.1 = −2.0. The P part says push hard: 8·0.3 = +2.4. But the D part says brake: 4·(−2.0) = −8.0. The total command is 2.4 − 8.0 = −5.6 — negative, against the direction of travel, even though there's still positive error. Without D, the mass would keep accelerating into the target and overshoot; with D, it's already decelerating while still a bit short. That early braking is what converts overshoot into a clean stop.

Tune all three on a second-order system

A real robot is rarely first-order. A mass being pushed (a cart, a drone's altitude, a robot arm's joint) is second-order: the command sets acceleration, not velocity, so the system has inertia and naturally wants to overshoot and oscillate. This is where the full PID earns its keep. The widget below is a mass on a spring-damper that you push with a PID command. Tune all three gains and watch the response.

python
# PID controller, from scratch. Stateful: it remembers the integral and last error.
class PID:
    def __init__(self, Kp, Ki, Kd, dt):
        self.Kp, self.Ki, self.Kd, self.dt = Kp, Ki, Kd, dt
        self.integral = 0.0          # running sum of error (the "past")
        self.prev_e   = 0.0          # last error, to estimate de/dt (the "future")

    def step(self, x, x_des):
        e = x_des - x                       # present error
        self.integral += e * self.dt        # accumulate the past
        derivative = (e - self.prev_e) / self.dt   # rate of change
        self.prev_e = e
        return self.Kp*e + self.Ki*self.integral + self.Kd*derivative

Notice the PID is stateful — unlike the pure P-controller it carries memory (integral) and history (prev_e) between calls. That memory is exactly the "past" and "future" the P-controller lacked.

Chapter 4: Tracking with a Unicycle — The Car Can't Slide Sideways

So far our robots could move freely in the direction we commanded. A real wheeled robot cannot. Recall the no-slip constraint from Lesson 1: a rolling wheel cannot translate along its axle. A car can drive forward and turn, but it physically cannot slide sideways. This is the nonholonomic constraint, and it breaks naive position feedback in a way worth understanding deeply.

The simplest model is the unicycle (Stanford's Eq. 1 in the homework). Its state is the pose (x, y, θ), and its two controls are forward speed V and turn rate ω:

Read it: the robot moves at speed V in the direction it faces (hence the cosθ, sinθ), and it rotates at rate ω. There is no input that produces sideways motion. The robot only has two control knobs but lives in a three-dimensional pose space — that mismatch is the whole difficulty.

Why naive (x, y) feedback fails

Suppose the robot is at the origin facing east (θ = 0), and the target is one meter directly to its left (north). Naive position feedback says: "the y-error is +1, so command motion in +y." But the robot cannot move in +y — with θ = 0 it can only move east or rotate. Worse, if you crudely try to map a "move north" command onto V and ω, the math can blow up: the controller asks for the impossible and the gains misbehave. A wheeled robot fundamentally cannot be smoothly stabilized to an arbitrary pose by a simple time-invariant feedback law — this is Brockett's theorem, and it is why parking a car requires that back-and-forth shuffle rather than a single smooth slide.

Trick 1: feedback linearization on the dynamically-extended unicycle

Stanford's elegant fix (and the one your homework Problem 3 implements) starts by giving the robot a little inertia — the dynamically extended unicycle, where you command acceleration a and turn rate ω instead of speed directly:

The magic: this system is differentially flat with flat outputs (x, y) — meaning every state and control can be recovered algebraically from x, y and their derivatives (the heart of Lesson 5). Differentiate the position twice and you find that the accelerations of x and y are a clean linear function of the real controls (a, ω):

Now define the bracketed accelerations as virtual controls (w₁, w₂) = (ẍ, ÿ). In these virtual coordinates the robot looks like two independent double-integrators — just like the second-order mass from Chapter 3! So we can use a plain PD-style law per axis (this is Stanford's exact tracking law, Eq. 5):

Each line is feedforward (the desired acceleration ẍ_d) plus PD feedback on position and velocity error — exactly the feedforward-plus-feedback split from Chapter 1. Then we invert J to recover the real commands the robot can actually execute:

Since det(J) = V, this inversion works as long as V ≠ 0 (the robot is moving). The non-linearities are exactly cancelled by the J⁻¹ — that is what "feedback linearization" means. We turned a hard nonlinear nonholonomic tracking problem into two trivial linear ones.

Inverting J, with numbers

Suppose the robot is moving at V = 0.5 with heading θ = 60° (cos = 0.5, sin = 0.866), and the PD laws above ask for virtual accelerations w₁ = 0.3 (push x) and w₂ = −0.1 (ease y). Then J = [[0.5, −0.433], [0.866, 0.25]] (using −V sinθ = −0.5·0.866 = −0.433 and V cosθ = 0.25). Its determinant is det(J) = V = 0.5, so:

Multiply through to recover the real commands [a; ω] = J⁻¹[w₁; w₂]: the acceleration a = 0.5(0.3) + 0.866(−0.1) = 0.150 − 0.087 = 0.063, and the turn rate ω = −1.732(0.3) + 1.0(−0.1) = −0.520 − 0.1 = −0.62. So the controller, asked to "accelerate a bit in x and ease off in y," translates that into "speed up slightly (a = 0.06) and turn right at 0.62 rad/s" — commands the wheeled robot can actually obey. The J⁻¹ did the translation from "where I wish I could go" to "what my wheels can do."

Trick 2: pose stabilization in polar coordinates

For the final parking phase — driving to a specific pose and stopping there — Stanford uses a change of coordinates to polar form (homework Problem 2). Measure the robot relative to the goal with three numbers: ρ (distance to goal), α (heading error toward the goal), and δ (the goal-line's angle). The Lyapunov-derived control law that drives all three to zero is:

You don't need to memorize this. The point is the shape: speed V is proportional to distance ρ (drive fast when far, slow when close), and turn rate ω corrects the heading errors α and δ. With k₁, k₂, k₃ > 0, a Lyapunov "energy decreases" argument proves the robot is driven to the goal pose (0,0,0).

Why does Stanford bother with a Lyapunov proof instead of just eigenvalues? Because the system is nonlinear — eigenvalues only certify local stability of a linearization. A Lyapunov function V(ρ, α, δ) is an "energy" of the system: it's positive everywhere except at the goal (where it's zero), and if you can show it strictly decreases along every trajectory (V̇ < 0), then the system must slide down to the goal from anywhere — global convergence, no linearization. Stanford's choice V = ½ρ² + ½(α² + k₃δ²) plugged into the dynamics gives V̇ = −k₁ρ²cos²α − k₂α², which is strictly negative whenever k₁, k₂ > 0. That negativity is the proof: the energy can only go down, so the robot can only approach the goal.

One step of the polar law, by hand

Robot at origin facing east (θ = 0), goal one unit north at (0, 1). Then ρ = √(0²+1²) = 1, the angle to the goal is atan2(1,0) = 90°, so the heading error α = 90° − 0 = 90° (π/2). With gains k₁ = 0.7, k₂ = 1.2 and ignoring δ for the first instant: V = 0.7·1·cos(90°) = 0.7·1·0 = 0, and ω ≈ k₂·α = 1.2·(π/2) ≈ 1.88 rad/s. Read it: because the goal is straight to the robot's side (α = 90°, cosα = 0), it commands zero forward speed and pure rotation — it turns toward the goal first, then drives. The robot never tries to slide sideways; the control law respects the nonholonomic constraint automatically. As it rotates, α shrinks, cosα grows, and V ramps up — the curving approach you'll see in the widget.

The widget lets you watch a unicycle park itself this way.

python
# Differential-flatness trajectory tracker for the extended unicycle.
# This is exactly Stanford's HW1 Problem 3, from scratch.
import numpy as np

class TrajectoryTracker:
    def __init__(self, kpx, kpy, kdx, kdy):
        self.kpx, self.kpy, self.kdx, self.kdy = kpx, kpy, kdx, kdy
        self.V_prev = 0.5          # last commanded speed; avoids the J singularity
        self.V_MIN  = 0.1          # floor so det(J)=V never hits zero

    def compute_control(self, x, y, th, xd, yd, xdd_d, ydd_d, xd_dot, yd_dot, dt):
        # 1) virtual controls: feedforward accel + PD on position & velocity error
        V = max(self.V_prev, self.V_MIN)
        xdot, ydot = V*np.cos(th), V*np.sin(th)        # current velocity from kinematics
        w1 = xdd_d + self.kpx*(xd - x) + self.kdx*(xd_dot - xdot)
        w2 = ydd_d + self.kpy*(yd - y) + self.kdy*(yd_dot - ydot)
        # 2) invert J to map (w1, w2) -> real controls (a, om)
        J = np.array([[np.cos(th), -V*np.sin(th)],
                      [np.sin(th),  V*np.cos(th)]])
        a, om = np.linalg.solve(J, np.array([w1, w2]))  # [a; om] = J^-1 [w1; w2]
        # 3) integrate the commanded acceleration into a speed command
        V_new = V + a*dt
        self.V_prev = V_new
        return V_new, om                            # the actuator commands the robot can obey

Trace the three steps against the math above: build the virtual accelerations (feedforward + PD), invert J to translate them into wheel-and-steering language, then turn the commanded acceleration into a speed. The V_prev floor is the singularity guard from the callout — without it, a noise-induced V near zero would make np.linalg.solve blow up.

Chapter 5: The State-Space View — Stability Lives in the Eigenvalues

We have been tuning gains by feel. To tune them by theory — and to set up LQR in Chapter 6 — we need one unifying language: the state-space model. Almost every linear control system is written:

where x is the state vector (stacked: position, velocity, …), u is the control vector, A (the system matrix) describes how the state evolves on its own, and B (the input matrix) describes how the control pushes the state. Everything — a mass, a drone, a linearized robot — fits this mold.

Building A and B for the spring-mass

Take the second-order mass from Chapter 3. Its position is p and velocity is v; the command u is a force (acceleration, for unit mass). The physics: position changes at the velocity rate (ṗ = v), and velocity changes at the commanded acceleration (v̇ = u). Stack the state as x = [p ; v]. Then:

So A = [[0,1],[0,0]] (the "double integrator") and B = [0;1]. Read row by row: row 1 says ṗ = 0·p + 1·v = v. Row 2 says v̇ = 0·p + 0·v + 1·u = u. The matrix is just the physics, organized.

Feedback = changing the system matrix

Now apply a linear state-feedback law: command a control that is a (negative) linear combination of the states, u = −K x, where K = [k₁, k₂] is the gain matrix (k₁ multiplies position error, k₂ multiplies velocity — exactly P and D!). Substitute into ẋ = A x + B u:

This is the key move. Feedback transforms the open-loop matrix A into the closed-loop matrix (A − BK). The robot now evolves according to a new matrix that you designed by choosing K. Control design is literally reshaping (A − BK) into something well-behaved.

Stability = where the eigenvalues sit

When does ẋ = M x decay to zero (stable) versus blow up (unstable)? The answer is one of the cleanest facts in engineering: it depends entirely on the eigenvalues of M. An eigenvalue λ is a number for which there's a direction the system shrinks or grows along like e^λt. So:

• If all eigenvalues have negative real part, every mode decays like e^−|λ|t — the system is stable, the state goes to zero.
• If any eigenvalue has positive real part, that mode grows — unstable, runaway.
• Imaginary parts produce oscillation; the more negative the real part, the faster the decay.

This is the rigorous version of Chapter 1's "ė = −k·e is stable when k > 0" — there, M = −k was a 1×1 matrix whose single eigenvalue is −k, negative exactly when k > 0. Same idea, now in any dimension.

Worked example: pole placement by hand

Take the double integrator A = [[0,1],[0,0]], B = [0;1], gain K = [k₁, k₂]. Then BK = [[0,0],[k₁,k₂]], so:

The eigenvalues λ solve det(A−BK − λI) = 0. Compute the determinant of [[−λ, 1], [−k₁, −k₂−λ]]:

That is a quadratic in λ whose coefficients are exactly your gains: the constant term is k₁ (the P gain) and the λ coefficient is k₂ (the D gain). Suppose you want both eigenvalues at −2 (a nice fast, non-oscillatory decay). The polynomial with both roots at −2 is (λ+2)² = λ² + 4λ + 4. Match coefficients:

We just chose where the eigenvalues live and read off the gains that put them there. That is pole placement (eigenvalues of the system are called "poles"). It is exact and principled — no slider-wiggling. Its weakness: where should the poles go? Pole placement makes you pick by hand, and for high-dimensional systems there's no obvious "best" location. That question — where to put the poles to balance speed against control effort, optimally — is precisely what LQR answers automatically. On to Chapter 6.

A second worked placement, end to end

Suppose instead you want a faster, slightly oscillatory response: both eigenvalues at −3 ± 3i (real part −3 for quick decay, imaginary part 3 for a little overshoot). The polynomial with those roots is (λ−(−3+3i))(λ−(−3−3i)) = (λ+3)² + 3² = λ² + 6λ + 18. Match it to λ² + k₂λ + k₁:

Both gains are bigger than the −2,−2 case (4 and 4) — faster poles cost more gain, which means more control effort. There's the trade again, now visible in the gains themselves. Drag the widget's k₁, k₂ to 18 and 6 and confirm the eigenvalue markers sit at −3 ± 3i on the complex plane, with a response that overshoots once then settles.

The same idea in discrete time

Because robots run discrete loops, it helps to see stability in discrete form too. A discrete system steps as x_t+1 = M x_t, and it is stable when every eigenvalue of M has magnitude less than 1 (inside the unit circle) — not "negative real part." That's the discrete analogue: a mode multiplies by λ each tick, so |λ| < 1 shrinks it and |λ| > 1 grows it.

Tie it back to Chapter 2's P-controller. There the discrete error obeyed e_t+1 = (1 − Δt·K_p)·e_t — a 1×1 system with the single eigenvalue λ = 1 − Δt·K_p. Stability needs |1 − Δt·K_p| < 1, which rearranges to 0 < Δt·K_p < 2. That is exactly the stability bound we'll re-derive in Chapter 9 — and now you can see it's just "keep the eigenvalue inside the unit circle." Continuous left-half-plane and discrete unit-circle are the same stability story told in two clocks.

Can we always place the poles? Controllability

Pole placement (and LQR) only works if the control input can actually reach every mode of the system — a property called controllability. If some part of the state evolves on its own and no input can touch it, no gain matrix will tame it. For our double integrator, the input drives velocity, which drives position, so both states are reachable: the system is controllable, and we can place the poles anywhere. A formal check (the controllability matrix [B, AB] being full rank) confirms it, but the intuition is enough here: feedback can only fix what the input can influence.

python
# Pole placement with a library, matching our hand result for the double integrator.
import numpy as np
from scipy.signal import place_poles
A = np.array([[0., 1.], [0., 0.]])
B = np.array([[0.], [1.]])
desired = [-2, -2.001]                       # both ~ -2 (solver dislikes exact repeats)
K = place_poles(A, B, desired).gain_matrix
print(K)                                  # ~ [[4, 4]]  -> k1=4 (P), k2=4 (D), as derived by hand
# closed-loop matrix and its eigenvalues:
print(np.linalg.eigvals(A - B @ K))       # ~ [-2, -2]

Chapter 6: LQR — The Optimal Gain, Computed for You

Pole placement asks you to pick where the eigenvalues go. But that just trades one tuning problem (gains) for another (pole locations). What you actually want is to say what you care about — "track well, don't waste energy" — and have the math hand you the best gain. That is the Linear Quadratic Regulator (LQR), the workhorse optimal controller for linear systems.

LQR is defined by an objective: among all possible control histories, find the one that minimizes a quadratic cost integrated over all time:

Decode each piece carefully — this cost is the controller's value system:

• x^TQ x — the state cost. Q is a positive matrix that penalizes being away from zero (being off the trajectory). Big Q = "I really hate tracking error, fix it fast."
• u^TR u — the control cost. R is a positive matrix that penalizes large commands. Big R = "actuators are expensive/fragile/slow, go easy on them."
• The integral sums these over all future time, so LQR optimizes the whole trajectory, not just the next instant. It balances getting back on track quickly against not thrashing the motors.

The optimal controller is still linear feedback

The remarkable result: the control law that minimizes that infinite integral is just linear state feedback — the same u = −Kx form from Chapter 5, but now with the optimal K:

where P is a special matrix found by solving the algebraic Riccati equation — the central equation of optimal control:

That equation looks intimidating, so let's strip it to one dimension and solve it completely by hand to see there's nothing magic inside.

Scalar LQR, solved by hand

Take the simplest system: scalar state x, scalar control u, dynamics ẋ = u (so a = 0, b = 1). Costs q and r are just positive numbers. The Riccati equation, with A = 0, B = 1, becomes:

Solve for p. We need p > 0 (P must be positive), so:

Then the optimal gain is k = r⁻¹·b·p = (1/r)·1·√(qr) = √(qr)/r = √(q/r):

Look how beautifully the Q/R trade-off falls out. Let's plug in numbers. If q = 1, r = 1 (care equally), then k = √1 = 1. The closed-loop dynamics become ẋ = −kx = −x, decaying like e^−t — modest. Now make tracking matter 100× more: q = 100, r = 1, then k = √100 = 10 — ten times more aggressive, dynamics e^−10t, snapping back fast. Conversely make control expensive, q = 1, r = 100, then k = √(1/100) = 0.1 — lazy, e^−0.1t, gentle.

And the closed-loop behavior follows immediately. The optimal control closes the loop as ẋ = u = −kx = −√(q/r)·x, whose solution is x(t) = x(0)·e^{−√(q/r)·t}. The time constant is τ = 1/k = √(r/q) — the very same exponential-decay machinery from Chapter 1, but now the decay rate is derived from your cost preferences rather than guessed. Want the state at 5% of its initial value within 1 second? You need 3τ ≤ 1, so τ ≤ 0.33, so k ≥ 3, so q/r ≥ 9. You can dial in a settling-time spec and read off the required cost ratio. That is engineering, not knob-twiddling.

A 2×2 sanity check (double integrator)

For the double integrator A = [[0,1],[0,0]], B = [0;1], with Q = diag(q, 0) (penalize position error only) and R = r, the algebra (which we'll spare you) yields the optimal gain K = [√(q/r), √(2)·(q/r)^1/4]. With q = 1, r = 1 that's K ≈ [1, 1.41], placing both poles at −0.71 ± 0.71i — a critically-damped-ish, no-overshoot response that pole-placement would have made you guess at. LQR found the "nice" poles for you, from a cost you actually understood. Notice the structure: the position gain is √(q/r) (the same P-like dial as the scalar case) and the velocity gain is √2·(q/r)^1/4 (a D-like damping term). LQR is, in the end, computing a principled P and D gain for you.

Discrete-time LQR: the version that runs on a robot

Real controllers are discrete (Chapter 9), and there's a discrete cousin of everything above. The dynamics are x_t+1 = A_dx_t + B_du_t, the cost is a sum ∑(x_t^TQx_t + u_t^TRu_t), and the optimal gain comes from the discrete algebraic Riccati equation. The conceptual payoff is the finite-horizon version, which you can compute by a simple backward recursion — and it reveals where LQR really comes from: dynamic programming working backward from the end.

Take a tiny scalar example to feel it. System x_t+1 = x_t + u_t (A_d = 1, B_d = 1), costs q = 1, r = 1, horizon of just 2 steps with a terminal penalty P₂ = q = 1. The backward Riccati recursion P_t = q + A_d²P_t+1 − (A_dB_dP_t+1)²/(r + B_d²P_t+1) unrolls:

The gain is different at each step — 0.60 then 0.50 — bigger early (lots of horizon left to benefit from correcting) and smaller near the end (no point spending control if the episode is ending). As the horizon grows long, P_t stops changing and settles to a fixed value (here ≈ 1.618, the golden ratio!), and the gain settles to a constant — that steady value is the infinite-horizon LQR gain. So the infinite-horizon controller we use day-to-day is just the limit of this backward recursion. The algebraic Riccati equation is the "I've converged" condition: P_t = P_t+1.

Designing Q and R for a real robot

For a multi-state robot, Q and R are matrices, and their diagonal entries are how you express "I care about this state/control more than that one." A clean recipe is Bryson's rule: set each diagonal entry to 1 over the square of the largest value you'll tolerate for that variable.

Say your unicycle's pose error matters: you'll tolerate at most 0.2 m of position error and 0.1 rad of heading error, and at most 0.5 m/s of speed command and 1 rad/s of turn. Then Q = diag(1/0.2², 1/0.2², 1/0.1²) = diag(25, 25, 100) — heading errors are penalized 4× harder because the tolerance is tighter. And R = diag(1/0.5², 1/1²) = diag(4, 1). Each entry now has a physical meaning, so tuning becomes "what error/effort can I live with?" instead of "what number feels right?"

python
# Scalar LQR solved by hand vs. the library, to prove they agree.
import numpy as np
from scipy.linalg import solve_continuous_are   # the Riccati solver

# by hand: x_dot = u, so A=0, B=1; cost weights q, r
q, r = 100.0, 1.0
k_hand = np.sqrt(q / r)                  # = 10.0, from p = sqrt(qr), k = p/r

# by library: solve A^T P + P A - P B R^-1 B^T P + Q = 0
A, B, Q, R = np.array([[0.]]), np.array([[1.]]), np.array([[q]]), np.array([[r]])
P = solve_continuous_are(A, B, Q, R)
K = np.linalg.inv(R) @ B.T @ P           # K = R^-1 B^T P
print(k_hand, K[0,0])               # 10.0  10.0  -- identical

Chapter 7: Gain-Scheduled LQR — One Gain Can't Fit a Curve

LQR is built on a linear model ẋ = Ax + Bu. But a robot tracking a curving trajectory is not linear — its dynamics depend on its heading θ, which changes constantly along the path. A single LQR gain, computed once, will track well on the parts of the path that look like the model used to compute it, and poorly elsewhere. The fix is gain scheduling: linearize the dynamics about each point of the trajectory and recompute the LQR gain there.

Linearizing about the trajectory

The robot's true dynamics are nonlinear, ẋ = f(x, u). At any point along the reference (x_d(t), u_d(t)), we approximate f by its tangent — a first-order Taylor expansion in the error coordinates δx = x − x_d, δu = u − u_d:

Here A(t) and B(t) are the Jacobians of f — the matrices of partial derivatives ∂f/∂x and ∂f/∂u — evaluated at the reference point at time t. Because the reference moves and curves, these Jacobians change along the trajectory. A sharp turn has a different A than a straight stretch.

The unicycle Jacobian, worked in numbers

Let's make "B depends on heading" concrete. The unicycle is ẋ = V cosθ, ẏ = V sinθ, θ̇ = ω, with controls (V, ω). The input matrix B is ∂f/∂u — how the two controls push the three states. Differentiating: V appears with cosθ in ẋ and sinθ in ẏ; ω appears alone in θ̇. So:

Now plug in two headings the robot passes through on a curving path. Heading east (θ = 0): cos0 = 1, sin0 = 0, so B = [[1,0],[0,0],[0,1]] — a forward command V moves x only. Heading north (θ = 90°): cos = 0, sin = 1, so B = [[0,0],[1,0],[0,1]] — the same forward command V now moves y only. The columns of B literally rotate with the robot.

A gain K computed assuming "V pushes x" (east) will, when the robot has turned north, push the wrong axis — it corrects an x-error by commanding forward motion that actually moves the robot in y. On a gentle curve the mismatch is small; on a hairpin it's catastrophic. Recomputing B(θ) and re-solving LQR at, say, every 15° of heading keeps the model honest the whole way around. That table of gains, indexed by heading, is the gain schedule.

python
# Build a gain schedule along a trajectory: one LQR gain per sampled point.
import numpy as np
from scipy.linalg import solve_continuous_are

def unicycle_B(theta, V):
    return np.array([[np.cos(theta), 0],
                     [np.sin(theta), 0],
                     [0,             1]])

A = np.zeros((3, 3))            # pose drifts only via the inputs here
Q = np.diag([2., 2., 1.]); R = np.eye(2)
schedule = []
for theta in np.linspace(0, 2*np.pi, 24):     # sample 24 headings around a loop
    B = unicycle_B(theta, V=0.4)
    P = solve_continuous_are(A, B, Q, R)
    K = np.linalg.inv(R) @ B.T @ P            # the LQR gain for THIS heading
    schedule.append((theta, K))
# at runtime: pick the K for the heading nearest the robot's current pose.

The recipe

At runtime the controller applies u = u_d(t) − K(t)·(x − x_d(t)) — the feedforward from the plan, plus the locally-optimal feedback for wherever the robot currently is on the path. As the robot moves along the curve, the active gain K(t) is swapped for the one computed at that segment. This is exactly how aircraft autopilots have worked for decades: a table of LQR gains indexed by flight condition (airspeed, altitude), interpolated in real time.

The continuous limit: the finite-horizon Riccati equation

Push gain scheduling to its limit — recompute the gain at every instant — and you get the time-varying LQR, where K(t) comes from a Riccati equation that you integrate backward in time from the end of the trajectory. This is the bridge to optimal trajectory tracking and, ultimately, to model predictive control (Chapter 9). For most robots, a modest schedule of a dozen gains around the path is plenty — you don't need the full continuum.

Why backward in time? Because the optimal action now depends on what you intend to do later — the cost-to-go from the end. You met exactly this in the discrete recursion of Chapter 6, where the gain was bigger early (lots of horizon left) and smaller near the end. Time-varying LQR is that recursion run on the linearized-along-the-trajectory model: at each instant t it has its own A(t), B(t), its own P(t), and its own K(t). Gain scheduling is the practical, sampled cousin: instead of the full continuum of K(t), you precompute a handful and interpolate.

Chapter 8: Showcase — The Live Trajectory Tracker

Everything comes together here. A robot must follow a reference path (the grey loop) while disturbances try to shove it off. You choose the controller — pure P, full PID, or LQR — tune its gains, and push the robot with the disturbance button to see how each recovers. The bottom strip plots the tracking error over time so you can compare them quantitatively.

Play with it until you feel each controller's personality. Set it to P, run, and push: it lurches back but settles a little off the line, and on the curved sections it permanently lags inside the turn — that's the steady-state error and phase lag you proved in Chapter 2. Switch to PID: the offset vanishes (integral) and the recovery from a push is smooth, not bouncy (derivative). Switch to LQR and slide q up: the robot hugs the reference like it's magnetized, and the error plot snaps back to zero after each push with minimal overshoot — the optimal gain at work.

Now break it. With any controller, drag K_p (or q) toward the maximum. The recovery from a push first gets faster, then starts overshooting, then oscillating around the path, and finally the error plot diverges — the robot spirals off and the loop is unstable. You are watching the closed-loop eigenvalues from Chapter 5 march from deep in the left half-plane, toward the imaginary axis (oscillation), and across it (blow-up). High gain is not free.

What to look for, controller by controller

If you want a checklist while you play, here is the behavior each controller should exhibit — and the chapter that explains why:

The single most instructive experiment: set P, run, and stare at the curved sections of the loop. The robot consistently cuts the corner — it lags inside every turn. That lag is the moving-reference version of steady-state error: the reference keeps moving, the P-controller always trails it by a fixed amount because it needs a standing error to generate the command that keeps up. Switch to PID and the corners snap tight: the integral term builds exactly the standing command the turn needs, so the error drops to zero. You are watching, live, the same effect the downhill-robot table proved with numbers in Chapter 2.

The whole tracker, in code

Here is the loop the showcase runs, distilled. It is the feedforward-plus-feedback structure from Chapter 1, with the feedback term swappable between P, PID, and LQR — exactly the toggle in the widget. This is the Control module of the autonomy stack, in about thirty lines.

python
# The trajectory-tracking loop: feedforward + pluggable feedback, with disturbance.
import numpy as np

def track(controller, ref_fn, x0, T, dt, disturbance):
    x = np.array(x0, dtype=float)        # [px, py]
    errors = []
    for step in range(int(T/dt)):
        t = step*dt
        xd, vd = ref_fn(t)                # desired position and its velocity (feedforward)
        e = xd - x                          # tracking error (Chapter 1)
        u_fb = controller.feedback(e, dt)  # P / PID / LQR all share this interface
        u = vd + u_fb                       # feedforward + feedback
        x = x + dt*u + disturbance(t)       # integrate; the world pushes back
        errors.append(np.linalg.norm(e))
    return errors

class P:
    def __init__(self, kp): self.kp = kp
    def feedback(self, e, dt): return self.kp*e

class PID:
    def __init__(self, kp, ki, kd):
        self.kp, self.ki, self.kd = kp, ki, kd
        self.I = np.zeros(2); self.prev = np.zeros(2)
    def feedback(self, e, dt):
        self.I += e*dt; d = (e - self.prev)/dt; self.prev = e
        return self.kp*e + self.ki*self.I + self.kd*d

class LQR:                                  # scalar-per-axis gain k = sqrt(q/r)
    def __init__(self, q, r): self.k = np.sqrt(q/r)
    def feedback(self, e, dt): return self.k*e

Notice all three controllers expose the same feedback(e, dt) interface — the tracker doesn't care which one it holds. That's the engineering payoff of thinking in terms of error: the loop is universal; only the feedback law changes. Swap P(3) for PID(3, 1, 2) for LQR(9, 1) and you change the robot's personality without touching the loop.

Chapter 9: From Theory to Hardware — Discretization, Saturation, MPC

The controllers above were written in continuous time, with infinitely precise sensors and infinitely strong motors. Real robots violate all three assumptions. Three practical realities separate a textbook controller from one that survives on hardware.

1. Discretization — the loop runs at a finite rate

A digital controller doesn't run continuously; it runs in a loop at some fixed sample rate (say 50 Hz, so Δt = 0.02 s). Between samples, it holds the last command constant (a "zero-order hold"). Every integral and derivative becomes a discrete sum and difference, exactly as in our worked tables:

Watching discretization bite, with numbers

Recall the P-controller table from Chapter 2: x_t+1 = x_t + Δt·K_p·(x_d − x_t). The discrete error obeys e_t+1 = (1 − Δt·K_p)·e_t. For this to decay, the multiplier must satisfy |1 − Δt·K_p| < 1, i.e. 0 < Δt·K_p < 2. Watch the three regimes with K_p = 1:

The exact same gain K_p = 1 that is unconditionally stable in continuous time (ė = −e always decays) goes unstable the moment the timestep Δt exceeds 2 — the discretization, not the gain, broke it. This is the single most common surprise when a controller that worked in simulation misbehaves on a robot whose loop runs slower than the simulator. Faster sampling (smaller Δt) shrinks Δt·K_p back into the stable zone.

2. Actuator saturation and anti-windup

No motor is infinitely strong. The unicycle homework caps |V| ≤ 0.5 m/s and |ω| ≤ 1 rad/s. When the controller commands more than the actuator can deliver, the command saturates — clipped to the limit. Two consequences:

• The robot responds more slowly than the controller "thinks," because part of the commanded effort is being thrown away at the clip.
• Worse, the integral keeps winding up (Chapter 3): the error stays large because the saturated actuator can't fix it fast enough, so the integral term balloons. When the error finally clears, that huge stored integral causes a violent overshoot.

The fix is anti-windup: stop accumulating the integral whenever the actuator is saturated (or "back-calculate" the integral from the clipped output). One clean version: only add to the integral if the unsaturated command is within limits.

python
# Saturation + anti-windup, the version that ships on real robots.
def step(self, x, x_des, u_max):
    e = x_des - x
    deriv = (e - self.prev_e) / self.dt; self.prev_e = e
    u_unsat = self.Kp*e + self.Ki*self.integral + self.Kd*deriv
    u = max(-u_max, min(u_max, u_unsat))     # clip to actuator limit
    if u == u_unsat:                              # NOT saturated -> safe to integrate
        self.integral += e * self.dt
    # if saturated, freeze the integral: no windup
    return u

3. Beyond PID/LQR: model predictive control (MPC)

PID and (gain-scheduled) LQR are reactive: they respond to the current error with a fixed feedback law and have no built-in way to respect hard constraints like "don't exceed 0.5 m/s" or "don't hit that wall ahead." Model Predictive Control is the next step: at every tick it solves a small optimization — "given my model, find the command sequence over the next N steps that minimizes cost while obeying all constraints" — executes only the first command, then re-solves next tick.

The shape of one MPC tick, in pseudocode, makes the difference from LQR crisp:

python
# One MPC tick: solve a constrained optimization, apply only the first command.
def mpc_step(x_now, ref, N, dt):
    # decide the next N commands u[0..N-1] all at once...
    u = solve_optimization(
        objective = lambda u: sum(cost(x_k, u_k) for x_k, u_k in rollout(x_now, u, dt)),
        constraints = [
            "|V_k| <= 0.5  for every k",        # hard actuator limit -- LQR can't say this
            "x_k not in obstacle  for every k",  # hard safety limit -- LQR can't say this
        ],
    )
    return u[0]   # apply ONLY the first command, then re-solve next tick ("receding horizon")

LQR's gain is computed once, offline, and ignores constraints — it just trusts that the unconstrained optimum stays feasible. MPC re-solves every tick with the constraints in the problem, so it can refuse to command 0.6 m/s when the limit is 0.5, and can plan a curve around an obstacle it sees ahead. The price is solving an optimization at control rate, which is why MPC needs more compute than the matrix-multiply that LQR's u = −Kx boils down to.

The arc of this lesson is the arc of control engineering itself: P (react to now) → PID (add memory and anticipation) → pole placement (design the eigenvalues) → LQR (optimize the gain) → MPC (optimize over a horizon with constraints). Each step buys more capability for more computation. Pick the simplest one that meets your spec — a Roomba is happy with P; a self-driving car needs MPC.

Chapter 10: The Controller's Toolbox — Connections & Cheat-Sheet

You now hold the Control module of the autonomy stack. It is the layer that takes Lesson 5's beautiful-but-fragile open-loop trajectory and makes it survive the real world by closing the loop — the very thing that, way back in Lesson 1, separated a Waymo from a welding arm.

When to use which controller

Cheat-sheet

Where this sits on the stack

Control is the green layer — the last one before the actuators. It receives the planner's trajectory and the localizer's pose estimate, and emits the motor commands that actually move the robot. Look back at the showcase: every push you injected was a disturbance, and every recovery was feedback doing the job the welding arm could never do.

Related lessons on Engineermaxxing

• Lesson 5: Trajectory Optimization — where the reference x_d(t), u_d(t) and the differential-flatness structure this lesson tracks come from.
• Lesson 2: Rigid-Body Transforms SE(2)/SE(3) — the pose representation (x, y, θ) and the kinematics the unicycle controller acts on.
• Lesson 1: The See-Think-Act Loop — the open-vs-closed-loop idea that motivates this entire lesson, and where Control sits on the stack.
• Visual Navigation: Control — a complementary take on PID and LQR from the aerial-robotics side.
• Markov Decision Processes — when control becomes a sequential decision problem and LQR generalizes to dynamic programming.