From 814f18c7f5b2fc0e9bfd797db7afefc533441223 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Mon, 4 Sep 2023 21:12:25 -0700
Subject: [PATCH] [wpimath] Fix computation of C for DARE (A, C) detectability
 check (#5609)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

If Q has off-diagonal entries, C and Cᵀ are different.
---
 wpimath/src/main/native/include/frc/DARE.h | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

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<double, States, States>& A,
   }
 
   // Require (A, C) pair be detectable where Q = CᵀC
+  //
+  // Q = CᵀC = LDLᵀ
+  // C = √(D)Lᵀ
   {
     Eigen::Matrix<double, States, States> C =
-        Eigen::Matrix<double, States, States>{Q_ldlt.matrixL()} *
-        Q_ldlt.vectorD().cwiseSqrt().asDiagonal();
+        Q_ldlt.vectorD().cwiseSqrt().asDiagonal() *
+        Eigen::Matrix<double, States, States>{Q_ldlt.matrixL().transpose()};
 
     if (!IsDetectable<States, States>(A, C)) {
       std::string msg = fmt::format(