This address some problems with the LinearSystemLoop class that were discovered through testing.
The initial state estimate of the observer was set to the provided initial state rather than zero as previously, a non zero initial state passed into reset() would lead to a discrepancy between the current state estimate and the actual system state.
By storing the previous dt, it can be moved into Correct() where it is
actually used. This lets us take the continuous R as an argument in the
user-provided R overload.
This does not introduce any breaking changes for teams that used the TrajectoryGenerator API for
quintic splines with poses.
The GetQuinticControlVectorsFromWaypoints() method was removed because it is not possible for us (or
would require breaking changes to the shape of the splines) to generate only one quintic control vector
per Pose2d. Users who actually have control vectors directly (i.e. not from Pose2d objects, but a
dashboard such as PathWeaver) should have the number of control vectors correspond to the number of
"waypoints" and can call GetQuinticSplinesFromControlVectors() directly.
This includes physics simulation support for arms/elevator models, as well as differential drivetrains.
Swerve might be added at a later date.
Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
It was added as part of Bryson's rule described in
https://file.tavsys.net/control/controls-engineering-in-frc.pdf. It
doesn't really simplify usage though, and the same thing can be
replicated by multiplying the elements of Q by rho manually. It's easier
to do it that way, it's how 3512 has been doing controller debugging for
a while, and it's probably what other teams will do as well instead of
using the "more structured" way.
Removing these unhelpful overloads also simplifies the LQR interface.
I didn't notice a performance difference between the original
implementation and this one for a flywheel simulation, so this
simplifies a lot of internals.
This class can no longer implement KalmanTypeFilter because that class
allows setting the error covariance for use in the
KalmanFilterLatencyCompensator class. This won't impact the holonomic pose
estimators that use KalmanFilterLatencyCompensator because they all use an EKF.
The wpimath library is a new library designed to separate the reusable math functionality
from the common utility library (wpiutil) and the hardware-dependent library (wpilibc/j).
Package names / include file names were NOT changed to minimize breakage. In a future year
it would be good to revamp these for a more uniform user experience and to reduce the risk
of accidental naming conflicts.
While theoretically all of this functionality could be placed into wpiutil, several pieces
of this library (e.g. DARE) are very time-consuming to compile, so it's nice to avoid this
expense for users who only want cscore or ntcore. It also allows for easy future separation
of build tasks vs number of workers on memory-constrained machines.
This moves the following functionality from wpiutil into wpimath:
- Eigen
- ejml
- Drake
- DARE
- wpiutil.math package (Matrix etc)
- units
And the following functionality from wpilibc/j into wpimath:
- Geometry
- Kinematics
- Spline
- Trajectory
- LinearFilter
- MedianFilter
- Feed-forward controllers
