Questions about Implementation of State-Space Models in Java

I am a programmer, exploring the full capabilities of WPILib and implementing new things in our team’s code. I saw state-space models and I am trying to control systems like a flywheel and turret with it. However, I am not sure how to tune the model. With PID Controllers, there are 3-4 values to change and you can change it based on how it reaches your target on a graph. With state-space models, I am unsure on how to get a visualization of the Kalman filter’s predicted output so that I can compare it to the real output. This would allow me to tune the filter. I’m looking to create a chart similar to what’s on this page.

I am also confused on how to tune the relms and qelms in the LinearSystemLoop. As stated in this page, changing the relms and qelms inversely can cause the same output. Therefore, how will I know which one to change based on the controller trying to reach the setpoint?

Any help is greatly appreciated.

4 Likes

When I was mentoring 3512, we used state-space, and we logged basically everything (#logallthethings). In particular, we pushed the references, state estimates, measurements, and control inputs to NetworkTables and plotted them in real time on the Glass dashboard. We also logged everything to CSV for diagnostics in Python afterward.

That docs page is incorrect. Multiplying q by 10 does not have the same effect as dividing r by 10. I’ll submit a PR to fix it.

The way I recommend tuning the tolerances is setting r to your max voltage, then choosing a desired q tolerance as a starting point (e.g., 5 rad/s). If the controller oscillates, increase the tolerance until the oscillation stops. Measurement delay is the most common cause of oscillation with otherwise optimal gains. See Debugging State-Space Models and Controllers — FIRST Robotics Competition documentation for more about that.

Model-based control requires very little tuning in general though as long as mechanical gave you a robot that has minimal mechanism backlash and clean, low-latency sensor readings. Now that I’ve used LQR, I’d never go back to tuning raw PID gains. LQR gains tend to be aggressive, but that’s because you really can get that kind of performance without overshoot if you engineer your electromechanical system well. You can always tune the tolerances to be less aggressive if you have to.

By the way, be on the lookout for the LTV controller classes in WPILib 2023. The LTV diff drive controller is a flat topology controller, and the LTV unicycle controller is a cascaded topology controller that’s a replacement for Ramsete with better tuning knobs:

3 Likes

After I first saw you mention it, I tried LTVUnicycleController out on my Romi and wow, what a difference. So much easier to get right than Ramsete. Nice work!

2 Likes

When I was mentoring 3512, we used state-space, and we logged basically everything (#logallthethings). In particular, we pushed the references, state estimates, measurements, and control inputs to NetworkTables and plotted them in real time on the Glass dashboard. We also logged everything to CSV for diagnostics in Python afterward.

Thanks so much for your response. This really helped me understand LQR’s more. I’m still confused on the specific methods to get these values, however. For example, if we have a LinearSystem<N1, N1, N1> plant, KalmanFilter<N1, N1, N1> filter, LinearQuadraticRegulator<N1, N1, N1> controller, and LinearSystemLoop<N1, N1, N1> loop, how do I get references, state estimates, measurements, and control inputs? There’s methods like
loop.getU(int row), but I don’t know which row is what.

LinearSystemLoop has getU() for the input, getXhat() for the state estimate, and getNextR() for the next reference. The measurements would just come directly from your sensors. You can get the current reference by calling getController().getR().

That depends on your model. A flywheel would have a voltage input, velocity state, and velocity measurement. Here’s an example where position is the first row of the state, velocity is the second row of the state, and voltage is the only input: https://github.com/wpilibsuite/allwpilib/blob/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java.

That clears it up a lot. Thank you so much. I’ll also try the LTV Controller when I can since it seems really cool. Again, thanks so much.

1 Like

Hello again, I have been able to try to implement state-space models with a flywheel, but unfortunately my attempt was not successful. The code and the sysid tool’s config are in this repository. The motor we are testing with is a Neo550 with a 10:1 gearbox on it. We were attempting to tune the Kalman filter to match the output velocity with an encoder. We first set velocity with the setVelocity(double velocity) method and compared the velocity graphs of getEstimatedVelocity() and getVelocity() on SmartDashboard. The graphs did not match and we were unable to tune the values to get them to match either. In addition, when we tried to set the velocity twice but with the same value, it did not stay the same velocity. Instead, the voltage to the motor switched between -12 and 12. The estimated velocity and encoder velocity changed in opposite directions. Mainly, I am unsure on how to tune the Kalman filter at this point. Any help would be greatly appreciated.

Your model could be really inaccurate, or you could be trusting your measurements nowhere near enough. Here’s what 3512 used for their state and measurement standard deviations:

That’s an angular velocity process noise stddev of 200 rad/s and an angular velocity measurement noise stddev of 2*pi()/8/512/0.005 = 0.3 rad/s.

We chose a really high process noise because you have to include unmodeled disturbances like a ball going through the flywheel when determining the model uncertainty. Unless your measurements are really bad, the model shouldn’t be doing that much, and the state estimate should be tracking the measurements really closely.

Hello, I am also working with vyang on this project. Since then, we have updated the gear ratio to a simple 1:1, as well as experimenting with new Kalman Filter and LQR values (in the repository vyang posted)

(Thevalues came from the WPILib walkthrough).
If all our values from the sysid are correct, then the problems were are having is still with the LQR and Kalman filter. Even trying the values from the 3512 repository, it doesn’t have much effect on the problems described earlier (the estimated and getVelocity do not match the input velocity, and the voltage switches from -12 to 12. About the measurements being off, are there specific conversion factors we are supposed to consider in our filters?

Could you post plots of your LQR reference and control input, and your Kalman filter measurements and state estimate?

(Hopefully I did this right)
I added the LQR reference, control input, state estimate, and the velocity of the flywheel. I wasn’t sure how to plot the Kalman filter measurements.

When the control input jumps from -12 to 12 and back it is when I click the instantCommand on shuffleBoard to set the velocity to 1. It seems that regardless of what I input as the velocity (even 0), it jumps to this maximum voltage.

Those are just the encoder measurements. I assume that’s what’s in the left-most plot.

Your velocity state estimate and velocity measurements are doing opposite things at the same time, which implies your voltages are different signs for each (e.g., one is negated). Make sure a positive output of LQR (positive voltage) makes the velocity measurement become more positive, and make sure that positive input is being fed to the state estimator as is. You can invert the motor controllers (setInverted()) and/or the measurements to get the signs to match.


I added the line motor.setInverted(true); (motor is the SparkMAX) into the public FlyWheel() constructor, but its not having an effect. In the picture, pressing instantCommand (with an input velocity of 1) has the motor and estimate moving regardless of whether the motor was inverted.

I did notice that in both situations (clearly highlighted by the first graphs in the setInverted(false) that when I first clicked InstantCommand only once, all four graphs moved together (but when it was clicked again, the inversion took place)

Is there another way to invert the motors, or am I making a mistake?

Assuming the motor wasn’t inverted before, setInverted(true) should be inverting it. If it’s a device setting rather than a flag in the motor controller class, maybe the setting isn’t making it across the CAN bus in a timely manner? You could work around it by negating what you pass to the motor’s set() function, although that’s not ideal.

Thank you so much! We were able to fix the issue with inverted issue. By automatically updating the setVelocity() function to take place periodically rather than sending the instant command, it also solved the issue of the -12 to 12 fluctutation. Along with some tuning with the filter and LQR, we ended up getting the input velocity to match the state estimates almost exactly. The only problem taking place now is likely an encoder ticks conversion rate.
image
I wasn’t sure the exact conversion ratio to RPM, because telling the motor to spin at 60 RPM was visually innacurate (it was spinning much faster than 1 rotation per second). Do I need to divide the encoder.getVelocity() by 42 to keep it at 1 count per revolution, or should there be other conversions?

These docs say the built-in encoder returns RPM:
https://codedocs.revrobotics.com/java/com/revrobotics/relativeencoder#getVelocity()

You said there’s a 10:1 gearbox (I assume 10:1 speed reduction?). The encoder value is from before the gearbox. If you want RPM of the flywheel, you’ll need to divide the encoder value by 10. That’ll also mean your model’s gear ratio needs to be changed. You can fix the model without rerunning SysId by doing what this does with new and old gear ratio.

I forgot to mention that we ended up removing the gearbox so the ratio is now 1:1.


Previously, we had the getVelocity() return encoder.getVelocity() / 42;, and that was what kept the estimated RPM and state estimate the same. However, we realized that the conversion rate was accounted for by REV, so it is now just encoder.getVelocity(); (since there is no gear ratio either).

The problem now is in the shuffleboard where by inputting an instant command of 20 RPM our velocity graphs are off. I think that there is still a conversion ratio problem, but I’m unsure, since the way we were able to get the graphs to match our input RPM was when we divided the encoder.getVelocity by some number (both 42 and 60 worked, but I don’t think that there should be any conversion ratio).

Have you tried doing a factory reset of the spark max? Some leftover conversion settings might be messing with it.

After the factory reset I think we have something more reasonable. We have our getVelocity() not divided by a number (as it probably should have been). Our input flywheel velocity almost exactly matches the state estimate now, and by looking at the motor we have the Flywheel Velocity (RPM) graph matching the motor! Everything seems to match now, except our input velocity not matching the flywheel velocity (our input of 2 is different from the reading of around 140).

Make sure your feedforward is correct first, then add in LQR. Feedforward might be throwing it off.