# State-Space Controller Walkthrough¶

הערה

Before following this tutorial, readers are recommended to have read an מבוא לבקרת State-Space.

The goal of this tutorial is to provide ”end-to-end“ instructions on implementing a state-space controller for a flywheel. By following this tutorial, readers will learn how to:

1. Create an accurate state-space model of a flywheel using or CAD software.

2. Implement a Kalman Filter to filter encoder velocity measurements without lag.

3. Implement a LQR feedback controller which, when combined with model-based feedforward, will generate voltage to drive the flywheel to a .

This tutorial is intended to be approachable for teams without a great deal of programming expertise. While the WPILib library offers significant flexibility in the manner in which its state-space control features are implemented, closely following the implementation outlined in this tutorial should provide teams with a basic structure which can be reused for a variety of state-space systems.

The full example is available in the state-space flywheel (Java/C++) and state-space flywheel system identification (Java/C++) example projects.

## Why Use State-Space Control?¶

Because state-space control focuses on creating an accurate model of our system, we can accurately predict how our will respond to control . This allows us to simulate our mechanisms without access to a physical robot, as well as easily choose that we know will work well. Having a model also allows us to create lagless filters, such as Kalman Filters, to optimally filter sensor readings.

## Modeling Our Flywheel¶

Recall that continuous state-space systems are modeled using the following system of equations:

$\begin{split}\dot{\mathbf{x}} &= \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u} \\ \mathbf{y} &= \mathbf{C}\mathbf{x} + \mathbf{D}\mathbf{u}\end{split}$

Where is the rate of change of the ’s , $$\mathbf{x}$$ is the system’s current state, $$\mathbf{u}$$ is the to the system, and $$\mathbf{y}$$ is the sytem’s .

Let’s use this system of equations to model our flywheel in two different ways. We’ll first model it using using the frc-characterization toolsuite, and then model it based on the motor and flywheel’s .

The first step of building up our state-space system is picking our system’s states. We can pick anything we want as a state – we could pick completely unrelated states if we wanted – but it helps to pick states that are important. We can include in our state (such as elevator velocity if we were only able to measure its position) and let our Kalman Filter estimate their values. Remember that the states we choose will be driven towards their respective by the feedback controller (typically the Linear-Quadratic Regulator since it’s optimal).

For our flywheel, we care only about one state: its velocity. While we could chose to also model its acceleration, the inclusion of this state isn’t necessary for our system.

Next, we identify the to our system. Inputs can be thought of as things we can put ”into“ our system to change its state. In the case of the flywheel (and many other single-jointed mechanisms in FRC®), we have just one input: voltage applied to the motor. By choosing voltage as our input (over something like motor duty cycle), we can compensate for battery voltage sag as battery load increases.

A continuous-time state-space system writes , or the instantaneous rate of change of the system’s 's state, as proportional to the current and . Because our state is angular velocity, $$\mathbf{\dot{x}}$$ will be the flywheel’s angular acceleration.

Next, we will model our flywheel as a continuous-time state-space system. WPILib’s LinearSystem will convert this to discrete-time internally. Review state-space notation for more on continuous-time and discrete-time systems.

### Modeling with System Identification¶

To rewrite this in state-space notation using , we recall from the flywheel state-space notation example, where we rewrote the following equation in terms of $$\mathbf{a}$$.

$\begin{split}V = kV \cdot \mathbf{v} + kA \cdot \mathbf{a}\\ \mathbf{a} = \mathbf{\dot{v}} = \begin{bmatrix}\frac{-kV}{kA}\end{bmatrix} v + \begin{bmatrix}\frac{1}{kA}\end{bmatrix} V\end{split}$

Where $$\mathbf{v}$$ is flywheel velocity, $$\mathbf{a}$$ and $$\mathbf{\dot{v}}$$ are flywheel acceleration, and $$V$$ is voltage. Rewriting this with the standard convention of $$\mathbf{x}$$ for the state vector and $$\mathbf{u}$$ for the input vector, we find:

$\mathbf{\dot{x}} = \begin{bmatrix}\frac{-kV}{kA} \end{bmatrix} \mathbf{x} + \begin{bmatrix}\frac{1}{kA} \end{bmatrix} \mathbf{u}$

The second part of state-space notation relates the system’s current and to the . In the case of a flywheel, our output vector $$\mathbf{y}$$ (or things that our sensors can measure) is our flywheel’s velocity, which also happens to be an element of our vector $$\mathbf{x}$$. Therefore, our output matrix is $$\mathbf{C} = \begin{bmatrix}1 \end{bmatrix}$$, and our system feedthrough matrix is $$\mathbf{D} = \begin{bmatrix}0 \end{bmatrix}$$. Writing this out in continuous-time state-space notation yields the following.

$\begin{split}\mathbf{\dot{x}} &= \begin{bmatrix}\frac{-kV}{kA} \end{bmatrix} \mathbf{x} + \begin{bmatrix}\frac{1}{kA} \end{bmatrix} \mathbf{u} \\ \mathbf{y} &= \begin{bmatrix}1\end{bmatrix} \mathbf{x} + \begin{bmatrix}0\end{bmatrix} \mathbf{u}\end{split}$

The LinearSystem class contains methods for easily creating state-space systems identified using . This example shows a flywheel model with a kV of 0.023 and a kA of 0.001:

 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50  // Volts per (radian per second) private static final double kFlywheelKv = 0.023; // Volts per (radian per second squared) private static final double kFlywheelKa = 0.001; // The plant holds a state-space model of our flywheel. This system has the following properties: // // States: [velocity], in radians per second. // Inputs (what we can "put in"): [voltage], in volts. // Outputs (what we can measure): [velocity], in radians per second. // // The Kv and Ka constants are found using the FRC Characterization toolsuite. private final LinearSystem m_flywheelPlant = LinearSystemId.identifyVelocitySystem( kFlywheelKv, kFlywheelKa); 

### Modeling Using Flywheel Moment of Inertia and Gearing¶

A flywheel can also be modeled without access to a physical robot, using information about the motors, gearing and flywheel’s . A full derivation of this model is presented in Section 8.2.1 of Controls Engineering in FRC.

The LinearSystem class contains methods to easily create a model of a flywheel from the flywheel’s motors, gearing and . The moment of inertia can be calculated using CAD software or using physics. The examples used here are detailed in the flywheel example project (Java/C++).

הערה

For WPILib’s state-space classes, gearing is written as output over input – that is, if the flywheel spins slower than the motors, this number should be greater than one.

הערה

The C++ LinearSystem class uses the C++ Units Library to prevent unit mixups and assert dimensionality.

 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51  private static final double kFlywheelMomentOfInertia = 0.00032; // kg * m^2 // Reduction between motors and encoder, as output over input. If the flywheel spins slower than // the motors, this number should be greater than one. private static final double kFlywheelGearing = 1.0; // The plant holds a state-space model of our flywheel. This system has the following properties: // // States: [velocity], in radians per second. // Inputs (what we can "put in"): [voltage], in volts. // Outputs (what we can measure): [velocity], in radians per second. private final LinearSystem m_flywheelPlant = LinearSystemId.createFlywheelSystem( DCMotor.getNEO(2), kFlywheelMomentOfInertia, kFlywheelGearing); 

## Kalman Filters: Observing Flywheel State¶

Kalman filters are used to filter our velocity measurements using our state-space model to generate a state estimate $$\mathbf{\hat{x}}$$. As our flywheel model is linear, we can use a Kalman filter to estimate the flywheel’s velocity. WPILib’s Kalman filter takes a LinearSystem (which we found above), along with standard deviations of model and sensor measurements. We can adjust how ”smooth“ our state estimate is by adjusting these weights. Larger state standard deviations will cause the filter to ”distrust“ our state estimate and favor new measurements more highly, while larger measurement standard deviations will do the opposite.

In the case of a flywheel we start with a state standard deviation of 3 rad/s and a measurement standard deviation of 0.01 rad/s. These values are up to the user to choose – these weights produced a filter that was tolerant to some noise but whose state estimate quickly reacted to external disturbances for a flywheel – and should be tuned to create a filter that behaves well for your specific flywheel. Graphing states, measurements, inputs, references, and outputs over time is a great visual way to tune Kalman filters.

The above graph shows two differently tuned Kalman filters, as well as a single-pole IIR filter and a Median Filter. This data was collected with a shooter over ~5 seconds, and four balls were run through the shooter (as seen in the four dips in velocity). While there are no hard rules on choosing good state and measurement standard deviations, they should in general be tuned to trust the model enough to reject noise while reacting quickly to external disturbances.

Because the feedback controller computes error using the estimated by the Kalman filter, the controller will react to disturbances only as quickly the filter’s state estimate changes. In the above chart, the upper left plot (with a state standard deviation of 3.0 and measurement standard deviation of 0.2) produced a filter that reacted quickly to disturbances while rejecting noise, while the upper right plot shows a filter that was barely affected by the velocity dips.

 54 55 56 57 58 59 60  private final KalmanFilter m_observer = new KalmanFilter<>( Nat.N1(), Nat.N1(), m_flywheelPlant, VecBuilder.fill(3.0), // How accurate we think our model is VecBuilder.fill(0.01), // How accurate we think our encoder // data is 0.020); 

Because Kalman filters use our state-space model in the Predict step, it is important that our model is as accurate as possible. One way to verify this is to record a flywheel’s input voltage and velocity over time, and replay this data by calling only predict on the Kalman filter. Then, the kV and kA gains (or moment of inertia and other constants) can be adjusted until the model closely matches the recorded data.

## Linear-Quadratic Regulators and Plant Inversion Feedforward¶

The Linear-Quadratic Regulator finds a feedback controller to drive our flywheel to its . Because our flywheel has just one state, the control law picked by our LQR will be in the form $$\mathbf{u = K (r - x)}$$ where $$\mathbf{K}$$ is a 1x1 matrix; in other words, the control law picked by LQR is simply a proportional controller, or a PID controller with only a P gain. This gain is chosen by our LQR based on the state excursion and control efforts we pass it. More on tuning LQR controllers can be found in the LQR application example.

Much like SimpleMotorFeedforward can be used to generate feedforward voltage inputs given kS, kV, and kA constants, the Plant Inversion Feedforward class generate feedforward voltage inputs given a state-space system. The voltage commands generated by the LinearSystemLoop class are the sum of the feedforward and feedback inputs.

 63 64 65 66 67 68 69 70 71 72  private final LinearQuadraticRegulator m_controller = new LinearQuadraticRegulator<>(m_flywheelPlant, VecBuilder.fill(8.0), // qelms. Velocity error tolerance, in radians per second. Decrease // this to more heavily penalize state excursion, or make the controller behave more // aggressively. VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more // heavily penalize control effort, or make the controller less aggressive. 12 is a good // starting point because that is the (approximate) maximum voltage of a battery. 0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be // lower if using notifiers. 

## Bringing it All Together: LinearSystemLoop¶

LinearSystemLoop combines our system, controller, and observer that we created earlier. The constructor shown will also instantiate a PlantInversionFeedforward.

 73 74 75 76 77 78  // The state-space loop combines a controller, observer, feedforward and plant for easy control. private final LinearSystemLoop m_loop = new LinearSystemLoop<>( m_flywheelPlant, m_controller, m_observer, 12.0, 

Once we have our LinearSystemLoop, the only thing left to do is actually run it. To do that, we’ll periodically update our Kalman filter with our new encoder velocity measurements and apply new voltage commands to it. To do that, we first set the , then correct with the current flywheel speed, predict the Kalman filter into the next timestep, and apply the inputs generated using getU.

 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126  @Override public void teleopPeriodic() { // Sets the target speed of our flywheel. This is similar to setting the setpoint of a // PID controller. if (m_joystick.getTriggerPressed()) { // We just pressed the trigger, so let's set our next reference m_loop.setNextR(VecBuilder.fill(kSpinupRadPerSec)); } else if (m_joystick.getTriggerReleased()) { // We just released the trigger, so let's spin down m_loop.setNextR(VecBuilder.fill(0.0)); } // Correct our Kalman filter's state vector estimate with encoder data. m_loop.correct(VecBuilder.fill(m_encoder.getRate())); // Update our LQR to generate new voltage commands and use the voltages to predict the next // state with out Kalman filter. m_loop.predict(0.020); // Send the new calculated voltage to the motors. // voltage = duty cycle * battery voltage, so // duty cycle = voltage / battery voltage double nextVoltage = m_loop.getU(0); m_motor.setVoltage(nextVoltage); } }