Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Address issues with LinearSystemLoop reset() and matrix initialization (#2819)

Browse Source 
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.
This commit is contained in:
Claudius Tewari
2020-10-29 18:10:48 -07:00
committed by GitHub
parent 6ac9683a32
commit 0ce9133b55
2 changed files with 20 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
View File

@@ -227,20 +227,21 @@ class LinearSystemLoop {
}
/**
* Zeroes reference r, controller output u and plant output y.
* The previous reference for PlantInversionFeedforward is set to the
* initial reference.
* @param initialReference The initial reference.
* Zeroes reference r and controller output u. The previous reference
* of the PlantInversionFeedforward and the initial state estimate of
* the KalmanFilter are set to the initial state provided.
*
* @param initialState The initial state.
*/
void Reset(Eigen::Matrix<double, States, 1> initialState) {
m_nextR.setZero();
m_controller.Reset();
m_feedforward.Reset(initialState);
m_observer.Reset();
m_nextR.setZero();
m_observer.SetXhat(initialState);
}
/**
* Returns difference between reference r and x-hat.
* Returns difference between reference r and current state x-hat.
*/
const Eigen::Matrix<double, States, 1> Error() const {
return m_controller.R() - m_observer.Xhat();