[wpimath] Refactor StateSpaceUtil into separate files (#8421)

* Moved makeWhiteNoiseVector() to random.Normal.normal()
* Moved isControllable() and isDetectable() to system.LinearSystemUtil
* Renamed makeCostMatrix() to costMatrix() (Java)
* Renamed makeCovarianceMatrix() to covarianceMatrix() (Java)
* Renamed MakeCostMatrix() to CostMatrix() (C++)
* Renamed MakeCovMatrix() to CovarianceMatrix() (C++)
* Removed deprecated poseTo3dVector(), poseTo4dVector(), poseToVector()
* Removed clampInputMaxMagnitude()
* We don't use it, and Eigen has this functionality built in via `u =
u.array().min(u_max.array()).max(u_min.array());`
* Simplified implementation of desaturateInputVector()

wpimath/src/main/native/include/wpi/math/controller/LTVDifferentialDriveController.hpp

@@ -62,8 +62,8 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController {
       : m_trackwidth{trackwidth},
         m_A{plant.A()},
         m_B{plant.B()},
-        m_Q{wpi::math::MakeCostMatrix(Qelems)},
-        m_R{wpi::math::MakeCostMatrix(Relems)},
+        m_Q{wpi::math::CostMatrix(Qelems)},
+        m_R{wpi::math::CostMatrix(Relems)},
         m_dt{dt} {}
 
   /**

wpimath/src/main/native/include/wpi/math/controller/LTVUnicycleController.hpp

@@ -56,8 +56,8 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
   LTVUnicycleController(const wpi::util::array<double, 3>& Qelems,
                         const wpi::util::array<double, 2>& Relems,
                         wpi::units::second_t dt)
-      : m_Q{wpi::math::MakeCostMatrix(Qelems)},
-        m_R{wpi::math::MakeCostMatrix(Relems)},
+      : m_Q{wpi::math::CostMatrix(Qelems)},
+        m_R{wpi::math::CostMatrix(Relems)},
         m_dt{dt} {}
 
   /**

wpimath/src/main/native/include/wpi/math/controller/LinearQuadraticRegulator.hpp

@@ -81,8 +81,8 @@ class LinearQuadraticRegulator {
                            const Matrixd<States, Inputs>& B,
                            const StateArray& Qelems, const InputArray& Relems,
                            wpi::units::second_t dt)
-      : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
-                                 MakeCostMatrix(Relems), dt) {}
+      : LinearQuadraticRegulator(A, B, CostMatrix(Qelems), CostMatrix(Relems),
+                                 dt) {}
 
   /**
    * Constructs a controller with the given coefficients and plant.

wpimath/src/main/native/include/wpi/math/estimator/ExtendedKalmanFilter.hpp

@@ -16,6 +16,7 @@
 #include "wpi/math/system/Discretization.hpp"
 #include "wpi/math/system/NumericalIntegration.hpp"
 #include "wpi/math/system/NumericalJacobian.hpp"
+#include "wpi/math/util/MathShared.hpp"
 #include "wpi/math/util/StateSpaceUtil.hpp"
 #include "wpi/units/time.hpp"
 #include "wpi/util/array.hpp"
@@ -79,8 +80,8 @@ class ExtendedKalmanFilter {
       const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
       wpi::units::second_t dt)
       : m_f(std::move(f)), m_h(std::move(h)) {
-    m_contQ = MakeCovMatrix(stateStdDevs);
-    m_contR = MakeCovMatrix(measurementStdDevs);
+    m_contQ = CovarianceMatrix(stateStdDevs);
+    m_contR = CovarianceMatrix(measurementStdDevs);
     m_residualFuncY = [](const OutputVector& a,
                          const OutputVector& b) -> OutputVector {
       return a - b;
@@ -170,8 +171,8 @@ class ExtendedKalmanFilter {
         m_h(std::move(h)),
         m_residualFuncY(std::move(residualFuncY)),
         m_addFuncX(std::move(addFuncX)) {
-    m_contQ = MakeCovMatrix(stateStdDevs);
-    m_contR = MakeCovMatrix(measurementStdDevs);
+    m_contQ = CovarianceMatrix(stateStdDevs);
+    m_contR = CovarianceMatrix(measurementStdDevs);
     m_dt = dt;
 
     StateMatrix contA = NumericalJacobianX<States, States, Inputs>(

wpimath/src/main/native/include/wpi/math/estimator/KalmanFilter.hpp

@@ -72,8 +72,8 @@ class KalmanFilter {
                const OutputArray& measurementStdDevs, wpi::units::second_t dt) {
     m_plant = &plant;
 
-    m_contQ = MakeCovMatrix(stateStdDevs);
-    m_contR = MakeCovMatrix(measurementStdDevs);
+    m_contQ = CovarianceMatrix(stateStdDevs);
+    m_contR = CovarianceMatrix(measurementStdDevs);
     m_dt = dt;
 
     // Find discrete A and Q

wpimath/src/main/native/include/wpi/math/estimator/SteadyStateKalmanFilter.hpp

@@ -76,8 +76,8 @@ class SteadyStateKalmanFilter {
                           wpi::units::second_t dt) {
     m_plant = &plant;
 
-    auto contQ = MakeCovMatrix(stateStdDevs);
-    auto contR = MakeCovMatrix(measurementStdDevs);
+    auto contQ = CovarianceMatrix(stateStdDevs);
+    auto contR = CovarianceMatrix(measurementStdDevs);
 
     Matrixd<States, States> discA;
     Matrixd<States, States> discQ;

wpimath/src/main/native/include/wpi/math/estimator/UnscentedKalmanFilter.hpp

@@ -93,8 +93,8 @@ class UnscentedKalmanFilter {
       const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
       wpi::units::second_t dt)
       : m_f(std::move(f)), m_h(std::move(h)) {
-    m_contQ = MakeCovMatrix(stateStdDevs);
-    m_contR = MakeCovMatrix(measurementStdDevs);
+    m_contQ = CovarianceMatrix(stateStdDevs);
+    m_contR = CovarianceMatrix(measurementStdDevs);
     m_meanFuncX = [](const Matrixd<States, NumSigmas>& sigmas,
                      const Vectord<NumSigmas>& Wm) -> StateVector {
       return sigmas * Wm;
@@ -169,8 +169,8 @@ class UnscentedKalmanFilter {
         m_residualFuncX(std::move(residualFuncX)),
         m_residualFuncY(std::move(residualFuncY)),
         m_addFuncX(std::move(addFuncX)) {
-    m_contQ = MakeCovMatrix(stateStdDevs);
-    m_contR = MakeCovMatrix(measurementStdDevs);
+    m_contQ = CovarianceMatrix(stateStdDevs);
+    m_contR = CovarianceMatrix(measurementStdDevs);
     m_dt = dt;
 
     Reset();

wpimath/src/main/native/include/wpi/math/linalg/DARE.hpp

@@ -10,7 +10,7 @@
 #include <Eigen/Core>
 #include <Eigen/LU>
 
-#include "wpi/math/util/StateSpaceUtil.hpp"
+#include "wpi/math/system/LinearSystemUtil.hpp"
 #include "wpi/util/expected"
 
 namespace wpi::math {

wpimath/src/main/native/include/wpi/math/random/Normal.hpp

@@ -0,0 +1,85 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <array>
+#include <concepts>
+#include <random>
+#include <span>
+
+#include <Eigen/Core>
+
+#include "wpi/util/Algorithm.hpp"
+#include "wpi/util/SymbolExports.hpp"
+
+namespace wpi::math {
+
+/**
+ * Creates a vector of normally distributed random values with the given
+ * standard deviations for each element.
+ *
+ * @param stdDevs An array whose elements are the standard deviations of each
+ *     element of the random vector.
+ * @return Vector of normally distributed values.
+ */
+template <std::same_as<double>... Ts>
+Eigen::Vector<double, sizeof...(Ts)> Normal(Ts... stdDevs) {
+  std::random_device rd;
+  std::mt19937 gen{rd()};
+
+  Eigen::Vector<double, sizeof...(Ts)> result;
+  wpi::util::for_each(
+      [&](int i, double stdDev) {
+        // Passing a standard deviation of 0.0 to std::normal_distribution is
+        // undefined behavior
+        if (stdDev == 0.0) {
+          result(i) = 0.0;
+        } else {
+          std::normal_distribution distr{0.0, stdDev};
+          result(i) = distr(gen);
+        }
+      },
+      stdDevs...);
+  return result;
+}
+
+/**
+ * Creates a vector of normally distributed random values with the given
+ * standard deviations for each element.
+ *
+ * @param stdDevs An array whose elements are the standard deviations of each
+ *     element of the random vector.
+ * @return Vector of normally distributed values.
+ */
+template <int N>
+Eigen::Vector<double, N> Normal(const std::array<double, N>& stdDevs) {
+  std::random_device rd;
+  std::mt19937 gen{rd()};
+
+  Eigen::Vector<double, N> result;
+  for (size_t i = 0; i < stdDevs.size(); ++i) {
+    // Passing a standard deviation of 0.0 to std::normal_distribution is
+    // undefined behavior
+    if (stdDevs[i] == 0.0) {
+      result(i) = 0.0;
+    } else {
+      std::normal_distribution distr{0.0, stdDevs[i]};
+      result(i) = distr(gen);
+    }
+  }
+  return result;
+}
+
+/**
+ * Creates a vector of normally distributed random values with the given
+ * standard deviations for each element.
+ *
+ * @param stdDevs A possibly variable length container whose elements are the
+ *     standard deviations of each element of the random vector.
+ * @return Vector of normally distributed values.
+ */
+WPILIB_DLLEXPORT Eigen::VectorXd Normal(const std::span<const double> stdDevs);
+
+}  // namespace wpi::math

wpimath/src/main/native/include/wpi/math/system/LinearSystemUtil.hpp

@@ -0,0 +1,98 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <complex>
+
+#include <Eigen/Core>
+#include <Eigen/Eigenvalues>
+#include <Eigen/QR>
+
+#include "wpi/util/SymbolExports.hpp"
+
+namespace wpi::math {
+
+/**
+ * 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.
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @param A System matrix.
+ * @param B Input matrix.
+ */
+template <int States, int Inputs>
+bool IsStabilizable(const Eigen::Matrix<double, States, States>& A,
+                    const Eigen::Matrix<double, States, Inputs>& B) {
+  Eigen::EigenSolver<Eigen::Matrix<double, States, States>> es{A, false};
+
+  for (int i = 0; i < A.rows(); ++i) {
+    if (std::norm(es.eigenvalues()[i]) < 1) {
+      continue;
+    }
+
+    if constexpr (States != Eigen::Dynamic && Inputs != Eigen::Dynamic) {
+      Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
+      E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
+                                               States>::Identity() -
+               A,
+          B;
+
+      Eigen::ColPivHouseholderQR<
+          Eigen::Matrix<std::complex<double>, States, States + Inputs>>
+          qr{E};
+      if (qr.rank() < States) {
+        return false;
+      }
+    } else {
+      Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()};
+      E << es.eigenvalues()[i] *
+                   Eigen::MatrixXcd::Identity(A.rows(), A.rows()) -
+               A,
+          B;
+
+      Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
+      if (qr.rank() < A.rows()) {
+        return false;
+      }
+    }
+  }
+  return true;
+}
+
+extern template WPILIB_DLLEXPORT bool IsStabilizable<1, 1>(
+    const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B);
+extern template WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B);
+extern template WPILIB_DLLEXPORT bool
+IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(const Eigen::MatrixXd& A,
+                                               const Eigen::MatrixXd& 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.
+ *
+ * @tparam States Number of states.
+ * @tparam Outputs Number of outputs.
+ * @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 IsStabilizable<States, Outputs>(A.transpose(), C.transpose());
+}
+
+extern template WPILIB_DLLEXPORT bool
+IsDetectable<Eigen::Dynamic, Eigen::Dynamic>(const Eigen::MatrixXd& A,
+                                             const Eigen::MatrixXd& C);
+
+}  // namespace wpi::math

wpimath/src/main/native/include/wpi/math/util/StateSpaceUtil.hpp

@@ -4,17 +4,14 @@
 
 #pragma once
 
+#include <algorithm>
 #include <array>
-#include <cmath>
 #include <concepts>
 #include <limits>
-#include <random>
+#include <span>
 
-#include <Eigen/Eigenvalues>
-#include <Eigen/QR>
+#include <Eigen/Core>
 
-#include "wpi/math/geometry/Pose2d.hpp"
-#include "wpi/math/linalg/EigenCore.hpp"
 #include "wpi/util/Algorithm.hpp"
 #include "wpi/util/SymbolExports.hpp"
 
@@ -28,15 +25,15 @@ namespace wpi::math {
  * infinity, its cost matrix entry is set to zero.
  *
  * @param tolerances An array. For a Q matrix, its elements are the maximum
- *                   allowed excursions of the states from the reference. For an
- *                   R matrix, its elements are the maximum allowed excursions
- *                   of the control inputs from no actuation.
+ *     allowed excursions of the states from the reference. For an R matrix, its
+ *     elements are the maximum allowed excursions of the control inputs from no
+ *     actuation.
  * @return State excursion or control effort cost matrix.
  */
 template <std::same_as<double>... Ts>
-constexpr Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(
+constexpr Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> CostMatrix(
     Ts... tolerances) {
-  Matrixd<sizeof...(Ts), sizeof...(Ts)> result;
+  Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> result;
 
   for (int row = 0; row < result.rows(); ++row) {
     for (int col = 0; col < result.cols(); ++col) {
@@ -59,37 +56,6 @@ constexpr Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(
   return result;
 }
 
-/**
- * Creates a covariance matrix from the given vector for use with Kalman
- * filters.
- *
- * Each element is squared and placed on the covariance matrix diagonal.
- *
- * @param stdDevs An array. For a Q matrix, its elements are the standard
- *                deviations of each state from how the model behaves. For an R
- *                matrix, its elements are the standard deviations for each
- *                output measurement.
- * @return Process noise or measurement noise covariance matrix.
- */
-template <std::same_as<double>... Ts>
-constexpr Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
-  Matrixd<sizeof...(Ts), sizeof...(Ts)> result;
-
-  for (int row = 0; row < result.rows(); ++row) {
-    for (int col = 0; col < result.cols(); ++col) {
-      if (row != col) {
-        result(row, col) = 0.0;
-      }
-    }
-  }
-
-  wpi::util::for_each(
-      [&](int i, double stdDev) { result(i, i) = stdDev * stdDev; },
-      stdDevs...);
-
-  return result;
-}
-
 /**
  * Creates a cost matrix from the given vector for use with LQR.
  *
@@ -98,14 +64,15 @@ constexpr Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
  * tolerance is infinity, its cost matrix entry is set to zero.
  *
  * @param costs An array. For a Q matrix, its elements are the maximum allowed
- *              excursions of the states from the reference. For an R matrix,
- *              its elements are the maximum allowed excursions of the control
- *              inputs from no actuation.
+ *     excursions of the states from the reference. For an R matrix, its
+ *     elements are the maximum allowed excursions of the control inputs from no
+ *     actuation.
  * @return State excursion or control effort cost matrix.
  */
 template <size_t N>
-constexpr Matrixd<N, N> MakeCostMatrix(const std::array<double, N>& costs) {
-  Matrixd<N, N> result;
+constexpr Eigen::Matrix<double, N, N> CostMatrix(
+    const std::array<double, N>& costs) {
+  Eigen::Matrix<double, N, N> result;
 
   for (int row = 0; row < result.rows(); ++row) {
     for (int col = 0; col < result.cols(); ++col) {
@@ -132,12 +99,12 @@ constexpr Matrixd<N, N> MakeCostMatrix(const std::array<double, N>& costs) {
  * tolerance is infinity, its cost matrix entry is set to zero.
  *
  * @param costs A possibly variable length container. For a Q matrix, its
- *              elements are the maximum allowed excursions of the states from
- *              the reference. For an R matrix, its elements are the maximum
- *              allowed excursions of the control inputs from no actuation.
+ *     elements are the maximum allowed excursions of the states from the
+ *     reference. For an R matrix, its elements are the maximum allowed
+ *     excursions of the control inputs from no actuation.
  * @return State excursion or control effort cost matrix.
  */
-WPILIB_DLLEXPORT Eigen::MatrixXd MakeCostMatrix(
+WPILIB_DLLEXPORT Eigen::MatrixXd CostMatrix(
     const std::span<const double> costs);
 
 /**
@@ -147,14 +114,45 @@ WPILIB_DLLEXPORT Eigen::MatrixXd MakeCostMatrix(
  * Each element is squared and placed on the covariance matrix diagonal.
  *
  * @param stdDevs An array. For a Q matrix, its elements are the standard
- *                deviations of each state from how the model behaves. For an R
- *                matrix, its elements are the standard deviations for each
- *                output measurement.
+ *     deviations of each state from how the model behaves. For an R matrix, its
+ *     elements are the standard deviations for each output measurement.
+ * @return Process noise or measurement noise covariance matrix.
+ */
+template <std::same_as<double>... Ts>
+constexpr Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> CovarianceMatrix(
+    Ts... stdDevs) {
+  Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> result;
+
+  for (int row = 0; row < result.rows(); ++row) {
+    for (int col = 0; col < result.cols(); ++col) {
+      if (row != col) {
+        result(row, col) = 0.0;
+      }
+    }
+  }
+
+  wpi::util::for_each(
+      [&](int i, double stdDev) { result(i, i) = stdDev * stdDev; },
+      stdDevs...);
+
+  return result;
+}
+
+/**
+ * Creates a covariance matrix from the given vector for use with Kalman
+ * filters.
+ *
+ * Each element is squared and placed on the covariance matrix diagonal.
+ *
+ * @param stdDevs An array. For a Q matrix, its elements are the standard
+ *     deviations of each state from how the model behaves. For an R matrix, its
+ *     elements are the standard deviations for each output measurement.
  * @return Process noise or measurement noise covariance matrix.
  */
 template <size_t N>
-constexpr Matrixd<N, N> MakeCovMatrix(const std::array<double, N>& stdDevs) {
-  Matrixd<N, N> result;
+constexpr Eigen::Matrix<double, N, N> CovarianceMatrix(
+    const std::array<double, N>& stdDevs) {
+  Eigen::Matrix<double, N, N> result;
 
   for (int row = 0; row < result.rows(); ++row) {
     for (int col = 0; col < result.cols(); ++col) {
@@ -176,247 +174,27 @@ constexpr Matrixd<N, N> MakeCovMatrix(const std::array<double, N>& stdDevs) {
  * Each element is squared and placed on the covariance matrix diagonal.
  *
  * @param stdDevs A possibly variable length container. For a Q matrix, its
- *                elements are the standard deviations of each state from how
- *                the model behaves. For an R matrix, its elements are the
- *                standard deviations for each output measurement.
+ *     elements are the standard deviations of each state from how the model
+ *     behaves. For an R matrix, its elements are the standard deviations for
+ *     each output measurement.
  * @return Process noise or measurement noise covariance matrix.
  */
-WPILIB_DLLEXPORT Eigen::MatrixXd MakeCovMatrix(
+WPILIB_DLLEXPORT Eigen::MatrixXd CovarianceMatrix(
     const std::span<const double> stdDevs);
 
-template <std::same_as<double>... Ts>
-Vectord<sizeof...(Ts)> MakeWhiteNoiseVector(Ts... stdDevs) {
-  std::random_device rd;
-  std::mt19937 gen{rd()};
-
-  Vectord<sizeof...(Ts)> result;
-  wpi::util::for_each(
-      [&](int i, double stdDev) {
-        // Passing a standard deviation of 0.0 to std::normal_distribution is
-        // undefined behavior
-        if (stdDev == 0.0) {
-          result(i) = 0.0;
-        } else {
-          std::normal_distribution distr{0.0, stdDev};
-          result(i) = distr(gen);
-        }
-      },
-      stdDevs...);
-  return result;
-}
-
-/**
- * Creates a vector of normally distributed white noise with the given noise
- * intensities for each element.
- *
- * @param stdDevs An array whose elements are the standard deviations of each
- *                element of the noise vector.
- * @return White noise vector.
- */
-template <int N>
-Vectord<N> MakeWhiteNoiseVector(const std::array<double, N>& stdDevs) {
-  std::random_device rd;
-  std::mt19937 gen{rd()};
-
-  Vectord<N> result;
-  for (size_t i = 0; i < stdDevs.size(); ++i) {
-    // Passing a standard deviation of 0.0 to std::normal_distribution is
-    // undefined behavior
-    if (stdDevs[i] == 0.0) {
-      result(i) = 0.0;
-    } else {
-      std::normal_distribution distr{0.0, stdDevs[i]};
-      result(i) = distr(gen);
-    }
-  }
-  return result;
-}
-
-/**
- * Creates a vector of normally distributed white noise with the given noise
- * intensities for each element.
- *
- * @param stdDevs A possibly variable length container whose elements are the
- *                standard deviations of each element of the noise vector.
- * @return White noise vector.
- */
-WPILIB_DLLEXPORT Eigen::VectorXd MakeWhiteNoiseVector(
-    const std::span<const double> stdDevs);
-
-/**
- * Converts a Pose2d into a vector of [x, y, theta].
- *
- * @param pose The pose that is being represented.
- *
- * @return The vector.
- * @deprecated Create the vector manually instead. If you were using this as an
- *     intermediate step for constructing affine transformations, use
- *     Pose2d.ToMatrix() instead.
- */
-[[deprecated("Use Pose2d.ToMatrix() instead.")]]
-WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) {
-  return Eigen::Vector3d{{pose.Translation().X().value(),
-                          pose.Translation().Y().value(),
-                          pose.Rotation().Radians().value()}};
-}
-
-/**
- * Converts a Pose2d into a vector of [x, y, cos(theta), sin(theta)].
- *
- * @param pose The pose that is being represented.
- *
- * @return The vector.
- * @deprecated Create the vector manually instead. If you were using this as an
- *     intermediate step for constructing affine transformations, use
- *     Pose2d.ToMatrix() instead.
- */
-[[deprecated("Use Pose2d.ToMatrix() instead.")]]
-WPILIB_DLLEXPORT constexpr Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) {
-  return Eigen::Vector4d{{pose.Translation().X().value(),
-                          pose.Translation().Y().value(), pose.Rotation().Cos(),
-                          pose.Rotation().Sin()}};
-}
-
-/**
- * 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.
- *
- * @tparam States Number of states.
- * @tparam Inputs Number of inputs.
- * @param A System matrix.
- * @param B Input matrix.
- */
-template <int States, int Inputs>
-bool IsStabilizable(const Matrixd<States, States>& A,
-                    const Matrixd<States, Inputs>& B) {
-  Eigen::EigenSolver<Matrixd<States, States>> es{A, false};
-
-  for (int i = 0; i < A.rows(); ++i) {
-    if (std::norm(es.eigenvalues()[i]) < 1) {
-      continue;
-    }
-
-    if constexpr (States != Eigen::Dynamic && Inputs != Eigen::Dynamic) {
-      Eigen::Matrix<std::complex<double>, States, States + Inputs> E;
-      E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
-                                               States>::Identity() -
-               A,
-          B;
-
-      Eigen::ColPivHouseholderQR<
-          Eigen::Matrix<std::complex<double>, States, States + Inputs>>
-          qr{E};
-      if (qr.rank() < States) {
-        return false;
-      }
-    } else {
-      Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()};
-      E << es.eigenvalues()[i] *
-                   Eigen::MatrixXcd::Identity(A.rows(), A.rows()) -
-               A,
-          B;
-
-      Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
-      if (qr.rank() < A.rows()) {
-        return false;
-      }
-    }
-  }
-  return true;
-}
-
-extern template WPILIB_DLLEXPORT bool IsStabilizable<1, 1>(
-    const Matrixd<1, 1>& A, const Matrixd<1, 1>& B);
-extern template WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(
-    const Matrixd<2, 2>& A, const Matrixd<2, 1>& B);
-extern template WPILIB_DLLEXPORT bool
-IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(const Eigen::MatrixXd& A,
-                                               const Eigen::MatrixXd& 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.
- *
- * @tparam States Number of states.
- * @tparam Outputs Number of outputs.
- * @param A System matrix.
- * @param C Output matrix.
- */
-template <int States, int Outputs>
-bool IsDetectable(const Matrixd<States, States>& A,
-                  const Matrixd<Outputs, States>& C) {
-  return IsStabilizable<States, Outputs>(A.transpose(), C.transpose());
-}
-
-extern template WPILIB_DLLEXPORT bool
-IsDetectable<Eigen::Dynamic, Eigen::Dynamic>(const Eigen::MatrixXd& A,
-                                             const Eigen::MatrixXd& C);
-
-/**
- * Converts a Pose2d into a vector of [x, y, theta].
- *
- * @param pose The pose that is being represented.
- *
- * @return The vector.
- * @deprecated Create the vector manually instead. If you were using this as an
- *     intermediate step for constructing affine transformations, use
- *     Pose2d.ToMatrix() instead.
- */
-[[deprecated("Use Pose2d.ToMatrix() instead.")]]
-WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseToVector(const Pose2d& pose) {
-  return Eigen::Vector3d{
-      {pose.X().value(), pose.Y().value(), pose.Rotation().Radians().value()}};
-}
-
-/**
- * Clamps input vector between system's minimum and maximum allowable input.
- *
- * @tparam Inputs Number of inputs.
- * @param u Input vector to clamp.
- * @param umin The minimum input magnitude.
- * @param umax The maximum input magnitude.
- * @return Clamped input vector.
- */
-template <int Inputs>
-constexpr Vectord<Inputs> ClampInputMaxMagnitude(const Vectord<Inputs>& u,
-                                                 const Vectord<Inputs>& umin,
-                                                 const Vectord<Inputs>& umax) {
-  Vectord<Inputs> result;
-  for (int i = 0; i < u.rows(); ++i) {
-    result(i) = std::clamp(u(i), umin(i), umax(i));
-  }
-  return result;
-}
-
-extern template WPILIB_DLLEXPORT Eigen::VectorXd
-ClampInputMaxMagnitude<Eigen::Dynamic>(const Eigen::VectorXd& u,
-                                       const Eigen::VectorXd& umin,
-                                       const Eigen::VectorXd& umax);
-
 /**
  * Renormalize all inputs if any exceeds the maximum magnitude. Useful for
  * systems such as differential drivetrains.
  *
- * @tparam Inputs      Number of inputs.
- * @param u            The input vector.
+ * @tparam Inputs Number of inputs.
+ * @param u The input vector.
  * @param maxMagnitude The maximum magnitude any input can have.
  * @return The normalizedInput
  */
 template <int Inputs>
-Vectord<Inputs> DesaturateInputVector(const Vectord<Inputs>& u,
-                                      double maxMagnitude) {
-  double maxValue = u.template lpNorm<Eigen::Infinity>();
-
-  if (maxValue > maxMagnitude) {
-    return u * maxMagnitude / maxValue;
-  }
-  return u;
+Eigen::Vector<double, Inputs> DesaturateInputVector(
+    const Eigen::Vector<double, Inputs>& u, double maxMagnitude) {
+  return u * std::min(1.0, maxMagnitude / u.template lpNorm<Eigen::Infinity>());
 }
 
 extern template WPILIB_DLLEXPORT Eigen::VectorXd