[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 =