search

wpilibsuite / allwpilib

Official Repository of WPILibJ and WPILibC
https://wpilib.org/
Other
1.08k stars 612 forks source link

Is DifferentialDriveStateEstimator a WIP feature? #3075

Closed LudumDareDevelopment closed 3 years ago

LudumDareDevelopment commented 3 years ago

In the WPILib docs, I saw at the end of the section that there is a DifferentialDriveStateEstimator class that from my interpretation uses the drivetrain feedforward model as a state estimation to fuse with the local measurements. My question is if this class can be used in conjunction with the LinearQuadraticRegulator class for more optimal trajectory following. Also, this class is not available in the current version, so I am wondering when it will be available or if it is rather an experimental feature. Thanks in advance.

calcmogul commented 3 years ago

You're current about DifferentialDriveStateEstimator's purpose. We removed it from the 2021 release because we weren't confident in the API yet. Looks like we forgot to update the docs to reflect that.

You can use DifferentialDrivePoseEstimator to estimate the robot's pose, then use Ramsete and some velocity LQRs or the following full state controller to close the loop on it.

Screenshot_2021-01-10_11-57-02 from section 8.6 of https://controls-in-frc.link

The actual implementation is complex.

https://github.com/frc3512/Robot-2020/blob/master/src/main/include/controllers/DrivetrainController.hpp https://github.com/frc3512/Robot-2020/blob/master/src/main/cpp/controllers/DrivetrainController.cpp

We originally planned to have an LTVDiffDriveController class, but it didn't make the 2021 cutoff because we were focused on debugging the pose estimators. If you have the time and expertise, you could try implementing it yourself, but I'd recommend using Ramsete otherwise.

pietroglyph commented 3 years ago

Like Tyler said, DifferentialDriveStateEstimator is designed for use in conjunction with the LQR (i.e. it observes all the states you need to control.) I didn't include it in the pose estimators PR mostly because the Java version was broken and we were running out of time. If I recall correctly, the LTV differential drive controller was in the same boat.

You can find the Java implementation we had here: https://github.com/pietroglyph/allwpilib/blob/b999d2e1cdd227b8b026848a8957b21a1f706b89/wpilibj/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDriveStateEstimator.java (note: this implementation doesn't work and also doesn't pass the right custom arithmetic functions to the UKF.)

LudumDareDevelopment commented 3 years ago

Thanks for the info! I think a velocity LQR would probably suffice for FRC purposes.