[wpimath] Print uncontrollable/unobservable models in LQR and KF (#3694)

IsDetectable() was added to make the code easier to read.

wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java

@@ -87,9 +87,9 @@ public final class StateSpaceUtil {
   /**
    * Returns true if (A, B) is a stabilizable pair.
    *
-   * <p>(A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
+   * <p>(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
    * absolute values less than one, where an eigenvalue is uncontrollable if rank(λI - A, B) %3C n
-   * where n is number of states.
+   * where n is the number of states.
    *
    * @param <States> Num representing the size of A.
    * @param <Inputs> Num representing the columns of B.
@@ -103,6 +103,26 @@ public final class StateSpaceUtil {
     return WPIMathJNI.isStabilizable(A.getNumRows(), B.getNumCols(), A.getData(), B.getData());
   }
 
+  /**
+   * Returns true if (A, C) is a detectable pair.
+   *
+   * <p>(A, C) is detectable if and only if the unobservable eigenvalues of A, if any, have absolute
+   * values less than one, where an eigenvalue is unobservable if rank(λI - A; C) %3C n where n is
+   * the number of states.
+   *
+   * @param <States> Num representing the size of A.
+   * @param <Outputs> Num representing the rows of C.
+   * @param A System matrix.
+   * @param C Output matrix.
+   * @return If the system is detectable.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  public static <States extends Num, Outputs extends Num> boolean isDetectable(
+      Matrix<States, States> A, Matrix<Outputs, States> C) {
+    return WPIMathJNI.isStabilizable(
+        A.getNumRows(), C.getNumRows(), A.transpose().getData(), C.transpose().getData());
+  }
+
   /**
    * Convert a {@link Pose2d} to a vector of [x, y, theta], where theta is in radians.
    *

wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java

@@ -79,9 +79,9 @@ public final class WPIMathJNI {
   /**
    * Returns true if (A, B) is a stabilizable pair.
    *
-   * <p>(A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
+   * <p>(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
    * absolute values less than one, where an eigenvalue is uncontrollable if rank(lambda * I - A, B)
-   * &lt; n where n is number of states.
+   * &lt; n where n is the number of states.
    *
    * @param states the number of states of the system.
    * @param inputs the number of inputs to the system.

wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java

@@ -5,6 +5,7 @@
 package edu.wpi.first.math.controller;
 
 import edu.wpi.first.math.Drake;
+import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.Num;
@@ -90,7 +91,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
    * @param R The input cost matrix.
    * @param dtSeconds Discretization timestep.
    */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  @SuppressWarnings({"LocalVariableName", "ParameterName"})
   public LinearQuadraticRegulator(
       Matrix<States, States> A,
       Matrix<States, Inputs> B,
@@ -101,6 +102,18 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
     var discA = discABPair.getFirst();
     var discB = discABPair.getSecond();
 
+    if (!StateSpaceUtil.isStabilizable(discA, discB)) {
+      var builder = new StringBuilder("The system passed to the LQR is uncontrollable!\n\nA =\n");
+      builder.append(discA.getStorage().toString());
+      builder.append("\nB =\n");
+      builder.append(discB.getStorage().toString());
+      builder.append("\n");
+
+      var msg = builder.toString();
+      MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
+      throw new IllegalArgumentException(msg);
+    }
+
     var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R);
 
     // K = (BᵀSB + R)⁻¹BᵀSA

wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java

@@ -141,9 +141,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
 
     final var discR = Discretization.discretizeR(m_contR, dtSeconds);
 
-    // IsStabilizable(Aᵀ, Cᵀ) will tell us if the system is observable.
-    boolean isObservable = StateSpaceUtil.isStabilizable(discA.transpose(), C.transpose());
-    if (isObservable && outputs.getNum() <= states.getNum()) {
+    if (StateSpaceUtil.isDetectable(discA, C) && outputs.getNum() <= states.getNum()) {
       m_initP =
           Drake.discreteAlgebraicRiccatiEquation(discA.transpose(), C.transpose(), discQ, discR);
     } else {

wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java

@@ -76,14 +76,17 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
 
     var C = plant.getC();
 
-    // isStabilizable(Aᵀ, Cᵀ) will tell us if the system is observable.
-    var isObservable = StateSpaceUtil.isStabilizable(discA.transpose(), C.transpose());
-    if (!isObservable) {
-      MathSharedStore.reportError(
-          "The system passed to the Kalman filter is not observable!",
-          Thread.currentThread().getStackTrace());
-      throw new IllegalArgumentException(
-          "The system passed to the Kalman filter is not observable!");
+    if (!StateSpaceUtil.isDetectable(discA, C)) {
+      var builder =
+          new StringBuilder("The system passed to the Kalman filter is unobservable!\n\nA =\n");
+      builder.append(discA.getStorage().toString());
+      builder.append("\nC =\n");
+      builder.append(C.getStorage().toString());
+      builder.append("\n");
+
+      var msg = builder.toString();
+      MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
+      throw new IllegalArgumentException(msg);
     }
 
     var P =

wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp

@@ -18,25 +18,33 @@
 
 using namespace wpi::java;
 
+/**
+ * Returns true if (A, B) is a stabilizable pair.
+ *
+ * (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
+ * any, have absolute values less than one, where an eigenvalue is
+ * uncontrollable if rank(λI - A, B) < n where n is the number of states.
+ *
+ * @param A System matrix.
+ * @param B Input matrix.
+ */
 bool check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
                         const Eigen::Ref<const Eigen::MatrixXd>& B) {
-  // This function checks if (A,B) is a stabilizable pair.
-  // (A,B) is stabilizable if and only if the uncontrollable eigenvalues of
-  // A, if any, have absolute values less than one, where an eigenvalue is
-  // uncontrollable if Rank[lambda * I - A, B] < n.
-  int n = B.rows(), m = B.cols();
-  Eigen::EigenSolver<Eigen::MatrixXd> es(A);
-  for (int i = 0; i < n; i++) {
+  int states = B.rows();
+  int inputs = B.cols();
+  Eigen::EigenSolver<Eigen::MatrixXd> es{A};
+  for (int i = 0; i < states; ++i) {
     if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
             es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
         1) {
       continue;
     }
 
-    Eigen::MatrixXcd E(n, n + m);
-    E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(n, n) - A, B;
-    Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr(E);
-    if (qr.rank() != n) {
+    Eigen::MatrixXcd E{states, states + inputs};
+    E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(states, states) - A,
+        B;
+    Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
+    if (qr.rank() < states) {
       return false;
     }
   }
@@ -196,7 +204,7 @@ Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
       Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
       B{nativeB, states, inputs};
 
-  bool isStabilizable = check_stabilizable(A, B);  // NOLINT
+  bool isStabilizable = check_stabilizable(A, B);
 
   env->ReleaseDoubleArrayElements(aSrc, nativeA, 0);
   env->ReleaseDoubleArrayElements(bSrc, nativeB, 0);

wpimath/src/main/native/include/frc/StateSpaceUtil.h

@@ -78,7 +78,7 @@ bool IsStabilizableImpl(const Eigen::Matrix<double, States, States>& A,
 
     Eigen::ColPivHouseholderQR<
         Eigen::Matrix<std::complex<double>, States, States + Inputs>>
-        qr(E);
+        qr{E};
     if (qr.rank() < States) {
       return false;
     }
@@ -258,9 +258,9 @@ Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose);
 /**
  * Returns true if (A, B) is a stabilizable pair.
  *
- * (A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
+ * (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
  * any, have absolute values less than one, where an eigenvalue is
- * uncontrollable if rank(λI - A, B) < n where n is number of states.
+ * uncontrollable if rank(λI - A, B) < n where n is the number of states.
  *
  * @param A System matrix.
  * @param B Input matrix.
@@ -271,6 +271,23 @@ bool IsStabilizable(const Eigen::Matrix<double, States, States>& A,
   return detail::IsStabilizableImpl<States, Inputs>(A, B);
 }
 
+/**
+ * Returns true if (A, C) is a detectable pair.
+ *
+ * (A, C) is detectable if and only if the unobservable eigenvalues of A, if
+ * any, have absolute values less than one, where an eigenvalue is unobservable
+ * if rank(λI - A; C) < n where n is the number of states.
+ *
+ * @param A System matrix.
+ * @param C Output matrix.
+ */
+template <int States, int Outputs>
+bool IsDetectable(const Eigen::Matrix<double, States, States>& A,
+                  const Eigen::Matrix<double, Outputs, States>& C) {
+  return detail::IsStabilizableImpl<States, Outputs>(A.transpose(),
+                                                     C.transpose());
+}
+
 // Template specializations are used here to make common state-input pairs
 // compile faster.
 template <>

wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h

@@ -4,6 +4,10 @@
 
 #pragma once
 
+#include <frc/fmt/Eigen.h>
+
+#include <string>
+
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
 
@@ -16,6 +20,7 @@
 #include "frc/system/LinearSystem.h"
 #include "units/time.h"
 #include "unsupported/Eigen/MatrixFunctions"
+#include "wpimath/MathShared.h"
 
 namespace frc {
 namespace detail {
@@ -82,6 +87,16 @@ class LinearQuadraticRegulatorImpl {
     Eigen::Matrix<double, States, Inputs> discB;
     DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
 
+    if (!IsStabilizable<States, Inputs>(discA, discB)) {
+      std::string msg = fmt::format(
+          "The system passed to the LQR is uncontrollable!\n\nA =\n{}\nB "
+          "=\n{}\n",
+          discA, discB);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    }
+
     Eigen::Matrix<double, States, States> S =
         drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
 

wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h

@@ -71,10 +71,7 @@ class ExtendedKalmanFilter {
     Eigen::Matrix<double, Outputs, Outputs> discR =
         DiscretizeR<Outputs>(m_contR, dt);
 
-    // IsStabilizable(Aᵀ, Cᵀ) will tell us if the system is observable.
-    bool isObservable =
-        IsStabilizable<States, Outputs>(discA.transpose(), C.transpose());
-    if (isObservable && Outputs <= States) {
+    if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
       m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
           discA.transpose(), C.transpose(), discQ, discR);
     } else {
@@ -137,10 +134,7 @@ class ExtendedKalmanFilter {
     Eigen::Matrix<double, Outputs, Outputs> discR =
         DiscretizeR<Outputs>(m_contR, dt);
 
-    // IsStabilizable(Aᵀ, Cᵀ) will tell us if the system is observable.
-    bool isObservable =
-        IsStabilizable<States, Outputs>(discA.transpose(), C.transpose());
-    if (isObservable && Outputs <= States) {
+    if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
       m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
           discA.transpose(), C.transpose(), discQ, discR);
     } else {

wpimath/src/main/native/include/frc/estimator/KalmanFilter.h

@@ -4,7 +4,10 @@
 
 #pragma once
 
+#include <frc/fmt/Eigen.h>
+
 #include <cmath>
+#include <string>
 
 #include <wpi/SymbolExports.h>
 #include <wpi/array.h>
@@ -65,14 +68,14 @@ class KalmanFilterImpl {
 
     const auto& C = plant.C();
 
-    // IsStabilizable(Aᵀ, Cᵀ) will tell us if the system is observable.
-    bool isObservable =
-        IsStabilizable<States, Outputs>(discA.transpose(), C.transpose());
-    if (!isObservable) {
-      wpi::math::MathSharedStore::ReportError(
-          "The system passed to the Kalman filter is not observable!");
-      throw std::invalid_argument(
-          "The system passed to the Kalman filter is not observable!");
+    if (!IsDetectable<States, Outputs>(discA, C)) {
+      std::string msg = fmt::format(
+          "The system passed to the Kalman filter is "
+          "unobservable!\n\nA =\n{}\nC =\n{}\n",
+          discA, C);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
     }
 
     Eigen::Matrix<double, States, States> P =

wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java

@@ -76,6 +76,33 @@ public class StateSpaceUtilTest {
     assertTrue(StateSpaceUtil.isStabilizable(A, B));
   }
 
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testIsDetectable() {
+    Matrix<N2, N2> A;
+    Matrix<N1, N2> C = Matrix.mat(Nat.N1(), Nat.N2()).fill(0, 1);
+
+    // First eigenvalue is unobservable and unstable.
+    // Second eigenvalue is observable and stable.
+    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.2, 0, 0, 0.5);
+    assertFalse(StateSpaceUtil.isDetectable(A, C));
+
+    // First eigenvalue is unobservable and marginally stable.
+    // Second eigenvalue is observable and stable.
+    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.5);
+    assertFalse(StateSpaceUtil.isDetectable(A, C));
+
+    // First eigenvalue is unobservable and stable.
+    // Second eigenvalue is observable and stable.
+    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 0.5);
+    assertTrue(StateSpaceUtil.isDetectable(A, C));
+
+    // First eigenvalue is unobservable and stable.
+    // Second eigenvalue is observable and unstable.
+    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 1.2);
+    assertTrue(StateSpaceUtil.isDetectable(A, C));
+  }
+
   @Test
   public void testMakeWhiteNoiseVector() {
     var firstData = new ArrayList<Double>();

wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp

@@ -134,3 +134,27 @@ TEST(StateSpaceUtilTest, IsStabilizable) {
   EXPECT_TRUE((frc::IsStabilizable<2, 1>(
       Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 1.2}}, B)));
 }
+
+TEST(StateSpaceUtilTest, IsDetectable) {
+  Eigen::Matrix<double, 1, 2> C{0, 1};
+
+  // First eigenvalue is unobservable and unstable.
+  // Second eigenvalue is observable and stable.
+  EXPECT_FALSE((frc::IsDetectable<2, 1>(
+      Eigen::Matrix<double, 2, 2>{{1.2, 0}, {0, 0.5}}, C)));
+
+  // First eigenvalue is unobservable and marginally stable.
+  // Second eigenvalue is observable and stable.
+  EXPECT_FALSE((frc::IsDetectable<2, 1>(
+      Eigen::Matrix<double, 2, 2>{{1, 0}, {0, 0.5}}, C)));
+
+  // First eigenvalue is unobservable and stable.
+  // Second eigenvalue is observable and stable.
+  EXPECT_TRUE((frc::IsDetectable<2, 1>(
+      Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 0.5}}, C)));
+
+  // First eigenvalue is unobservable and stable.
+  // Second eigenvalue is observable and unstable.
+  EXPECT_TRUE((frc::IsDetectable<2, 1>(
+      Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 1.2}}, C)));
+}