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

15
wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
View File

@@ -52,11 +52,11 @@ class LinearSystemSim {
* @param dt The time between updates.
*/
void Update(units::second_t dt) {
// Update x. By default, this is the linear system dynamics x_k+1 = Ax_k +
// Bu_k
// Update x. By default, this is the linear system dynamics xₖ₊₁ = Axₖ +
// Buₖ.
m_x = UpdateX(m_x, m_u, dt);
// y = Cx + Du
// yₖ = Cxₖ + Duₖ
m_y = m_plant.CalculateY(m_x, m_u);
// Add noise. If the user did not pass a noise vector to the
@@ -115,7 +115,14 @@ class LinearSystemSim {
*
* @param state The new state.
*/
void SetState(const Vectord<States>& state) { m_x = state; }
void SetState(const Vectord<States>& state) {
m_x = state;
// Update the output to reflect the new state.
//
// yₖ = Cxₖ + Duₖ
m_y = m_plant.CalculateY(m_x, m_u);
}
protected:
/**