diff --git a/wpimath/src/main/native/include/frc/DARE.h b/wpimath/src/main/native/include/frc/DARE.h index 512941982a..97244d7332 100644 --- a/wpimath/src/main/native/include/frc/DARE.h +++ b/wpimath/src/main/native/include/frc/DARE.h @@ -73,10 +73,13 @@ void CheckDARE_ABQ(const Eigen::Matrix& A, } // Require (A, C) pair be detectable where Q = CᵀC + // + // Q = CᵀC = LDLᵀ + // C = √(D)Lᵀ { Eigen::Matrix C = - Eigen::Matrix{Q_ldlt.matrixL()} * - Q_ldlt.vectorD().cwiseSqrt().asDiagonal(); + Q_ldlt.vectorD().cwiseSqrt().asDiagonal() * + Eigen::Matrix{Q_ldlt.matrixL().transpose()}; if (!IsDetectable(A, C)) { std::string msg = fmt::format(