[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/java/org/wpilib/math/controller/LTVDifferentialDriveController.java

@@ -74,8 +74,8 @@ public class LTVDifferentialDriveController {
     m_trackwidth = trackwidth;
     m_A = plant.getA();
     m_B = plant.getB();
-    m_Q = StateSpaceUtil.makeCostMatrix(qelems);
-    m_R = StateSpaceUtil.makeCostMatrix(relems);
+    m_Q = StateSpaceUtil.costMatrix(qelems);
+    m_R = StateSpaceUtil.costMatrix(relems);
     m_dt = dt;
   }
 

wpimath/src/main/java/org/wpilib/math/controller/LTVUnicycleController.java

@@ -64,8 +64,8 @@ public class LTVUnicycleController {
    * @param dt Discretization timestep in seconds.
    */
   public LTVUnicycleController(Vector<N3> qelems, Vector<N2> relems, double dt) {
-    m_Q = StateSpaceUtil.makeCostMatrix(qelems);
-    m_R = StateSpaceUtil.makeCostMatrix(relems);
+    m_Q = StateSpaceUtil.costMatrix(qelems);
+    m_R = StateSpaceUtil.costMatrix(relems);
     m_dt = dt;
   }
 

wpimath/src/main/java/org/wpilib/math/controller/LinearQuadraticRegulator.java

@@ -56,8 +56,8 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
     this(
         plant.getA(),
         plant.getB(),
-        StateSpaceUtil.makeCostMatrix(qelms),
-        StateSpaceUtil.makeCostMatrix(relms),
+        StateSpaceUtil.costMatrix(qelms),
+        StateSpaceUtil.costMatrix(relms),
         dt);
   }
 
@@ -81,7 +81,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
       Vector<States> qelms,
       Vector<Inputs> relms,
       double dt) {
-    this(A, B, StateSpaceUtil.makeCostMatrix(qelms), StateSpaceUtil.makeCostMatrix(relms), dt);
+    this(A, B, StateSpaceUtil.costMatrix(qelms), StateSpaceUtil.costMatrix(relms), dt);
   }
 
   /**

wpimath/src/main/java/org/wpilib/math/estimator/ExtendedKalmanFilter.java

@@ -9,6 +9,7 @@ import org.wpilib.math.linalg.DARE;
 import org.wpilib.math.linalg.Matrix;
 import org.wpilib.math.numbers.N1;
 import org.wpilib.math.system.Discretization;
+import org.wpilib.math.system.LinearSystemUtil;
 import org.wpilib.math.system.NumericalIntegration;
 import org.wpilib.math.system.NumericalJacobian;
 import org.wpilib.math.util.Nat;
@@ -137,8 +138,8 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
     m_residualFuncY = residualFuncY;
     m_addFuncX = addFuncX;
 
-    m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
-    m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+    m_contQ = StateSpaceUtil.covarianceMatrix(states, stateStdDevs);
+    m_contR = StateSpaceUtil.covarianceMatrix(outputs, measurementStdDevs);
     m_dt = dt;
 
     reset();
@@ -156,7 +157,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
 
     final var discR = Discretization.discretizeR(m_contR, dt);
 
-    if (StateSpaceUtil.isDetectable(discA, C) && outputs.getNum() <= states.getNum()) {
+    if (LinearSystemUtil.isDetectable(discA, C) && outputs.getNum() <= states.getNum()) {
       m_initP = DARE.dare(discA.transpose(), C.transpose(), discQ, discR);
     } else {
       m_initP = new Matrix<>(states, states);

wpimath/src/main/java/org/wpilib/math/estimator/KalmanFilter.java

@@ -71,8 +71,8 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
 
     this.m_plant = plant;
 
-    m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
-    m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+    m_contQ = StateSpaceUtil.covarianceMatrix(states, stateStdDevs);
+    m_contR = StateSpaceUtil.covarianceMatrix(outputs, measurementStdDevs);
     m_dt = dt;
 
     // Find discrete A and Q

wpimath/src/main/java/org/wpilib/math/estimator/SteadyStateKalmanFilter.java

@@ -72,8 +72,8 @@ public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Out
 
     this.m_plant = plant;
 
-    var contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
-    var contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+    var contQ = StateSpaceUtil.covarianceMatrix(states, stateStdDevs);
+    var contR = StateSpaceUtil.covarianceMatrix(outputs, measurementStdDevs);
 
     var pair = Discretization.discretizeAQ(plant.getA(), contQ, dt);
     var discA = pair.getFirst();

wpimath/src/main/java/org/wpilib/math/estimator/UnscentedKalmanFilter.java

@@ -167,8 +167,8 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
 
     m_dt = nominalDt;
 
-    m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
-    m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+    m_contQ = StateSpaceUtil.covarianceMatrix(states, stateStdDevs);
+    m_contR = StateSpaceUtil.covarianceMatrix(outputs, measurementStdDevs);
 
     m_pts = pts;
 

wpimath/src/main/java/org/wpilib/math/jni/LinearSystemUtilJNI.java

@@ -4,8 +4,8 @@
 
 package org.wpilib.math.jni;
 
-/** StateSpaceUtil JNI. */
-public final class StateSpaceUtilJNI extends WPIMathJNI {
+/** LinearSystemUtil JNI. */
+public final class LinearSystemUtilJNI extends WPIMathJNI {
   /**
    * Returns true if (A, B) is a stabilizable pair.
    *
@@ -22,5 +22,5 @@ public final class StateSpaceUtilJNI extends WPIMathJNI {
   public static native boolean isStabilizable(int states, int inputs, double[] A, double[] B);
 
   /** Utility class. */
-  private StateSpaceUtilJNI() {}
+  private LinearSystemUtilJNI() {}
 }

wpimath/src/main/java/org/wpilib/math/random/Normal.java

@@ -0,0 +1,37 @@
+// 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.
+
+package org.wpilib.math.random;
+
+import java.util.Random;
+import org.ejml.simple.SimpleMatrix;
+import org.wpilib.math.linalg.Matrix;
+import org.wpilib.math.numbers.N1;
+import org.wpilib.math.util.Num;
+
+/** Utilities for generating normally distributed random values. */
+public final class Normal {
+  private static Random rand = new Random();
+
+  private Normal() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Creates a vector of normally distributed random values with the given standard deviations for
+   * each element.
+   *
+   * @param <N> Num representing the dimensionality of the noise vector to create.
+   * @param stdDevs A matrix whose elements are the standard deviations of each element of the
+   *     random vector.
+   * @return Vector of normally distributed values.
+   */
+  public static <N extends Num> Matrix<N, N1> normal(Matrix<N, N1> stdDevs) {
+    Matrix<N, N1> result = new Matrix<>(new SimpleMatrix(stdDevs.getNumRows(), 1));
+    for (int i = 0; i < stdDevs.getNumRows(); i++) {
+      result.set(i, 0, rand.nextGaussian() * stdDevs.get(i, 0));
+    }
+    return result;
+  }
+}

wpimath/src/main/java/org/wpilib/math/system/LinearSystemUtil.java

@@ -0,0 +1,54 @@
+// 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.
+
+package org.wpilib.math.system;
+
+import org.wpilib.math.jni.LinearSystemUtilJNI;
+import org.wpilib.math.linalg.Matrix;
+import org.wpilib.math.util.Num;
+
+/** Linear system utilities. */
+public final class LinearSystemUtil {
+  private LinearSystemUtil() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * 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
+   * absolute values less than one, where an eigenvalue is uncontrollable if rank([λI - A, B]) %3C n
+   * where n is the number of states.
+   *
+   * @param <States> Num representing the size of A.
+   * @param <Inputs> Num representing the columns of B.
+   * @param A System matrix.
+   * @param B Input matrix.
+   * @return If the system is stabilizable.
+   */
+  public static <States extends Num, Inputs extends Num> boolean isStabilizable(
+      Matrix<States, States> A, Matrix<States, Inputs> B) {
+    return LinearSystemUtilJNI.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.
+   */
+  public static <States extends Num, Outputs extends Num> boolean isDetectable(
+      Matrix<States, States> A, Matrix<Outputs, States> C) {
+    return LinearSystemUtilJNI.isStabilizable(
+        A.getNumRows(), C.getNumRows(), A.transpose().getData(), C.transpose().getData());
+  }
+}

wpimath/src/main/java/org/wpilib/math/util/StateSpaceUtil.java

@@ -4,62 +4,16 @@
 
 package org.wpilib.math.util;
 
-import java.util.Random;
 import org.ejml.simple.SimpleMatrix;
-import org.wpilib.math.geometry.Pose2d;
-import org.wpilib.math.jni.StateSpaceUtilJNI;
 import org.wpilib.math.linalg.Matrix;
-import org.wpilib.math.linalg.VecBuilder;
 import org.wpilib.math.numbers.N1;
-import org.wpilib.math.numbers.N3;
-import org.wpilib.math.numbers.N4;
 
 /** State-space utilities. */
 public final class StateSpaceUtil {
-  private static Random rand = new Random();
-
   private StateSpaceUtil() {
     throw new UnsupportedOperationException("This is a utility class!");
   }
 
-  /**
-   * Creates a covariance matrix from the given vector for use with Kalman filters.
-   *
-   * <p>Each element is squared and placed on the covariance matrix diagonal.
-   *
-   * @param <States> Num representing the states of the system.
-   * @param states A Nat representing the states of the system.
-   * @param stdDevs 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.
-   */
-  public static <States extends Num> Matrix<States, States> makeCovarianceMatrix(
-      Nat<States> states, Matrix<States, N1> stdDevs) {
-    var result = new Matrix<>(states, states);
-    for (int i = 0; i < states.getNum(); i++) {
-      result.set(i, i, Math.pow(stdDevs.get(i, 0), 2));
-    }
-    return result;
-  }
-
-  /**
-   * Creates a vector of normally distributed white noise with the given noise intensities for each
-   * element.
-   *
-   * @param <N> Num representing the dimensionality of the noise vector to create.
-   * @param stdDevs A matrix whose elements are the standard deviations of each element of the noise
-   *     vector.
-   * @return White noise vector.
-   */
-  public static <N extends Num> Matrix<N, N1> makeWhiteNoiseVector(Matrix<N, N1> stdDevs) {
-    Matrix<N, N1> result = new Matrix<>(new SimpleMatrix(stdDevs.getNumRows(), 1));
-    for (int i = 0; i < stdDevs.getNumRows(); i++) {
-      result.set(i, 0, rand.nextGaussian() * stdDevs.get(i, 0));
-    }
-    return result;
-  }
-
   /**
    * Creates a cost matrix from the given vector for use with LQR.
    *
@@ -73,7 +27,7 @@ public final class StateSpaceUtil {
    *     excursions of the control inputs from no actuation.
    * @return State excursion or control effort cost matrix.
    */
-  public static <Elements extends Num> Matrix<Elements, Elements> makeCostMatrix(
+  public static <Elements extends Num> Matrix<Elements, Elements> costMatrix(
       Matrix<Elements, N1> tolerances) {
     Matrix<Elements, Elements> result =
         new Matrix<>(new SimpleMatrix(tolerances.getNumRows(), tolerances.getNumRows()));
@@ -91,70 +45,22 @@ public final class StateSpaceUtil {
   }
 
   /**
-   * Returns true if (A, B) is a stabilizable pair.
+   * Creates a covariance matrix from the given vector for use with Kalman filters.
    *
-   * <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 the number of states.
+   * <p>Each element is squared and placed on the covariance matrix diagonal.
    *
-   * @param <States> Num representing the size of A.
-   * @param <Inputs> Num representing the columns of B.
-   * @param A System matrix.
-   * @param B Input matrix.
-   * @return If the system is stabilizable.
+   * @param <States> Num representing the states of the system.
+   * @param states A Nat representing the states of the system.
+   * @param stdDevs 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.
    */
-  public static <States extends Num, Inputs extends Num> boolean isStabilizable(
-      Matrix<States, States> A, Matrix<States, Inputs> B) {
-    return StateSpaceUtilJNI.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.
-   */
-  public static <States extends Num, Outputs extends Num> boolean isDetectable(
-      Matrix<States, States> A, Matrix<Outputs, States> C) {
-    return StateSpaceUtilJNI.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.
-   *
-   * @param pose A pose to convert to a vector.
-   * @return The given pose in vector form, with the third element, theta, in radians.
-   * @deprecated Create the vector manually instead. If you were using this as an intermediate step
-   *     for constructing affine transformations, use {@link Pose2d#toMatrix()} instead.
-   */
-  @Deprecated(forRemoval = true, since = "2025")
-  public static Matrix<N3, N1> poseToVector(Pose2d pose) {
-    return VecBuilder.fill(pose.getX(), pose.getY(), pose.getRotation().getRadians());
-  }
-
-  /**
-   * Clamp the input u to the min and max.
-   *
-   * @param u The input to clamp.
-   * @param umin The minimum input magnitude.
-   * @param umax The maximum input magnitude.
-   * @param <I> Number of inputs.
-   * @return The clamped input.
-   */
-  public static <I extends Num> Matrix<I, N1> clampInputMaxMagnitude(
-      Matrix<I, N1> u, Matrix<I, N1> umin, Matrix<I, N1> umax) {
-    var result = new Matrix<I, N1>(new SimpleMatrix(u.getNumRows(), 1));
-    for (int i = 0; i < u.getNumRows(); i++) {
-      result.set(i, 0, Math.clamp(u.get(i, 0), umin.get(i, 0), umax.get(i, 0)));
+  public static <States extends Num> Matrix<States, States> covarianceMatrix(
+      Nat<States> states, Matrix<States, N1> stdDevs) {
+    var result = new Matrix<>(states, states);
+    for (int i = 0; i < states.getNum(); i++) {
+      result.set(i, i, Math.pow(stdDevs.get(i, 0), 2));
     }
     return result;
   }
@@ -170,46 +76,6 @@ public final class StateSpaceUtil {
    */
   public static <I extends Num> Matrix<I, N1> desaturateInputVector(
       Matrix<I, N1> u, double maxMagnitude) {
-    double maxValue = u.maxAbs();
-    boolean isCapped = maxValue > maxMagnitude;
-
-    if (isCapped) {
-      return u.times(maxMagnitude / maxValue);
-    }
-    return u;
-  }
-
-  /**
-   * Convert a {@link Pose2d} to a vector of [x, y, cos(theta), sin(theta)], where theta is in
-   * radians.
-   *
-   * @param pose A pose to convert to a vector.
-   * @return The given pose in as a 4x1 vector of x, y, cos(theta), and sin(theta).
-   * @deprecated Create the vector manually instead. If you were using this as an intermediate step
-   *     for constructing affine transformations, use {@link Pose2d#toMatrix()} instead.
-   */
-  @Deprecated(forRemoval = true, since = "2025")
-  public static Matrix<N4, N1> poseTo4dVector(Pose2d pose) {
-    return VecBuilder.fill(
-        pose.getTranslation().getX(),
-        pose.getTranslation().getY(),
-        pose.getRotation().getCos(),
-        pose.getRotation().getSin());
-  }
-
-  /**
-   * Convert a {@link Pose2d} to a vector of [x, y, theta], where theta is in radians.
-   *
-   * @param pose A pose to convert to a vector.
-   * @return The given pose in vector form, with the third element, theta, in radians.
-   * @deprecated Create the vector manually instead. If you were using this as an intermediate step
-   *     for constructing affine transformations, use {@link Pose2d#toMatrix()} instead.
-   */
-  @Deprecated(forRemoval = true, since = "2025")
-  public static Matrix<N3, N1> poseTo3dVector(Pose2d pose) {
-    return VecBuilder.fill(
-        pose.getTranslation().getX(),
-        pose.getTranslation().getY(),
-        pose.getRotation().getRadians());
+    return u.times(Math.min(1.0, maxMagnitude / u.maxAbs()));
   }
 }