diff --git a/wpimath/src/main/native/include/frc/DARE.h b/wpimath/src/main/native/include/frc/DARE.h index 6a3104e5c1..4681d3c825 100644 --- a/wpimath/src/main/native/include/frc/DARE.h +++ b/wpimath/src/main/native/include/frc/DARE.h @@ -75,14 +75,12 @@ void CheckDARE_ABQ(const Eigen::Matrix& 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 C = - (Q_ldlt.transpositionsP().transpose() * - Eigen::Matrix{Q_ldlt.matrixL()} * - Q_ldlt.vectorD().cwiseSqrt().asDiagonal()) - .transpose(); + Q_ldlt.vectorD().cwiseSqrt().asDiagonal() * + Eigen::Matrix{Q_ldlt.matrixL().transpose()} * + Q_ldlt.transpositionsP(); if (!IsDetectable(A, C)) { std::string msg = fmt::format(