Drone control: from PID to nonlinear MPC

Quadrotor dynamics are nonlinear, underactuated, and fast. A 1 kg drone responds in milliseconds. The control architecture has converged on a three-layer hierarchy that's been the production standard for ten years — and is still the answer in 2026 even with RL gaining ground. Here's how the layers work and why this structure won.

The three layers

Layer	Rate	Input	Output
Position	~100 Hz	Position trajectory	Desired thrust + attitude
Attitude	~500 Hz	Desired attitude	Desired body rates
Rate	~2 kHz	Desired body rates	Rotor torques

Each layer is faster than what's above. Position controls thrust + attitude; attitude controls rates; rates control rotors. Outside-in cascade.

The rate loop (PID, the foundation)

Three independent PIDs on roll-rate, pitch-rate, yaw-rate. Each takes a desired rate from the attitude layer; outputs a torque command. PD with optional I term.

Why PID at this layer:

• Rate dynamics are nearly linear (motors respond linearly to PWM, body rates respond linearly to torques).
• PID is fast enough to run at 2 kHz on a Pixhawk-class autopilot.
• Hand-tuned in flight; no model required.

Tune the rate PID first; everything above depends on it.

The attitude loop

Convert desired attitude (a quaternion or rotation matrix) to desired body rates. Common implementation: SO(3) attitude controller (Lee et al. 2010):

e_R = (1/2) · (R_d^T R − R^T R_d)^vee
ω_d = −K_R · e_R

The error is computed on the rotation manifold (SO(3)) rather than as Euler angles, avoiding gimbal-lock issues. Output: desired body rates fed to the rate loop.

K_R is a diagonal gain matrix; tune empirically.

The position loop

Two outputs: total thrust u_1 and desired attitude R_d.

F_des = m · (a_des + g·ẑ)
u_1 = ‖F_des‖
ẑ_b_des = F_des / ‖F_des‖

Construct R_d from the desired body z-axis and a yaw command. Feed R_d to the attitude loop and u_1 directly to motors.

a_{\text{des}} comes from a position-tracking PID:

a_des = K_p (p_des − p) + K_v (v_des − v) + a_ref

Where a_{\text{ref}} is the reference acceleration from the trajectory generator.

Trajectory generation: minimum snap

Aggressive flight needs smooth trajectories. Minimum-snap polynomials (Mellinger 2011) produce dynamically-feasible trajectories through waypoints:

• Each segment is a 7th-degree polynomial in position.
• Continuity in position, velocity, acceleration, jerk, snap at waypoints.
• Coefficients chosen to minimize integrated snap squared.

Closed-form QP solution. Compute trajectories in milliseconds. Used by every aggressive drone (drone racing, agile aerial cinematography).

Why snap, not jerk? In differential flatness theory, snap is roughly proportional to thrust changes. Smooth snap → smooth motors → safe maneuvers.

Differential flatness — the magic

Quadrotor dynamics have a beautiful property: any sufficiently smooth trajectory in the four "flat outputs" — (x, y, z, \psi) (position + yaw) — is dynamically feasible. The required orientation, body rates, and motor commands can all be computed analytically.

Implication: planning is easy. Plan a smooth path in 4D output space; controllers track it perfectly. No worrying about whether a trajectory respects the dynamics — by construction it does.

This is why drone trajectory generation is far simpler than humanoid trajectory generation. The math handed us a gift.

NMPC for aggressive flight

For drone racing or aggressive maneuvering, replace the position PID + attitude PID with a single nonlinear MPC. Optimize over the full quadrotor model with constraints:

• Thrust limits.
• Body rate limits.
• Obstacle keep-outs.

Solve at 100 Hz. acados is the production solver. UZH's Swift drone uses this stack to beat human champions at racing.

Adds ~10× compute over the cascaded PID stack. Worth it for the agility; not needed for a delivery drone.

Production autopilots

• PX4: open-source flight stack. Cascaded PID + complementary filter for state estimation. The default for hobby and many commercial drones.
• ArduPilot: similar; longer history; broader vehicle support (planes, copters, rovers, boats).
• Crazyswarm: research-friendly framework on Crazyflie hardware. Same hierarchy at smaller scale.
• Custom NMPC stacks: research labs and racing teams. ETH's RPG, UZH's lab, Cyverse Robotics.

The state estimation problem

Drones need pose at 100+ Hz. Sources:

• IMU: 1+ kHz; integrated for body rates; drifts for orientation.
• Magnetometer: yaw absolute reference; magnetically noisy environments degrade it.
• Barometer: altitude reference; drifts but corrects vertical drift.
• GPS: outdoor only; ~1 m accuracy without RTK.
• Optical flow: downward camera; provides body-frame velocity estimate.
• Visual-inertial odometry: indoor or GPS-denied; the modern default.

Fuse with EKF (PX4's EKF2, ArduPilot's EKF3). All the standard tricks from the SLAM lessons apply.

RL for drones

Increasingly common (2024+):

• Swift (UZH 2023): RL drone-racing policy beat human champions.
• Vision-conditioned RL policies: from camera + IMU directly to rotor commands.
• Hybrid: RL on top of NMPC; NMPC for safety, RL for the last bit of agility.

Same pattern as quadrupeds. Faster training (drone dynamics are simpler than legged dynamics), cleaner sim-to-real.

Common gotchas

• Battery sag: throttle command produces less thrust as voltage drops. Compensate or use voltage feedforward.
• Wind: large unmodeled disturbance. Disturbance observers help; wind-feedforward if measured.
• Yaw drift: pure IMU integration drifts. Magnetometer or visual yaw is required.
• Saturation: an aggressive turn can saturate motors; trajectory tracking degrades. NMPC handles this naturally; cascaded PID needs careful gain scheduling.

Exercise

In a sim (PyBullet quadrotor or Crazyflie sim), implement the three-layer hierarchy. Tune rate first, attitude second, position third — outside-in tuning fails. Hover, then track a circle, then a figure-8. Each adds a layer of difficulty; debugging each cleanly is the foundation of agile flight.

Next

Autonomous driving stack anatomy — the same hierarchical pattern at car scale, with much more perception bolted on top.