Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix DARE Q decomposition (#5611)

Browse Source
This commit is contained in:
Tyler Veness
2023-09-05 10:48:43 -07:00
committed by GitHub
parent 9b3f7fb548
commit 1a6df6fec6
5 changed files with 49 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -74,12 +74,15 @@ 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ᵀ
// Q = CᵀC = PᵀLDLᵀP
// Cᵀ = PᵀL√(D)
// C = (PᵀL√(D))ᵀ
{
Eigen::Matrix<double, States, States> C =
Q_ldlt.vectorD().cwiseSqrt().asDiagonal() *
Eigen::Matrix<double, States, States>{Q_ldlt.matrixL().transpose()};
(Q_ldlt.transpositionsP().transpose() *
Eigen::Matrix<double, States, States>{Q_ldlt.matrixL()} *
Q_ldlt.vectorD().cwiseSqrt().asDiagonal())
.transpose();
if (!IsDetectable<States, States>(A, C)) {
std::string msg = fmt::format(