PID controller

Deep dive

How it works

In: setpoint r, measurement y, real dt, gains Kp, Ki, Kd · Out: actuator command u · State: integral I, last measurement, filtered derivative (3 floats) · Cost: O(1) per sample, about 10 multiply-adds

Parallel form, u = Kp*(beta*r - y) + I + D:

1. P: Kp*(beta*r - y). beta = 1 is the classic form; beta < 1 softens setpoint steps without changing disturbance rejection.
2. D on measurement: differentiate -y, not e. A setpoint step then never produces an infinite spike ("derivative kick").
3. Filtered D: D(s) = -Kd*s / (Tf*s + 1), Tf = Kd/(Kp*N). Backward Euler, which is stable for any dt: d_k = (Tf*d_{k-1} - Kd*(y_k - y_{k-1})) / (Tf + dt).
4. Output clamp: u = clamp(P + I + D, u_min, u_max).
5. I with back-calculation: I += dt*(Ki*e + Kt*(u - u_unclamped)). While unsaturated the second term is 0; while saturated it bleeds the integral so that u_unclamped stays near the limit. Kt = 1/Tt, Tt of order Ti (Astrom and Hagglund suggest between Td and Ti).
6. Bumpless transfer: when switching manual/auto or changing gains, set I = u_current - P - D instead of resetting to 0.

 r ──►(+)──e──► Kp ──────────────┐
      ▲ -                         ├─►(+)──► clamp ──► plant ──┐
      │        ┌► Ki∫ ◄─(Kt)─────┤                            │
      │        │      back-calc: u - u_unclamped              │
      │        └─────────────────┘                            │
      └──────────── y ──── d/dt, low-pass (Kd) ───────────────┘

Code

pub struct Pid {
    pub kp: f64,
    pub ki: f64,
    pub kd: f64,
    pub tf: f64,       // derivative filter time constant (s); tf = kd / (kp * N), N ~ 5..20
    pub kt: f64,       // anti-windup back-calculation gain (1/s); 0 disables; ~ 1/sqrt(Ti*Td) or 1/Ti
    pub beta: f64,     // setpoint weight on P (1 = classic, 0 = P acts on -y only)
    pub u_min: f64,
    pub u_max: f64,
    i: f64,            // integral term, already scaled by ki
    d: f64,            // filtered derivative term
    y_prev: f64,
    primed: bool,
}

impl Pid {
    pub fn new(kp: f64, ki: f64, kd: f64, n: f64, u_min: f64, u_max: f64) -> Self {
        let tf = if kd > 0.0 && kp > 0.0 { kd / (kp * n) } else { 0.0 };
        let kt = if ki > 0.0 && kp > 0.0 { ki / kp } else { 0.0 }; // 1/Ti
        Pid { kp, ki, kd, tf, kt, beta: 1.0, u_min, u_max, i: 0.0, d: 0.0, y_prev: 0.0, primed: false }
    }

    /// One step. `dt` is the real elapsed time of this sample (s).
    pub fn update(&mut self, r: f64, y: f64, dt: f64) -> f64 {
        if !self.primed { self.y_prev = y; self.primed = true; } // no derivative kick on first call
        let e = r - y;
        let p = self.kp * (self.beta * r - y);
        // Derivative on measurement (not on error): setpoint steps never kick.
        // Backward-Euler first-order filter: D(s) = -kd*s / (tf*s + 1) applied to y.
        self.d = (self.tf * self.d - self.kd * (y - self.y_prev)) / (self.tf + dt);
        self.y_prev = y;

        let u_unsat = p + self.i + self.d;
        let u = u_unsat.clamp(self.u_min, self.u_max);
        // Integrate error, plus back-calculation: bleed the integral by the saturation excess.
        self.i += dt * (self.ki * e + self.kt * (u - u_unsat));
        u
    }

    pub fn reset(&mut self) { self.i = 0.0; self.d = 0.0; self.primed = false; }
    /// Bumpless start/handover: make the next output equal `u` at the current error.
    pub fn preload(&mut self, u: f64, r: f64, y: f64) {
        self.i = u - self.kp * (self.beta * r - y);
        self.d = 0.0; self.y_prev = y; self.primed = true;
    }
}

Test harness used to verify it (first-order plant y' = -y + 2u, u in [-1, 1], dt = 10 ms, Kp=2, Ki=3): zero steady-state error to 1e-3 after 20 s; for a setpoint of 1.9 (heavy saturation) overshoot is 0.012 with back-calculation and 0.100 without; a setpoint step produces no D spike; preload makes the next output equal the preloaded value.

Complexity and constants

• Per update: about 10 multiply-adds, one divide (precompute 1/(Tf+dt) when dt is fixed), no allocation, no branch except the clamp. Order of tens of ns on a Cortex-M4 FPU, hundreds of ns in software float.
• State: integral, filtered derivative, previous measurement (3 values). Gains: 4-6 values.
• The cost is never the arithmetic. It is the I/O around it: reading the sensor (ADC or bus transaction) and writing the actuator (PWM or DAC register) at every tick.
• Loop rate: sample 10-20 times faster than the desired closed-loop bandwidth (order of); faster buys little and costs energy.

Efficiency

• Bound: latency and jitter of the loop, not compute. A late sample acts like extra dead time and eats phase margin.
• Lowest sufficient rate: halving the rate halves ADC conversions, bus transactions and wakeups. A thermal loop needs 1-10 Hz; a motor current loop 10-40 kHz.
• Cascade: fast inner loop (rate or current) on a cheap P or PI, slow outer loop (position or speed) at a tenth the rate. The outer loop's compute and I/O cost is cut by that factor.
• Fixed-point: Q15/Q16.16 with precomputed Ki*dt is exact enough on MCUs without an FPU; see deterministic fixed-point math. Scale gains so the integral cannot overflow.
• Skip the D term when the plant is slow and noise matters: PI is cheaper, has one fewer filter and no noise-induced actuator wear, and covers most thermal and flow loops.
• Sleep between ticks: drive the loop from a hardware timer (fixed-rate control loop) and let the core idle; avoid busy-wait polling.

Implementation notes

• Run from a timer interrupt or a dedicated RTOS task at a fixed period. Measure dt anyway (monotonic clock) and clamp it to avoid a spike after a stall.
• Anti-windup alternative: conditional integration (freeze I when saturated and the error pushes further into saturation). Simpler, no Kt, slightly worse recovery.
• Integrate with trapezoid (dt*Ki*(e_k + e_{k-1})/2) if the loop rate is low relative to the dynamics. At a high rate the difference is negligible.
• Store gains in Ki/Kd form (parallel) internally; users often enter Kp, Ti, Td (standard form: Ki = Kp/Ti, Kd = Kp*Td). Changing Kp online with the standard form rescales all three terms, in parallel form it does not.
• Feed-forward (u += Kff*r) lifts the main load off the PID: the integrator only trims the error, so setpoint response is faster.
• Quantized sensors (encoders at low speed) make D jumpy: filter harder or compute speed over a longer window.
• Output slew limit and deadband compensation belong after the controller, not inside the gains.

Pitfalls

• D on error: one big output spike at every setpoint step.
• Unfiltered D: a few counts of sensor noise become full-scale output chatter, heating motors and wearing valves.
• Integrator not clamped or reset on mode change: windup, or a jump when switching manual to auto.
• Changing dt (jittery loop, missed deadlines) without passing it in.
• Integer arithmetic: Ki*dt*e underflows to zero for small errors; the integrator stops working. Keep extra fractional bits in the accumulator.
• Tuning for a step response in one operating point: gain scheduling is needed when the plant gain changes with speed, load or temperature.
• Using the same loop on a sensor with large delay: derivative acts on stale data.

Variants and alternatives

In production

Everywhere: PX4 and ArduPilot attitude and rate loops, motor drives (current and speed loops of every FOC drive), 3D-printer hotend heaters (Marlin, M301; Klipper, config), multicopter attitude and rate loops (PX4 tuning guide), HVAC thermostats, cruise control, ROS ros2_control controllers, industrial PLC loops (the large majority of industrial loops are PI or PID, per Astrom and Hagglund).

Implementations

Sources

• Karl J. Astrom, Richard M. Murray, Feedback Systems: An Introduction for Scientists and Engineers, 2nd ed., Princeton University Press 2021 (ch. 11 PID control). free PDF
• Karl J. Astrom, Tore Hagglund, Advanced PID Control, ISA 2006 (filtering, anti-windup, 2-DOF, tuning).
• J. G. Ziegler, N. B. Nichols, Optimum Settings for Automatic Controllers, Transactions of the ASME 64, 1942.
• K. J. Astrom, T. Hagglund, Automatic tuning of simple regulators with specifications on phase and amplitude margins, Automatica 20(5), 1984 (relay autotuning). doi
• Brett Beauregard, Improving the Beginner's PID, 2011. blog series (derivative kick, windup, bumpless transfer; basis of the Arduino library).