[wpimath] Fix DARE Q decomposition (#5611)

wpilibsuite · Sep 5, 2023 · 1a6df6f · 1a6df6f
1 parent 9b3f7fb
commit 1a6df6f
Show file tree

Hide file tree

Showing 5 changed files with 49 additions and 8 deletions.
diff --git a/wpimath/src/main/native/include/frc/DARE.h b/wpimath/src/main/native/include/frc/DARE.h
@@ -74,12 +74,15 @@ 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ᵀ
+  // Q = CᵀC = PᵀLDLᵀP
+  // Cᵀ = PᵀL√(D)
+  // C = (PᵀL√(D))ᵀ
   {
     Eigen::Matrix<double, States, States> C =
-        Q_ldlt.vectorD().cwiseSqrt().asDiagonal() *
-        Eigen::Matrix<double, States, States>{Q_ldlt.matrixL().transpose()};
+        (Q_ldlt.transpositionsP().transpose() *
+         Eigen::Matrix<double, States, States>{Q_ldlt.matrixL()} *
+         Q_ldlt.vectorD().cwiseSqrt().asDiagonal())
+            .transpose();
 
     if (!IsDetectable<States, States>(A, C)) {
       std::string msg = fmt::format(

diff --git a/wpimath/src/test/java/edu/wpi/first/math/DARETest.java b/wpimath/src/test/java/edu/wpi/first/math/DARETest.java
@@ -4,6 +4,7 @@
 
 package edu.wpi.first.math;
 
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertThrows;
 
@@ -312,4 +313,23 @@ void testACNotDetectable_ABQRN() {
 
     assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
   }
+
+  @Test
+  void testQDecomposition() {
+    // Ensures the decomposition of Q into CᵀC is correct
+
+    var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1.0, 0.0, 0.0, 0.0});
+    var B = Matrix.eye(Nat.N2());
+    var R = Matrix.eye(Nat.N2());
+
+    // (A, C₁) should be detectable pair
+    var C_1 = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0.0, 0.0, 1.0, 0.0});
+    var Q_1 = C_1.transpose().times(C_1);
+    assertDoesNotThrow(() -> DARE.dare(A, B, Q_1, R));
+
+    // (A, C₂) shouldn't be detectable pair
+    var C_2 = C_1.transpose();
+    var Q_2 = C_2.transpose().times(C_2);
+    assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q_2, R));
+  }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
@@ -148,10 +148,10 @@ void testMatrixOverloadsWithDoubleIntegrator() {
     assertEquals(0.51182128351092726, K.get(0, 1), 1e-10);
 
     // QRN overload
-    var Aref = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / (Ka * 2.0));
+    var Aref = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / (Ka * 5.0));
     var Kimf = getImplicitModelFollowingK(A, B, Q, R, Aref, 0.005);
     assertEquals(0.0, Kimf.get(0, 0), 1e-10);
-    assertEquals(-5.367540084534802e-05, Kimf.get(0, 1), 1e-10);
+    assertEquals(-6.9190500116751458e-05, Kimf.get(0, 1), 1e-10);
   }
 
   @Test

diff --git a/wpimath/src/test/native/cpp/DARETest.cpp b/wpimath/src/test/native/cpp/DARETest.cpp
@@ -279,3 +279,21 @@ TEST(DARETest, ACNotDetectable_ABQRN) {
 
   EXPECT_THROW((frc::DARE<2, 2>(A, B, Q, R, N)), std::invalid_argument);
 }
+
+TEST(DARETest, QDecomposition) {
+  // Ensures the decomposition of Q into CᵀC is correct
+
+  const Eigen::Matrix2d A{{1.0, 0.0}, {0.0, 0.0}};
+  const Eigen::Matrix2d B{Eigen::Matrix2d::Identity()};
+  const Eigen::Matrix2d R{Eigen::Matrix2d::Identity()};
+
+  // (A, C₁) should be detectable pair
+  const Eigen::Matrix2d C_1{{0.0, 0.0}, {1.0, 0.0}};
+  const Eigen::Matrix2d Q_1 = C_1.transpose() * C_1;
+  EXPECT_NO_THROW((frc::DARE<2, 2>(A, B, Q_1, R)));
+
+  // (A, C₂) shouldn't be detectable pair
+  const Eigen::Matrix2d C_2 = C_1.transpose();
+  const Eigen::Matrix2d Q_2 = C_2.transpose() * C_2;
+  EXPECT_THROW((frc::DARE<2, 2>(A, B, Q_2, R)), std::invalid_argument);
+}
diff --git a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
@@ -158,10 +158,10 @@ TEST(LinearQuadraticRegulatorTest, MatrixOverloadsWithDoubleIntegrator) {
   EXPECT_NEAR(0.51182128351092726, K(0, 1), 1e-10);
 
   // QRN overload
-  Matrixd<2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 2.0)}};
+  Matrixd<2, 2> Aref{{0, 1}, {0, -Kv / (Ka * 5.0)}};
   Matrixd<1, 2> Kimf = GetImplicitModelFollowingK<2, 1>(A, B, Q, R, Aref, 5_ms);
   EXPECT_NEAR(0.0, Kimf(0, 0), 1e-10);
-  EXPECT_NEAR(-5.367540084534802e-05, Kimf(0, 1), 1e-10);
+  EXPECT_NEAR(-6.9190500116751458e-05, Kimf(0, 1), 1e-10);
 }
 
 TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {