Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add static matrix support to DARE solver (#5536)

Browse Source 
Using static matrices where possible results in a 2x performance
improvement.
This commit is contained in:
Tyler Veness
2023-08-14 09:15:58 -07:00
committed by GitHub
parent 394cfeadbd
commit 03764dfe93
9 changed files with 354 additions and 436 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
View File

@@ -43,7 +43,8 @@ ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP = DARE(discA.transpose(), C.transpose(), discQ, discR);
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = StateMatrix::Zero();
}
@@ -78,7 +79,8 @@ ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP = DARE(discA.transpose(), C.transpose(), discQ, discR);
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = StateMatrix::Zero();
}

2
wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
View File

@@ -48,7 +48,7 @@ KalmanFilter<States, Inputs, Outputs>::KalmanFilter(
}
Matrixd<States, States> P =
DARE(discA.transpose(), C.transpose(), discQ, discR);
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
// S = CPCᵀ + R
Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;