Control: PID, LQR & Geometric Control

Chapter 1: State-Space & Stability

Before designing controllers, we need a precise language for describing how systems evolve. The language is state-space form. Rather than writing out Newton's equations from scratch each time, we pack everything into two compact equations:

Here x ∈ ℝⁿ is the state vector — everything you need to predict the future. u ∈ ℝ^m is the control input — the actuator commands. y is the output you observe. The dot notation ẋ means dx/dt — the time derivative of the state.

Quadrotor state vector

For a quadrotor, the state is: position p ∈ ℝ³, velocity v ∈ ℝ³, rotation R ∈ SO(3), angular velocity ω ∈ ℝ³. We can list this as x = [p, v, R, ω]. The full state has 12 real-valued degrees of freedom (3+3+3+3, since SO(3) is a 3-DOF manifold even though R is a 3×3 matrix). The control inputs are the four rotor speeds w = [w₁, w₂, w₃, w₄].

Linear state-space: ẋ = Ax + Bu

When f is linear in x and u, the dynamics simplify to:

where A ∈ ℝ^n×n is the system matrix (captures how the state drives its own derivative) and B ∈ ℝ^n×m is the input matrix (captures how control inputs affect the derivative). This linear form is not always exact, but near an equilibrium point, any nonlinear system can be linearized to this form via a Taylor expansion.

Stability: eigenvalues decide everything

The long-term behavior of ẋ = Ax (with u=0) is entirely determined by the eigenvalues of A. Let λ be an eigenvalue. If Re(λ) < 0 for all eigenvalues, perturbations decay exponentially — the system is asymptotically stable. If any Re(λ) > 0, perturbations grow exponentially — the system is unstable.

A hovering quadrotor without control is unstable — its linearized A matrix has eigenvalues in the right half-plane. Every position perturbation grows. Control must place all closed-loop eigenvalues in the left half-plane.

Worked numbers: a simple 1D system

Consider a mass on a spring: ẍ = −kx/m − bẋ/m + u/m. In state-space form with x = [position, velocity]:

With m=1 kg, k=4 N/m, b=0: A = [[0,1],[−4,0]]. Eigenvalues: λ = ±2j (purely imaginary — undamped oscillation). Add b=4: eigenvalues λ = −2±0j (both real negative — overdamped, stable, no oscillation). Drag b is the key to stability here.

Chapter 2: Open vs Closed Loop

There are two fundamentally different ways to control a system. In open-loop control, you compute your commands from the desired trajectory alone, ignoring what the system actually does. In closed-loop (feedback) control, you continuously measure the actual state, compare it to the desired state, and compute commands from the error.

Why open-loop fails in robotics

Open-loop control works beautifully if your model is perfect and there are no disturbances. In a real robot, neither is true. Motors have friction that varies with temperature. Wind applies unknown forces. Sensor calibration drifts. An open-loop controller that was tuned on a calm day will fail on a windy day — it never "knows" it's drifting.

The feedback loop equation

For a linear system, the closed-loop dynamics look like this. Without control: ẋ = Ax. With a linear feedback law u = −Kx (we'll derive K later):

The closed-loop system matrix is A_cl = A − BK. The controller K reshapes the eigenvalues of A into whatever we want. An unstable A (right-half eigenvalues) can become a stable A_cl with the right K. This is the fundamental promise of feedback control: you can move the poles to stable locations by choosing K.

A concrete 1D example

Scalar system: ẋ = ax + bu. Without control (u=0), if a > 0, x(t) = x₀e^at → ∞. Now apply u = −kx. Then ẋ = ax − bkx = (a−bk)x. If we choose k > a/b, the exponent (a−bk) becomes negative and x decays to zero. Worked numbers: a=2, b=1. For stability we need k > 2. Take k=5: closed-loop pole = 2−5 = −3. Time constant τ = 1/3 ≈ 0.33 s. Starting at x₀=1, after 1 s: x = e⁻³ ≈ 0.05. The feedback gain k=5 stabilized an unstable system (a=+2) in under a second.

Chapter 3: PID: Derive Each Term

PID stands for Proportional-Integral-Derivative. Each term responds to a different aspect of the error signal e(t) = r(t) − y(t), where r is the reference (setpoint) and y is the measured output. The control law is:

Let's derive why each term is necessary, and what happens if you remove any one of them.

P term: stiffness (push proportional to displacement)

The proportional term u_P = K_P·e is the simplest feedback law. Think of it as a spring: the further you are from the setpoint, the harder you push back. It gives the system stiffness. Increasing K_P makes the response faster but also more prone to overshoot and oscillation. For the drone 0.5 m below setpoint, K_P=4 gives u_P = 4×0.5 = 2 N of upward thrust correction.

D term: damping (resist the velocity of change)

The derivative term u_D = K_D·ė = K_D·(de/dt) acts like a damper. It opposes the rate of change of the error. When the drone is falling toward the setpoint fast (error decreasing rapidly), ė < 0, so u_D < 0 — it brakes the approach. This prevents overshoot. Without D, a highly-tuned P gain causes oscillation; the D term is the "anti-oscillation" term. If the drone is 0.5 m below and approaching at 0.3 m/s: ė = −0.3 m/s (error decreasing). K_D=2 gives u_D = 2×(−0.3) = −0.6 N (braking).

I term: memory (eliminate steady-state error)

The integral term u_I = K_I·∫e(τ)dτ accumulates all past errors. If there is a constant disturbance (say, a steady 1 N wind), the P term alone settles at a steady-state error e_ss = disturbance/K_P. No matter how long you wait, this offset persists. The integral builds up over time until K_I·∫e·dt = 1 N, fully canceling the disturbance. The I term gives the controller infinite DC gain — it will keep pushing until the error is exactly zero.

Worked numbers: step response at t=0

Initial conditions: error e₀=1 m, ėe₀=0, ∫e₀=0. Gains: K_P=4, K_D=2, K_I=1.

At t=0.5 s (suppose error has dropped to 0.4 m, ė = −1.2 m/s, ∫e=0.3 m·s):

The D term has gone negative (braking) because the system is approaching the setpoint fast. Without D, u(0.5) = 1.9 N, still pushing forward — causing overshoot.

Chapter 4: PID Tuning & Anti-Windup

Having a PID controller and having a working PID controller are two different things. The gains K_P, K_I, K_D must be tuned. Too low: sluggish. Too high: oscillation or instability. The interaction between the three terms makes tuning non-trivial.

Tuning intuition: the Ziegler-Nichols mental model

Start with K_I=0, K_D=0. Increase K_P until the system oscillates continuously — this is the ultimate gain K_u at the ultimate period T_u. Ziegler-Nichols suggests K_P=0.6K_u, K_I=1.2K_u/T_u, K_D=0.075K_uT_u. This is a starting point, not a final answer — always retune on the real hardware.

Integrator windup

The I term has a dangerous failure mode called integrator windup. Suppose the drone is trying to lift off from the ground but is held down (saturated actuator). The error is large and constant: e = +2 m. The integral keeps accumulating: ∫e grows to 10, 100, 1000 m·s. When the constraint is released, the massive integral term causes the drone to shoot up far beyond the setpoint — severe overshoot or even a crash.

The fix is anti-windup: when the actuator saturates (command clips to max or min), stop integrating. Concretely: don't accumulate the integral when u is at its limit.

python
import numpy as np

class PIDController:
    def __init__(self, Kp, Ki, Kd, u_min, u_max, dt):
        self.Kp, self.Ki, self.Kd = Kp, Ki, Kd
        self.u_min, self.u_max = u_min, u_max
        self.dt = dt
        self.integral = 0.0
        self.prev_e   = 0.0

    def step(self, e):
        # Proportional
        u_p = self.Kp * e
        # Derivative (backward difference)
        u_d = self.Kd * (e - self.prev_e) / self.dt
        # Unclamped output (without I)
        u_raw = u_p + self.Kd * (e - self.prev_e) / self.dt

        # Anti-windup: only integrate when NOT saturated
        u_test = u_p + self.Ki * self.integral * self.dt + u_d
        if self.u_min < u_test < self.u_max:
            self.integral += e * self.dt   # accumulate

        u_i = self.Ki * self.integral
        u = np.clip(u_p + u_i + u_d, self.u_min, self.u_max)
        self.prev_e = e
        return u

Chapter 5: LQR: Optimal Control

PID works well when you can tune three numbers by hand. But a quadrotor has 12 state dimensions and 4 inputs. Tuning 12-dimensional feedback gains manually is impractical. Linear Quadratic Regulator (LQR) automates the gain design by solving an optimization problem: find the feedback law u = −Kx that minimizes a weighted cost over all future time.

The cost function

LQR minimizes the infinite-horizon quadratic cost:

where Q ∈ ℝ^n×n (positive semidefinite) penalizes state error and R ∈ ℝ^m×m (positive definite) penalizes control effort. The term x^TQx says "I care about states x_i being near zero, with weight Q_ii." The term u^TRu says "I care about keeping inputs small — don't waste actuator authority."

Q/R tradeoff intuition

The ratio Q/R is the fundamental design knob. Large Q/R: the optimizer is told "state errors are very expensive, control effort is cheap" — it pushes the state hard toward zero, using lots of control. Small Q/R: "control effort is expensive, state error is acceptable" — gentle corrections, slower convergence, smaller actuator commands. This is directly analogous to the PID gain: high K_P ↔ large Q, low K_D ↔ small R.

The optimal feedback law

The remarkable result of LQR: the optimal control is always a linear state feedback law:

Here P ∈ ℝ^n×n is the unique positive definite solution to the Algebraic Riccati Equation (ARE):

You do not need to solve the ARE by hand. In practice, scipy.linalg.solve_continuous_are(A, B, Q, R) computes P in milliseconds. The resulting K is the globally optimal linear feedback gain for the given Q and R.

Worked numbers: LQR cost evaluation for two K choices

Scalar system: A=0, B=1, Q=1, R=1. The ARE simplifies to: −P² + 1 = 0, so P=1, K=P=1. Optimal gain K=1. Compare two controllers from x₀=1:

The optimal K=1 achieves the minimum cost J=1.0. The over-gain (K=3) drives the state to zero faster but pays a high control cost (9u²), giving a higher total J. The under-gain settles slowly, paying a high state cost. LQR finds the sweet spot automatically.

python
import numpy as np
from scipy.linalg import solve_continuous_are

# System: quadrotor altitude axis (double integrator + drag)
# State x = [z, z_dot], input u = thrust deviation from hover
m = 1.0   # kg
b = 0.1   # drag coefficient
A = np.array([[0, 1],
              [0, -b/m]])
B = np.array([[0],
              [1/m]])

# LQR weights: penalize altitude error 10x more than thrust effort
Q = np.diag([10.0, 1.0])   # [z, z_dot] weights
R = np.array([[1.0]])         # thrust cost

# Solve Riccati equation for P
P = solve_continuous_are(A, B, Q, R)
# Optimal gain: K = R^{-1} B^T P
K = np.linalg.inv(R) @ B.T @ P

# K = [Kz, Kzdot] — feedback on altitude error and velocity
# Control law: u = -K @ (x - x_desired)
print(f"K = {K.flatten()}")  # e.g., K = [3.16, 4.63]

# LQR cost evaluator: simulate and compute J
def lqr_cost(K, A, B, Q, R, x0, T=5.0, dt=0.01):
    x = x0.copy(); J = 0.0
    for _ in np.arange(0, T, dt):
        u = -K @ x
        J += (x @ Q @ x + u @ R @ u) * dt
        x = x + (A @ x + B.flatten() * u[0]) * dt
    return J

Chapter 6: Quadrotor Dynamics

Everything so far has been generic control theory. Now we apply it to the quadrotor — the canonical VNAV platform. The quadrotor's dynamics are richer and stranger than a simple mass-spring because it is underactuated: four scalar inputs (rotor speeds) must control six degrees of freedom.

The Newton-Euler equations

The quadrotor obeys Newton's second law in both translation and rotation. In compact form:

The translational equation says: mass × acceleration = gravity (downward) + thrust force rotated to world frame. The rotational equation says: inertia × angular acceleration = Euler term (gyroscopic coupling) + applied torques. Here R^w_B is the rotation from body to world frame, f^B is force in body frame (only the z component is nonzero for a standard quadrotor), τ^B is torque in body frame, and J is the 3×3 inertia matrix.

Rotor physics: thrust and torque from spin speed

Each rotor i spinning at angular velocity w_i produces:

The total thrust is the sum of all four: f^B_z = c_f(w₁|w₁| + w₂|w₂| + w₃|w₃| + w₄|w₄|). The torques come from both the drag and from the geometry: off-center thrust at arm position ρ^B_i creates a moment ρ^B_i × T_ie₃.

The mixing matrix: rotor speeds to thrust/torque

We can pack the input mapping into a 4×4 matrix F̄ (the "mixing matrix") that maps the signed-square rotor speeds w = [w₁|w₁|, w₂|w₂|, w₃|w₃|, w₄|w₄|]^T to [f^B_z, τ^B_x, τ^B_y, τ^B_z]:

For a + configuration quad with arm length L and rotor positions at [±L,0] and [0,±L]:

Because F̄ is invertible, you can always go from desired [f^B_z, τ^B] to required rotor speeds via w = F̄⁻¹[f^B_z, τ^B]. This inverse mapping is called the thrust-torque mixer or control allocation.

Worked thrust-torque mixing example

Parameters: m=1 kg, g=9.81 m/s², c_f=1e−5 N/(rad/s)², c_d=1e−6 Nm/(rad/s)², L=0.25 m. At hover: total thrust = mg = 9.81 N, all torques = 0. Each rotor: T_i = 9.81/4 = 2.4525 N. Rotor speed: w_i = √(T_i/c_f) = √(2.4525/1e−5) ≈ 495 rad/s ≈ 4730 rpm. Now apply a roll torque τ_x=0.5 Nm. From F̄ row 2: −c_fL(w₂−w₄) = 0.5 → Δw²=200 → w₂ decreases, w₄ increases by √200 ≈ 14 rad/s each.

python
import numpy as np

# Quadrotor thrust-torque mixer
def mixer(cf=1e-5, cd=1e-6, L=0.25):
    """Returns 4x4 mixing matrix F_bar and its inverse."""
    F = np.array([
        [cf,      cf,      cf,      cf     ],
        [0,      -cf*L,   0,       cf*L  ],
        [cf*L,    0,      -cf*L,   0     ],
        [-cd,     cd,     -cd,      cd     ]
    ])
    return F, np.linalg.inv(F)

F, Finv = mixer()

# Hover: total thrust = mg = 9.81 N, zero torques
m, g = 1.0, 9.81
desired = np.array([m*g, 0.0, 0.0, 0.0])  # [fz, tx, ty, tz]
w_sq = Finv @ desired   # signed-square rotor speeds
w_rpm = np.sqrt(np.abs(w_sq)) / (2*np.pi/60)
print(f"Hover RPM: {w_rpm}")  # ~4730 rpm each

# Roll maneuver: add 0.5 Nm roll torque
desired_roll = np.array([m*g, 0.5, 0.0, 0.0])
w_sq_roll = Finv @ desired_roll
print(f"Roll w_sq: {w_sq_roll}")   # rotor 2 decreases, rotor 4 increases

Chapter 7: Cascaded Control Loops & Geometric Control

A quadrotor's full controller isn't one monolithic block — it's a hierarchy of nested loops. The outer loop handles position; the inner loop handles attitude. This cascaded control structure exploits the timescale separation: attitude dynamics (rotating the drone) are much faster than position dynamics (moving the drone through space).

The position loop: from error to desired attitude

The position controller receives the position error e_p = p^w − p^w_d and velocity error e_v = v^w − v^w_d. It computes an "ideal force" (what force, in any direction, would PD-correct the position):

The gravity term mge₃ and feedforward term mp̈_d pre-cancel gravity and track the desired acceleration. But the quadrotor can only thrust along its own body z-axis. So the controller: (1) sets the desired body z direction z_d^w = f_ideal/‖f_ideal‖, pointing the drone to produce the ideal force; (2) sets the thrust magnitude f_z = f_ideal · R^w_Be₃ (projected onto actual body z).

Geometric control: why SO(3) matters for attitude

The attitude controller must track the desired rotation R_d ∈ SO(3). A naive approach would represent attitude as Euler angles and apply PID to each angle. This fails at singularities (gimbal lock at pitch=±90°) and doesn't respect the geometry of SO(3).

Geometric control (Lee, Leok, McClamroch 2010) works directly on SO(3) by defining the rotation error using the Lie group structure (exactly as in L2). The rotation error vector is:

This is the vee (∨) of the skew-symmetric part of R_d^TR_B — essentially the axis-angle error between current and desired rotation, expressed in the body frame. The angular velocity error is e_ω = ω^B − R_B^TR_dω_d.

The geometric control law

The full geometric controller torque:

The first two terms are PD on the rotation error (in SO(3)). The remaining terms cancel the gyroscopic coupling (ω×Jω) and feedforward the desired angular acceleration. This controller is proven to be almost globally stable on SO(3) — it converges from almost any initial orientation, with only the "upside-down" configuration excluded (a measure-zero set).

Chapter 8: Showcase: Quadrotor Hover Simulator

This chapter is the payoff. A 2D quadrotor (two rotors, planar motion) uses a PID position controller feeding into a PD attitude controller. Click anywhere to set a new target. Use sliders to tune gains and add wind disturbance. Watch how the drone tilts to move, corrects attitude, and hovers at the target.

Chapter 9: Connections & Cheat Sheet

You have now built the complete control stack: from state-space fundamentals through PID and LQR to the geometric controller running on SO(3). Here's how everything connects.

Control cheat sheet

What comes next: Trajectory Optimization (L4)

The control stack assumes a desired position p_d(t) and velocity v_d(t) are given at each instant. Where do those come from? That's the job of the trajectory optimizer (Lectures 9–11), which computes smooth, dynamically feasible paths from start to goal. The interface is clean: trajectory optimizer outputs [p_d(t), v_d(t), p̈_d(t), ψ_d(t)] → geometric controller turns it into rotor commands → drone flies the path.

Connection to Lie groups (L2)

The geometric controller is the application of L2 ideas. The rotation error e_R uses the vee map (∨) to extract the axis-angle vector from the relative rotation matrix R_d^TR_B — this is exactly the log map from L2 (at small angles). The proof of almost-global stability relies on the Lyapunov function constructed on the SO(3) manifold, not on a linearized Euler-angle representation.

Related Gleams

• VNAV L1: 3D Geometry & Transforms — rotation matrices, SE(3), prerequisite
• VNAV L2: Lie Groups & exp Map — SO(3) Lie algebra, retraction, log map
• Kalman Filter — the estimator that feeds pose error into this control stack
• MDP & Optimal Control — how LQR relates to the broader optimal control / RL framework
• Actor-Critic RL — learning-based control that replaces hand-tuned gains