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

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

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 =