State-Space Control and Kalman Filtering

PID treats the mechanism as a black box. State-space control instead uses a model of the physics, which lets you place poles deliberately, fuse noisy sensors optimally, and reason about stability. WPILib ships first-class support via LinearSystem, LinearQuadraticRegulator, KalmanFilter, and LinearSystemLoop.

The three pieces

• Plant model (LinearSystem): the math of how input voltage produces velocity. WPILib’s LinearSystemId builds one from physical constants or from SysId’s kV/kA via identifyVelocitySystem(kV, kA).
• LQR (LinearQuadraticRegulator): the controller. You tell it how much you care about state error vs. control effort, and it computes the optimal gains. For a single-state flywheel the result is mathematically just a P controller – but a principled one.
• Kalman filter (KalmanFilter): the observer. It fuses the noisy encoder reading with the model prediction, giving a smooth state estimate with little lag – so it rejects sensor noise while still reacting fast to real disturbances (like a game piece passing through the flywheel).

A flywheel loop

private final LinearSystem<N1, N1, N1> m_plant =
    LinearSystemId.identifyVelocitySystem(kV, kA);

private final KalmanFilter<N1, N1, N1> m_observer =
    new KalmanFilter<>(Nat.N1(), Nat.N1(), m_plant,
        VecBuilder.fill(3.0),   // model (state) std dev
        VecBuilder.fill(0.01),  // encoder std dev
        0.020);

private final LinearQuadraticRegulator<N1, N1, N1> m_controller =
    new LinearQuadraticRegulator<>(m_plant,
        VecBuilder.fill(8.0),   // how badly we want to hit the target rad/s
        VecBuilder.fill(12.0),  // max control effort (volts)
        0.020);

private final LinearSystemLoop<N1, N1, N1> m_loop =
    new LinearSystemLoop<>(m_plant, m_controller, m_observer, 12.0, 0.020);

Each loop you correct with the measurement, set the next reference, predict, then apply the computed voltage:

m_loop.setNextR(VecBuilder.fill(targetRadPerSec)); // desired state
m_loop.correct(VecBuilder.fill(m_encoder.getRate()));
m_loop.predict(0.020);
m_motor.setVoltage(m_loop.getU(0));

Why teams do this

A well-tuned Kalman flywheel shows little measurement lag during spin-up while still rejecting noise and recovering fast when a ball loads it – behavior that’s hard to get from hand-tuned PID. The C++ includes mirror the Java classes: <frc/estimator/KalmanFilter.h>, <frc/controller/LinearQuadraticRegulator.h>, <frc/system/LinearSystemLoop.h>.

Where to apply it

State-space shines for flywheels, drivetrains, and elevators where you have a good kV/kA model. For a first attempt, characterize with SysId, plug kV/kA into LinearSystemId, and start from the WPILib state-space flywheel example rather than from scratch. Tune the two std-dev knobs: smaller measurement std-dev trusts the encoder more (faster, noisier); larger trusts the model more (smoother, laggier).

Key takeaways

• State-space uses a physics model: LinearSystem (plant) + LinearQuadraticRegulator (controller) + KalmanFilter (observer), tied together by LinearSystemLoop.
• Build the plant from SysId’s kV/kA via LinearSystemId.identifyVelocitySystem().
• Each loop: setNextR -> correct(measurement) -> predict -> setVoltage(getU(0)).
• Tune Kalman std-devs to trade encoder trust (fast/noisy) against model trust (smooth/laggy).

This lesson was adapted from learnfrc.com.