Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +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

23
wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java
View File

@@ -272,29 +272,30 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num,
}
/**
* 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.
*/
public void reset(Matrix<States, N1> initialReference) {
m_controller.reset();
m_feedforward.reset(initialReference);
m_observer.reset();
public void reset(Matrix<States, N1> initialState) {
m_nextR.fill(0.0);
m_controller.reset();
m_feedforward.reset(initialState);
m_observer.setXhat(initialState);
}
/**
* Returns difference between reoid predict(double dtSference r and x-hat.
* Returns difference between reference r and current state x-hat.
*
* @return the
* @return The state error matrix.
*/
public Matrix<States, N1> getError() {
return getController().getR().minus(m_observer.getXhat());
}
/**
* Returns difference between reference r and x-hat.
* Returns difference between reference r and current state x-hat.
*
* @param index The index of the error matrix to return.
* @return The error at that index.