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

[wpilib] LinearSystemSim.setState: calculate new output state (#7799)

Browse Source 
Establishes the invariant that the state and measurement always match up, even immediately after construction.
This commit is contained in:
HarryXChen
2025-02-19 01:49:28 -05:00
committed by GitHub
parent bd2211119f
commit c898853b4d
6 changed files with 80 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java
View File

@@ -75,10 +75,10 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
* @param dtSeconds The time between updates.
*/
public void update(double dtSeconds) {
// Update X. By default, this is the linear system dynamics X = Ax + Bu
// Update x. By default, this is the linear system dynamics xₖ₊₁ = Axₖ + Buₖ.
m_x = updateX(m_x, m_u, dtSeconds);
// y = cx + du
// yₖ = Cxₖ + Duₖ
m_y = m_plant.calculateY(m_x, m_u);
// Add measurement noise.
@@ -164,6 +164,11 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
*/
public void setState(Matrix<States, N1> state) {
m_x = state;
// Update the output to reflect the new state.
//
// yₖ = Cxₖ + Duₖ
m_y = m_plant.calculateY(m_x, m_u);
}
/**