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

[wpimath] Simplify calculation of C for DARE precondition (#6022)

Browse Source
This commit is contained in:
Tyler Veness
2023-12-08 20:25:03 -08:00
committed by GitHub
parent 5200316c14
commit 7d4e515a6b
1 changed files with 4 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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