Update KF for multiple predicts per correct

[Shaun345]

Addresses the java portion of issue #4088.

[virtuald]

This feels like it needs a test to see if it actually works.

[calcmogul]

Could you remove the getStorage() calls from the other m_K update in the constructor while you're at it, since that seems to work? (I assume you tried to compile it.)

EDIT: Looks like they're needed to compile. The CI build failed on it.

[Shaun345]

I've added the Joseph form as requested and made the new SteadyStateKalmanFilter.java class that has the original code for the KalmanFilter.java class (with the only changes being the class and constructor name).

There doesn't seem to be an "Allow edits and access to secrets by maintainers" box however, and I can't run the tools for formatting on my machine.

I have added the changes requested by the pull request on frc6357:kalman-filter-predict-fix to run wpiformat and spotless.

[Shaun345]

@calcmogul Updated the discretization as requested. Doesn't strictly match the Extended Kalman filter, but I believe this accomplishes the same goal.

[calcmogul]

The docs at the top of the SteadyStateKalmanFilter class should include something like this:

This class should only be used if predict and correct are called in pairs as
opposed to multiple predicts per correct. If the user wants the latter, they
should use KalmanFilter instead.

[Shaun345]

LinearSystemLoop.java uses an Kalman filter, however, the correct step on the Kalman filter now requires a new parameter (dtSeconds - the time since the last correction step) to rediscretize R. If we were to add the parameter to the correct function for LinearSystemLoop.java, it would cause errors in various example projects. Would it be best to change the Kalman filter used in LinearSystemLoop.java to the SteadyStateKalmanFilter?

[calcmogul]

Just save the dt to a member variable like the EKF does.