From 7d4e515a6b5bc71caa1cfba0735afe6270fa4b00 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Fri, 8 Dec 2023 20:25:03 -0800
Subject: [PATCH] [wpimath] Simplify calculation of C for DARE precondition
 (#6022)

---
 wpimath/src/main/native/include/frc/DARE.h | 10 ++++------
 1 file changed, 4 insertions(+), 6 deletions(-)

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<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(