Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix computation of C for DARE (A, C) detectability check (#5609)

Browse Source 
If Q has off-diagonal entries, C and Cᵀ are different.
This commit is contained in:
Tyler Veness
2023-09-04 21:12:25 -07:00
committed by GitHub
parent ac23f92451
commit 814f18c7f5
1 changed files with 5 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
wpimath/src/main/native/include/frc/DARE.h
View File

@@ -73,10 +73,13 @@ void CheckDARE_ABQ(const Eigen::Matrix<double, States, States>& A,
}
// Require (A, C) pair be detectable where Q = CᵀC
//
// Q = CᵀC = LDLᵀ
// C = √(D)Lᵀ
{
Eigen::Matrix<double, States, States> C =
Eigen::Matrix<double, States, States>{Q_ldlt.matrixL()} *
Q_ldlt.vectorD().cwiseSqrt().asDiagonal();
Q_ldlt.vectorD().cwiseSqrt().asDiagonal() *
Eigen::Matrix<double, States, States>{Q_ldlt.matrixL().transpose()};
if (!IsDetectable<States, States>(A, C)) {
std::string msg = fmt::format(