What PID Doesn't Know

Across Unit 8 you've built powerful control with PID and feedforward. For most FRC mechanisms, that combination works extremely well. But PID has a structural limitation worth understanding: it operates on a single number — the current error — and has no direct knowledge of how the mechanism got there or where it's going.

Consider a shooter flywheel. PID measures the difference between target RPM and current RPM. When the error is large, it applies more voltage. When the error shrinks, it backs off. This works, but PID doesn't inherently know the flywheel's rotational inertia, how quickly it can accelerate, or what voltage is physically needed to hold a given RPM against back-EMF. You provide that knowledge indirectly through kP, kI, kD — but the gains don't encode the physics. They encode your tuning intuition.

State-space control takes a fundamentally different approach: it starts with a mathematical model of the mechanism's physics and designs a controller directly from that model. Instead of asking "what gain makes the response look right," it asks "what control law is mathematically optimal given what we know about how this system behaves."

The State Vector: Everything the System Knows About Itself

The central concept in state-space control is the state vector — a compact mathematical description of everything relevant about a mechanism's current condition. For a flywheel, the state might be just one thing: angular velocity. For an arm, it might be two things: angle and angular velocity. For a robot's full position on the field, it might be three: x position, y position, and heading.

Formally, the state vector is written as x (bold x, or x with an arrow). The control input — the voltage or command sent to the actuator — is written as u. The measurement from sensors is written as y.

The state-space model then describes two things:

• How the state evolves over time given a control input: ẋ = Ax + Bu
• What measurements the sensors produce from the current state: y = Cx + Du

A, B, C, and D are matrices — grids of numbers that encode the system's physics. They look intimidating but each has a clear physical meaning. Click each one to understand what it represents for a real mechanism.

The Linear Quadratic Regulator: Optimal State Feedback

Once you have a model of the system, you need a controller. The Linear Quadratic Regulator (LQR) is a method for designing that controller automatically, given two user-specified cost matrices that express your priorities:

• Q — how much you penalize state error. A high Q entry for velocity means you want velocity to track the setpoint aggressively. A low Q entry means you're tolerant of velocity error.
• R — how much you penalize control effort (voltage). A high R means you want the controller to be conservative with voltage. A low R means you're willing to use aggressive voltage to achieve the goal.

Given the system model (A, B matrices) and your Q and R preferences, LQR solves a mathematical optimization problem and produces a gain matrix K. The controller output is then simply:

This looks like PID — multiply a gain by an error — but K is a matrix that accounts for all state variables simultaneously. For a flywheel with only velocity as a state, K is a 1×1 matrix and the result is nearly identical to a well-tuned proportional controller with good feedforward. For an arm with position and velocity as states, K is 1×2 and the controller simultaneously considers both "how far off am I?" and "how fast am I moving?" This is the D term's insight — baked in automatically from the model rather than tuned manually.

The Kalman Filter: Trusting Your Model Alongside Your Sensors

Controllers need to know the current state. For a flywheel, you can measure velocity directly from an encoder. But encoders are noisy — the measured value jumps around the true value by some amount on every loop. LQR driving off noisy measurements produces noisy control outputs, which causes the mechanism to jitter.

The Kalman filter solves this by combining two sources of information on every timestep:

1. The model prediction — "given the voltage we applied last loop and our physics model, where should the state be now?"
2. The sensor measurement — "what does the encoder actually read right now?"

Neither source is perfectly trusted. The model has uncertainty (the physics model isn't exact). The sensor has noise (the encoder reading bounces). The Kalman filter blends the two in proportion to how much each one is trusted, producing a smoothed state estimate that is more accurate than either source alone.

You configure the Kalman filter with two noise matrices:

• stateStdDevs — how much you trust your model. High values mean the model is uncertain; the filter leans more on measurements.
• measurementStdDevs — how noisy your sensors are. High values mean measurements are unreliable; the filter leans more on the model.

Comparing Controllers: A Live Flywheel Simulation

The following simulation runs three controller strategies simultaneously against the same flywheel model. Adjust the target RPM and watch how each controller responds. The differences in settling time, overshoot, and noise rejection illustrate why state-space adds value in high-precision velocity control.

State-Space in WPILib: A Flywheel Example

WPILib's state-space classes are in the edu.wpi.first.math.controller and edu.wpi.first.math.estimator packages. The following is a complete, commented flywheel subsystem using LinearSystemId, LinearQuadraticRegulator, and KalmanFilter. Read through it with the preceding theory in mind — each class corresponds directly to one of the three concepts: model, controller, estimator.

import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.LinearQuadraticRegulator;
import edu.wpi.first.math.estimator.KalmanFilter;
import edu.wpi.first.math.numbers.*;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.LinearSystemLoop;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;

public class FlywheelSubsystem extends SubsystemBase {

    private final TalonFX m_motor = new TalonFX(FlywheelConstants.MOTOR_ID);

    // ── 1. SYSTEM MODEL ───────────────────────────────────────────────────────
    // LinearSystemId builds the A, B, C, D matrices for a flywheel.
    // State: [angular velocity (rad/s)]   Input: [voltage (V)]   Output: [angular velocity]
    // DCMotor.getKrakenX60(1) = one Kraken X60, with no gearing reduction.
    private final LinearSystem<N1, N1, N1> m_plant =
        LinearSystemId.createFlywheelSystem(
            DCMotor.getKrakenX60(1),
            FlywheelConstants.MOMENT_OF_INERTIA_KG_M2,   // measure or estimate from CAD
            FlywheelConstants.GEAR_RATIO);

    // ── 2. KALMAN FILTER (Estimator) ───────────────────────────────────────────
    // Blends model prediction with encoder measurement.
    // stateStdDevs:       how much we trust the model   (rad/s uncertainty)
    // measurementStdDevs: how noisy the encoder is       (rad/s noise floor)
    // Smaller value = more trust. Larger value = less trust.
    private final KalmanFilter<N1, N1, N1> m_observer =
        new KalmanFilter<>(
            Nat.N1(), Nat.N1(),
            m_plant,
            VecBuilder.fill(3.0),    // model uncertainty: 3 rad/s
            VecBuilder.fill(0.01),   // measurement noise: 0.01 rad/s (encoder is accurate)
            0.020);                  // dt: 20 ms standard WPILib loop

    // ── 3. LQR CONTROLLER ─────────────────────────────────────────────────────
    // Q: how much we penalize velocity error (rad/s). Lower = more aggressive.
    // R: how much we penalize voltage usage (V).    Higher = more conservative.
    // LQR solves for the optimal gain matrix K automatically.
    private final LinearQuadraticRegulator<N1, N1, N1> m_controller =
        new LinearQuadraticRegulator<>(
            m_plant,
            VecBuilder.fill(8.0),    // Q: tolerate up to 8 rad/s error
            VecBuilder.fill(12.0),   // R: limit to 12V max effort
            0.020);

    // ── 4. LOOP (combines all three) ───────────────────────────────────────────
    // LinearSystemLoop wires together the plant, controller, and observer.
    // maxVoltage clamps output to protect against voltage saturation.
    private final LinearSystemLoop<N1, N1, N1> m_loop =
        new LinearSystemLoop<>(m_plant, m_controller, m_observer, 12.0, 0.020);

    @Override
    public void periodic() {
        // Correct the Kalman filter with the latest encoder measurement (converted to rad/s)
        m_loop.correct(VecBuilder.fill(getVelocityRadPerSec()));

        // Predict: advance the model one timestep and compute the control output voltage
        m_loop.predict(0.020);

        // Apply the computed voltage to the motor
        m_motor.setVoltage(m_loop.getU(0));
    }

    public void setGoalVelocity(double velocityRadPerSec) {
        m_loop.setNextR(VecBuilder.fill(velocityRadPerSec));
    }

    public boolean atGoal() {
        double errorRadPerSec = m_loop.getError(0);
        return Math.abs(errorRadPerSec) < FlywheelConstants.VELOCITY_TOLERANCE_RAD_PER_S;
    }

    private double getVelocityRadPerSec() {
        // TalonFX returns velocity in rot/s — convert to rad/s for state-space units
        return m_motor.getVelocity().getValueAsDouble()
               * 2.0 * Math.PI / FlywheelConstants.GEAR_RATIO;
    }
}

The One Parameter You Have to Get Right: Moment of Inertia

LinearSystemId.createFlywheelSystem() takes a moment of inertia in kg·m². This is the rotational equivalent of mass — it describes how hard it is to spin the flywheel up or slow it down. Getting it wrong produces a model that doesn't match the real mechanism, which means the LQR gains are computed for the wrong system and performance degrades.

There are three ways to get a good value:

• From CAD. Most modern FRC teams use Onshape or SolidWorks. Both can compute the moment of inertia of a part or assembly about any axis in a single click. This is the most accurate method and should be your first choice.
• From SysId. The kA value from SysId is the acceleration feedforward — and kA is physically related to the moment of inertia: J ≈ kA / (kV / 12V) for a rough estimate. This isn't exact, but it's a usable starting point when CAD isn't available.
• From the WPILib simulation classes. DCMotorSim and FlywheelSim accept a moment of inertia and simulate the mechanism response. If your simulation matches the real hardware behavior with a particular J value, that J is likely correct.

When State-Space Earns Its Complexity

State-space control has real overhead: you need to understand the model, set appropriate noise parameters, and ensure your moment of inertia estimate is accurate. For many FRC mechanisms, feedforward + PID is the right choice. Here's an honest comparison:

🔌 System Check

Knowledge Check