[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/test/java/org/wpilib/math/estimator/ExtendedKalmanFilterTest.java

@@ -17,6 +17,7 @@ import org.wpilib.math.numbers.N1;
 import org.wpilib.math.numbers.N2;
 import org.wpilib.math.numbers.N3;
 import org.wpilib.math.numbers.N5;
+import org.wpilib.math.random.Normal;
 import org.wpilib.math.system.NumericalIntegration;
 import org.wpilib.math.system.NumericalJacobian;
 import org.wpilib.math.system.plant.DCMotor;
@@ -89,7 +90,7 @@ class ExtendedKalmanFilterTest {
           observer.correct(u, localY);
 
           var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
-          var R = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
+          var R = StateSpaceUtil.costMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
           observer.correct(
               Nat.N5(), u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R);
         });
@@ -154,8 +155,7 @@ class ExtendedKalmanFilterTest {
       nextR.set(4, 0, vr);
 
       var localY = getLocalMeasurementModel(groundTruthX, u);
-      var whiteNoiseStdDevs = VecBuilder.fill(0.0001, 0.5, 0.5);
-      observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(whiteNoiseStdDevs)));
+      observer.correct(u, localY.plus(Normal.normal(VecBuilder.fill(0.0001, 0.5, 0.5))));
 
       Matrix<N5, N1> rdot = nextR.minus(r).div(dt);
       u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
@@ -172,7 +172,7 @@ class ExtendedKalmanFilterTest {
     observer.correct(u, localY);
 
     var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
-    var R = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
+    var R = StateSpaceUtil.costMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
     observer.correct(Nat.N5(), u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R);
 
     var finalPosition = trajectory.sample(trajectory.getTotalTime());

wpimath/src/test/java/org/wpilib/math/estimator/MerweUKFTest.java

@@ -22,6 +22,7 @@ import org.wpilib.math.numbers.N2;
 import org.wpilib.math.numbers.N3;
 import org.wpilib.math.numbers.N4;
 import org.wpilib.math.numbers.N5;
+import org.wpilib.math.random.Normal;
 import org.wpilib.math.system.Discretization;
 import org.wpilib.math.system.NumericalIntegration;
 import org.wpilib.math.system.NumericalJacobian;
@@ -100,7 +101,7 @@ class MerweUKFTest {
 
           var globalY = driveGlobalMeasurementModel(observer.getXhat(), u);
           var R =
-              StateSpaceUtil.makeCovarianceMatrix(
+              StateSpaceUtil.covarianceMatrix(
                   Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.01, 0.01));
           observer.correct(
               Nat.N5(),
@@ -181,7 +182,7 @@ class MerweUKFTest {
           driveLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
       var noiseStdDev = VecBuilder.fill(0.0001, 0.5, 0.5);
 
-      observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(noiseStdDev)));
+      observer.correct(u, localY.plus(Normal.normal(noiseStdDev)));
 
       var rdot = nextR.minus(r).div(dt);
       u = new Matrix<>(B.solve(rdot.minus(driveDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
@@ -197,8 +198,7 @@ class MerweUKFTest {
 
     var globalY = driveGlobalMeasurementModel(trueXhat, u);
     var R =
-        StateSpaceUtil.makeCovarianceMatrix(
-            Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
+        StateSpaceUtil.covarianceMatrix(Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
     observer.correct(
         Nat.N5(),
         u,
@@ -358,9 +358,7 @@ class MerweUKFTest {
       measurements.set(
           i,
           motorMeasurementModel(states.get(i + 1), inputs.get(i))
-              .plus(
-                  StateSpaceUtil.makeWhiteNoiseVector(
-                      VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev))));
+              .plus(Normal.normal(VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev))));
     }
 
     var P0 =

wpimath/src/test/java/org/wpilib/math/estimator/S3UKFTest.java

@@ -22,6 +22,7 @@ import org.wpilib.math.numbers.N2;
 import org.wpilib.math.numbers.N3;
 import org.wpilib.math.numbers.N4;
 import org.wpilib.math.numbers.N5;
+import org.wpilib.math.random.Normal;
 import org.wpilib.math.system.Discretization;
 import org.wpilib.math.system.NumericalIntegration;
 import org.wpilib.math.system.NumericalJacobian;
@@ -100,7 +101,7 @@ class S3UKFTest {
 
           var globalY = driveGlobalMeasurementModel(observer.getXhat(), u);
           var R =
-              StateSpaceUtil.makeCovarianceMatrix(
+              StateSpaceUtil.covarianceMatrix(
                   Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.01, 0.01));
           observer.correct(
               Nat.N5(),
@@ -181,7 +182,7 @@ class S3UKFTest {
           driveLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
       var noiseStdDev = VecBuilder.fill(0.0001, 0.5, 0.5);
 
-      observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(noiseStdDev)));
+      observer.correct(u, localY.plus(Normal.normal(noiseStdDev)));
 
       var rdot = nextR.minus(r).div(dt);
       u = new Matrix<>(B.solve(rdot.minus(driveDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
@@ -197,8 +198,7 @@ class S3UKFTest {
 
     var globalY = driveGlobalMeasurementModel(trueXhat, u);
     var R =
-        StateSpaceUtil.makeCovarianceMatrix(
-            Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
+        StateSpaceUtil.covarianceMatrix(Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
     observer.correct(
         Nat.N5(),
         u,
@@ -358,9 +358,7 @@ class S3UKFTest {
       measurements.set(
           i,
           motorMeasurementModel(states.get(i + 1), inputs.get(i))
-              .plus(
-                  StateSpaceUtil.makeWhiteNoiseVector(
-                      VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev))));
+              .plus(Normal.normal(VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev))));
     }
 
     var P0 =

wpimath/src/test/java/org/wpilib/math/jni/LinearSystemUtilJNITest.java

@@ -8,9 +8,9 @@ import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 
 import org.junit.jupiter.api.Test;
 
-public class StateSpaceUtilJNITest {
+public class LinearSystemUtilJNITest {
   @Test
   public void testLink() {
-    assertDoesNotThrow(StateSpaceUtilJNI::forceLoad);
+    assertDoesNotThrow(LinearSystemUtilJNI::forceLoad);
   }
 }

wpimath/src/test/java/org/wpilib/math/random/NormalTest.java

@@ -0,0 +1,50 @@
+// 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 static org.junit.jupiter.api.Assertions.assertEquals;
+
+import java.util.ArrayList;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+import org.wpilib.UtilityClassTest;
+import org.wpilib.math.linalg.VecBuilder;
+
+class NormalTest extends UtilityClassTest<Normal> {
+  NormalTest() {
+    super(Normal.class);
+  }
+
+  @Test
+  void testNormal() {
+    var firstData = new ArrayList<Double>();
+    var secondData = new ArrayList<Double>();
+    for (int i = 0; i < 1000; i++) {
+      var noiseVec = Normal.normal(VecBuilder.fill(1.0, 2.0));
+      firstData.add(noiseVec.get(0, 0));
+      secondData.add(noiseVec.get(1, 0));
+    }
+    assertEquals(1.0, calculateStandardDeviation(firstData), 0.2);
+    assertEquals(2.0, calculateStandardDeviation(secondData), 0.2);
+  }
+
+  private double calculateStandardDeviation(List<Double> numArray) {
+    double sum = 0.0;
+    double standardDeviation = 0.0;
+    int length = numArray.size();
+
+    for (double num : numArray) {
+      sum += num;
+    }
+
+    double mean = sum / length;
+
+    for (double num : numArray) {
+      standardDeviation += Math.pow(num - mean, 2);
+    }
+
+    return Math.sqrt(standardDeviation / length);
+  }
+}

wpimath/src/test/java/org/wpilib/math/system/LinearSystemUtilTest.java

@@ -0,0 +1,75 @@
+// 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 static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+import org.wpilib.UtilityClassTest;
+import org.wpilib.math.linalg.MatBuilder;
+import org.wpilib.math.linalg.Matrix;
+import org.wpilib.math.linalg.VecBuilder;
+import org.wpilib.math.numbers.N1;
+import org.wpilib.math.numbers.N2;
+import org.wpilib.math.util.Nat;
+
+class LinearSystemUtilTest extends UtilityClassTest<LinearSystemUtil> {
+  LinearSystemUtilTest() {
+    super(LinearSystemUtil.class);
+  }
+
+  @Test
+  void testIsStabilizable() {
+    Matrix<N2, N2> A;
+    Matrix<N2, N1> B = VecBuilder.fill(0, 1);
+
+    // First eigenvalue is uncontrollable and unstable.
+    // Second eigenvalue is controllable and stable.
+    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.2, 0, 0, 0.5);
+    assertFalse(LinearSystemUtil.isStabilizable(A, B));
+
+    // First eigenvalue is uncontrollable and marginally stable.
+    // Second eigenvalue is controllable and stable.
+    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.5);
+    assertFalse(LinearSystemUtil.isStabilizable(A, B));
+
+    // First eigenvalue is uncontrollable and stable.
+    // Second eigenvalue is controllable and stable.
+    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 0.5);
+    assertTrue(LinearSystemUtil.isStabilizable(A, B));
+
+    // First eigenvalue is uncontrollable and stable.
+    // Second eigenvalue is controllable and unstable.
+    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 1.2);
+    assertTrue(LinearSystemUtil.isStabilizable(A, B));
+  }
+
+  @Test
+  void testIsDetectable() {
+    Matrix<N2, N2> A;
+    Matrix<N1, N2> C = MatBuilder.fill(Nat.N1(), Nat.N2(), 0, 1);
+
+    // First eigenvalue is unobservable and unstable.
+    // Second eigenvalue is observable and stable.
+    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.2, 0, 0, 0.5);
+    assertFalse(LinearSystemUtil.isDetectable(A, C));
+
+    // First eigenvalue is unobservable and marginally stable.
+    // Second eigenvalue is observable and stable.
+    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.5);
+    assertFalse(LinearSystemUtil.isDetectable(A, C));
+
+    // First eigenvalue is unobservable and stable.
+    // Second eigenvalue is observable and stable.
+    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 0.5);
+    assertTrue(LinearSystemUtil.isDetectable(A, C));
+
+    // First eigenvalue is unobservable and stable.
+    // Second eigenvalue is observable and unstable.
+    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 1.2);
+    assertTrue(LinearSystemUtil.isDetectable(A, C));
+  }
+}

wpimath/src/test/java/org/wpilib/math/util/StateSpaceUtilTest.java

@@ -5,19 +5,13 @@
 package org.wpilib.math.util;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
-import java.util.ArrayList;
-import java.util.List;
 import org.junit.jupiter.api.Test;
 import org.wpilib.UtilityClassTest;
-import org.wpilib.math.geometry.Pose2d;
-import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.linalg.MatBuilder;
 import org.wpilib.math.linalg.Matrix;
 import org.wpilib.math.linalg.VecBuilder;
-import org.wpilib.math.numbers.N1;
 import org.wpilib.math.numbers.N2;
 
 class StateSpaceUtilTest extends UtilityClassTest<StateSpaceUtil> {
@@ -27,7 +21,7 @@ class StateSpaceUtilTest extends UtilityClassTest<StateSpaceUtil> {
 
   @Test
   void testCostArray() {
-    var mat = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(1.0, 2.0, 3.0));
+    var mat = StateSpaceUtil.costMatrix(VecBuilder.fill(1.0, 2.0, 3.0));
 
     assertEquals(1.0, mat.get(0, 0), 1e-3);
     assertEquals(0.0, mat.get(0, 1), 1e-3);
@@ -42,7 +36,7 @@ class StateSpaceUtilTest extends UtilityClassTest<StateSpaceUtil> {
 
   @Test
   void testCovArray() {
-    var mat = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), VecBuilder.fill(1.0, 2.0, 3.0));
+    var mat = StateSpaceUtil.covarianceMatrix(Nat.N3(), VecBuilder.fill(1.0, 2.0, 3.0));
 
     assertEquals(1.0, mat.get(0, 0), 1e-3);
     assertEquals(0.0, mat.get(0, 1), 1e-3);
@@ -55,89 +49,6 @@ class StateSpaceUtilTest extends UtilityClassTest<StateSpaceUtil> {
     assertEquals(9.0, mat.get(2, 2), 1e-3);
   }
 
-  @Test
-  void testIsStabilizable() {
-    Matrix<N2, N2> A;
-    Matrix<N2, N1> B = VecBuilder.fill(0, 1);
-
-    // First eigenvalue is uncontrollable and unstable.
-    // Second eigenvalue is controllable and stable.
-    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.2, 0, 0, 0.5);
-    assertFalse(StateSpaceUtil.isStabilizable(A, B));
-
-    // First eigenvalue is uncontrollable and marginally stable.
-    // Second eigenvalue is controllable and stable.
-    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.5);
-    assertFalse(StateSpaceUtil.isStabilizable(A, B));
-
-    // First eigenvalue is uncontrollable and stable.
-    // Second eigenvalue is controllable and stable.
-    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 0.5);
-    assertTrue(StateSpaceUtil.isStabilizable(A, B));
-
-    // First eigenvalue is uncontrollable and stable.
-    // Second eigenvalue is controllable and unstable.
-    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 1.2);
-    assertTrue(StateSpaceUtil.isStabilizable(A, B));
-  }
-
-  @Test
-  void testIsDetectable() {
-    Matrix<N2, N2> A;
-    Matrix<N1, N2> C = MatBuilder.fill(Nat.N1(), Nat.N2(), 0, 1);
-
-    // First eigenvalue is unobservable and unstable.
-    // Second eigenvalue is observable and stable.
-    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 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 = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.5);
-    assertFalse(StateSpaceUtil.isDetectable(A, C));
-
-    // First eigenvalue is unobservable and stable.
-    // Second eigenvalue is observable and stable.
-    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 0.5);
-    assertTrue(StateSpaceUtil.isDetectable(A, C));
-
-    // First eigenvalue is unobservable and stable.
-    // Second eigenvalue is observable and unstable.
-    A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 1.2);
-    assertTrue(StateSpaceUtil.isDetectable(A, C));
-  }
-
-  @Test
-  void testMakeWhiteNoiseVector() {
-    var firstData = new ArrayList<Double>();
-    var secondData = new ArrayList<Double>();
-    for (int i = 0; i < 1000; i++) {
-      var noiseVec = StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(1.0, 2.0));
-      firstData.add(noiseVec.get(0, 0));
-      secondData.add(noiseVec.get(1, 0));
-    }
-    assertEquals(1.0, calculateStandardDeviation(firstData), 0.2);
-    assertEquals(2.0, calculateStandardDeviation(secondData), 0.2);
-  }
-
-  private double calculateStandardDeviation(List<Double> numArray) {
-    double sum = 0.0;
-    double standardDeviation = 0.0;
-    int length = numArray.size();
-
-    for (double num : numArray) {
-      sum += num;
-    }
-
-    double mean = sum / length;
-
-    for (double num : numArray) {
-      standardDeviation += Math.pow(num - mean, 2);
-    }
-
-    return Math.sqrt(standardDeviation / length);
-  }
-
   @Test
   void testMatrixExp() {
     Matrix<N2, N2> wrappedMatrix = Matrix.eye(Nat.N2());
@@ -156,12 +67,20 @@ class StateSpaceUtilTest extends UtilityClassTest<StateSpaceUtil> {
   }
 
   @Test
-  @SuppressWarnings("removal")
-  void testPoseToVector() {
-    Pose2d pose = new Pose2d(1, 2, new Rotation2d(3));
-    var vector = StateSpaceUtil.poseToVector(pose);
-    assertEquals(pose.getTranslation().getX(), vector.get(0, 0), 1e-6);
-    assertEquals(pose.getTranslation().getY(), vector.get(1, 0), 1e-6);
-    assertEquals(pose.getRotation().getRadians(), vector.get(2, 0), 1e-6);
+  void testDesaturateInputVector() {
+    final var vec1 = MatBuilder.fill(Nat.N2(), Nat.N1(), 10.0, 12.0);
+    assertEquals(vec1, StateSpaceUtil.desaturateInputVector(vec1, 12.0));
+    assertEquals(
+        MatBuilder.fill(Nat.N2(), Nat.N1(), 25.0 / 3.0, 10.0),
+        StateSpaceUtil.desaturateInputVector(vec1, 10.0));
+
+    final var vec2 = MatBuilder.fill(Nat.N2(), Nat.N1(), 10.0, -12.0);
+    assertEquals(vec2, StateSpaceUtil.desaturateInputVector(vec2, 12.0));
+    assertEquals(
+        MatBuilder.fill(Nat.N2(), Nat.N1(), 25.0 / 3.0, -10.0),
+        StateSpaceUtil.desaturateInputVector(vec2, 10.0));
+
+    final var vec3 = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 0.0);
+    assertEquals(vec3, StateSpaceUtil.desaturateInputVector(vec3, 12.0));
   }
 }

wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp

@@ -11,10 +11,10 @@
 #include <gtest/gtest.h>
 
 #include "wpi/math/linalg/EigenCore.hpp"
+#include "wpi/math/random/Normal.hpp"
 #include "wpi/math/system/NumericalJacobian.hpp"
 #include "wpi/math/system/plant/DCMotor.hpp"
 #include "wpi/math/trajectory/TrajectoryGenerator.hpp"
-#include "wpi/math/util/StateSpaceUtil.hpp"
 #include "wpi/units/moment_of_inertia.hpp"
 
 namespace {
@@ -79,7 +79,7 @@ TEST(ExtendedKalmanFilterTest, Init) {
   observer.Correct(u, localY);
 
   auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
-  auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
+  auto R = wpi::math::CovarianceMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
   observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
 }
 
@@ -123,8 +123,7 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
         ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
 
     auto localY = LocalMeasurementModel(nextR, wpi::math::Vectord<2>::Zero());
-    observer.Correct(
-        u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
+    observer.Correct(u, localY + wpi::math::Normal(0.0001, 0.5, 0.5));
 
     wpi::math::Vectord<5> rdot = (nextR - r) / dt.value();
     u = B.householderQr().solve(rdot -
@@ -139,7 +138,7 @@ TEST(ExtendedKalmanFilterTest, Convergence) {
   observer.Correct(u, localY);
 
   auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
-  auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
+  auto R = wpi::math::CovarianceMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
   observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
 
   auto finalPosition = trajectory.Sample(trajectory.TotalTime());

wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp

@@ -14,13 +14,13 @@
 
 #include "wpi/math/estimator/AngleStatistics.hpp"
 #include "wpi/math/linalg/EigenCore.hpp"
+#include "wpi/math/random/Normal.hpp"
 #include "wpi/math/system/Discretization.hpp"
 #include "wpi/math/system/NumericalIntegration.hpp"
 #include "wpi/math/system/NumericalJacobian.hpp"
 #include "wpi/math/system/plant/DCMotor.hpp"
 #include "wpi/math/system/plant/LinearSystemId.hpp"
 #include "wpi/math/trajectory/TrajectoryGenerator.hpp"
-#include "wpi/math/util/StateSpaceUtil.hpp"
 #include "wpi/units/moment_of_inertia.hpp"
 
 namespace {
@@ -90,7 +90,7 @@ TEST(MerweUKFTest, DriveInit) {
   observer.Correct(u, localY);
 
   auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u);
-  auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
+  auto R = wpi::math::CovarianceMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
   observer.Correct<5>(
       u, globalY, DriveGlobalMeasurementModel, R,
       wpi::math::AngleMean<5, 2 * 5 + 1>(2), wpi::math::AngleResidual<5>(2),
@@ -146,8 +146,7 @@ TEST(MerweUKFTest, DriveConvergence) {
 
     auto localY =
         DriveLocalMeasurementModel(trueXhat, wpi::math::Vectord<2>::Zero());
-    observer.Correct(
-        u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
+    observer.Correct(u, localY + wpi::math::Normal(0.0001, 0.5, 0.5));
 
     wpi::math::Vectord<5> rdot = (nextR - r) / dt.value();
     u = B.householderQr().solve(
@@ -163,7 +162,7 @@ TEST(MerweUKFTest, DriveConvergence) {
   observer.Correct(u, localY);
 
   auto globalY = DriveGlobalMeasurementModel(trueXhat, u);
-  auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
+  auto R = wpi::math::CovarianceMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
   observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
                       wpi::math::AngleMean<5, 2 * 5 + 1>(2),
                       wpi::math::AngleResidual<5>(2),
@@ -296,9 +295,8 @@ TEST(MerweUKFTest, MotorConvergence) {
   for (int i = 0; i < steps; ++i) {
     inputs[i] = MotorControlInput(i * dt.value());
     states[i + 1] = discA * states[i] + discB * inputs[i];
-    measurements[i] =
-        MotorMeasurementModel(states[i + 1], inputs[i]) +
-        wpi::math::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev);
+    measurements[i] = MotorMeasurementModel(states[i + 1], inputs[i]) +
+                      wpi::math::Normal(pos_stddev, vel_stddev, accel_stddev);
   }
 
   wpi::math::Vectord<4> P0{0.001, 0.001, 10, 10};

wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp

@@ -14,13 +14,13 @@
 
 #include "wpi/math/estimator/AngleStatistics.hpp"
 #include "wpi/math/linalg/EigenCore.hpp"
+#include "wpi/math/random/Normal.hpp"
 #include "wpi/math/system/Discretization.hpp"
 #include "wpi/math/system/NumericalIntegration.hpp"
 #include "wpi/math/system/NumericalJacobian.hpp"
 #include "wpi/math/system/plant/DCMotor.hpp"
 #include "wpi/math/system/plant/LinearSystemId.hpp"
 #include "wpi/math/trajectory/TrajectoryGenerator.hpp"
-#include "wpi/math/util/StateSpaceUtil.hpp"
 #include "wpi/units/moment_of_inertia.hpp"
 
 namespace {
@@ -90,7 +90,7 @@ TEST(S3UKFTest, DriveInit) {
   observer.Correct(u, localY);
 
   auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u);
-  auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
+  auto R = wpi::math::CovarianceMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
   observer.Correct<5>(
       u, globalY, DriveGlobalMeasurementModel, R,
       wpi::math::AngleMean<5, 5 + 2>(2), wpi::math::AngleResidual<5>(2),
@@ -146,8 +146,7 @@ TEST(S3UKFTest, DriveConvergence) {
 
     auto localY =
         DriveLocalMeasurementModel(trueXhat, wpi::math::Vectord<2>::Zero());
-    observer.Correct(
-        u, localY + wpi::math::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
+    observer.Correct(u, localY + wpi::math::Normal(0.0001, 0.5, 0.5));
 
     wpi::math::Vectord<5> rdot = (nextR - r) / dt.value();
     u = B.householderQr().solve(
@@ -163,7 +162,7 @@ TEST(S3UKFTest, DriveConvergence) {
   observer.Correct(u, localY);
 
   auto globalY = DriveGlobalMeasurementModel(trueXhat, u);
-  auto R = wpi::math::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
+  auto R = wpi::math::CovarianceMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
   observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
                       wpi::math::AngleMean<5, 5 + 2>(2),
                       wpi::math::AngleResidual<5>(2),
@@ -296,9 +295,8 @@ TEST(S3UKFTest, MotorConvergence) {
   for (int i = 0; i < steps; ++i) {
     inputs[i] = MotorControlInput(i * dt.value());
     states[i + 1] = discA * states[i] + discB * inputs[i];
-    measurements[i] =
-        MotorMeasurementModel(states[i + 1], inputs[i]) +
-        wpi::math::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev);
+    measurements[i] = MotorMeasurementModel(states[i + 1], inputs[i]) +
+                      wpi::math::Normal(pos_stddev, vel_stddev, accel_stddev);
   }
 
   wpi::math::Vectord<4> P0{0.001, 0.001, 10, 10};

wpimath/src/test/native/cpp/random/NormalTest.cpp

@@ -0,0 +1,23 @@
+// 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.
+
+#include "wpi/math/random/Normal.hpp"
+
+#include <Eigen/Core>
+#include <gtest/gtest.h>
+
+TEST(NormalTest, NormalParameterPack) {
+  [[maybe_unused]]
+  Eigen::Vector<double, 2> vec = wpi::math::Normal(2.0, 3.0);
+}
+
+TEST(NormalTest, NormalArray) {
+  [[maybe_unused]]
+  Eigen::Vector<double, 2> vec = wpi::math::Normal<2>({2.0, 3.0});
+}
+
+TEST(NormalTest, NormalDynamic) {
+  [[maybe_unused]]
+  Eigen::VectorXd vec = wpi::math::Normal(std::vector{2.0, 3.0});
+}

wpimath/src/test/native/cpp/system/LinearSystemUtilTest.cpp

@@ -0,0 +1,56 @@
+// 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.
+
+#include "wpi/math/system/LinearSystemUtil.hpp"
+
+#include <Eigen/Core>
+#include <gtest/gtest.h>
+
+TEST(LinearSystemUtilTest, IsStabilizable) {
+  Eigen::Matrix<double, 2, 1> B{0, 1};
+
+  // First eigenvalue is uncontrollable and unstable.
+  // Second eigenvalue is controllable and stable.
+  EXPECT_FALSE((wpi::math::IsStabilizable<2, 1>(
+      Eigen::Matrix<double, 2, 2>{{1.2, 0}, {0, 0.5}}, B)));
+
+  // First eigenvalue is uncontrollable and marginally stable.
+  // Second eigenvalue is controllable and stable.
+  EXPECT_FALSE((wpi::math::IsStabilizable<2, 1>(
+      Eigen::Matrix<double, 2, 2>{{1, 0}, {0, 0.5}}, B)));
+
+  // First eigenvalue is uncontrollable and stable.
+  // Second eigenvalue is controllable and stable.
+  EXPECT_TRUE((wpi::math::IsStabilizable<2, 1>(
+      Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 0.5}}, B)));
+
+  // First eigenvalue is uncontrollable and stable.
+  // Second eigenvalue is controllable and unstable.
+  EXPECT_TRUE((wpi::math::IsStabilizable<2, 1>(
+      Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 1.2}}, B)));
+}
+
+TEST(LinearSystemUtilTest, IsDetectable) {
+  Eigen::Matrix<double, 1, 2> C{0, 1};
+
+  // First eigenvalue is unobservable and unstable.
+  // Second eigenvalue is observable and stable.
+  EXPECT_FALSE((wpi::math::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((wpi::math::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((wpi::math::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((wpi::math::IsDetectable<2, 1>(
+      Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 1.2}}, C)));
+}

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

@@ -9,8 +9,7 @@
 #include "wpi/math/linalg/EigenCore.hpp"
 
 TEST(StateSpaceUtilTest, CostParameterPack) {
-  constexpr wpi::math::Matrixd<3, 3> mat =
-      wpi::math::MakeCostMatrix(1.0, 2.0, 3.0);
+  constexpr wpi::math::Matrixd<3, 3> mat = wpi::math::CostMatrix(1.0, 2.0, 3.0);
   EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
   EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
   EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -24,7 +23,7 @@ TEST(StateSpaceUtilTest, CostParameterPack) {
 
 TEST(StateSpaceUtilTest, CostArray) {
   constexpr wpi::math::Matrixd<3, 3> mat =
-      wpi::math::MakeCostMatrix<3>({1.0, 2.0, 3.0});
+      wpi::math::CostMatrix<3>({1.0, 2.0, 3.0});
   EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
   EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
   EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -37,7 +36,7 @@ TEST(StateSpaceUtilTest, CostArray) {
 }
 
 TEST(StateSpaceUtilTest, CostDynamic) {
-  Eigen::MatrixXd mat = wpi::math::MakeCostMatrix(std::vector{1.0, 2.0, 3.0});
+  Eigen::MatrixXd mat = wpi::math::CostMatrix(std::vector{1.0, 2.0, 3.0});
   EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
   EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
   EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -51,7 +50,7 @@ TEST(StateSpaceUtilTest, CostDynamic) {
 
 TEST(StateSpaceUtilTest, CovParameterPack) {
   constexpr wpi::math::Matrixd<3, 3> mat =
-      wpi::math::MakeCovMatrix(1.0, 2.0, 3.0);
+      wpi::math::CovarianceMatrix(1.0, 2.0, 3.0);
   EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
   EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
   EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -65,7 +64,7 @@ TEST(StateSpaceUtilTest, CovParameterPack) {
 
 TEST(StateSpaceUtilTest, CovArray) {
   constexpr wpi::math::Matrixd<3, 3> mat =
-      wpi::math::MakeCovMatrix<3>({1.0, 2.0, 3.0});
+      wpi::math::CovarianceMatrix<3>({1.0, 2.0, 3.0});
   EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
   EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
   EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -78,7 +77,7 @@ TEST(StateSpaceUtilTest, CovArray) {
 }
 
 TEST(StateSpaceUtilTest, CovDynamic) {
-  Eigen::MatrixXd mat = wpi::math::MakeCovMatrix(std::vector{1.0, 2.0, 3.0});
+  Eigen::MatrixXd mat = wpi::math::CovarianceMatrix(std::vector{1.0, 2.0, 3.0});
   EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
   EXPECT_NEAR(mat(0, 1), 0.0, 1e-3);
   EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
@@ -90,65 +89,17 @@ TEST(StateSpaceUtilTest, CovDynamic) {
   EXPECT_NEAR(mat(2, 2), 9.0, 1e-3);
 }
 
-TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) {
-  [[maybe_unused]]
-  wpi::math::Vectord<2> vec = wpi::math::MakeWhiteNoiseVector(2.0, 3.0);
-}
-
-TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) {
-  [[maybe_unused]]
-  wpi::math::Vectord<2> vec = wpi::math::MakeWhiteNoiseVector<2>({2.0, 3.0});
-}
-
-TEST(StateSpaceUtilTest, WhiteNoiseVectorDynamic) {
-  [[maybe_unused]]
-  Eigen::VectorXd vec = wpi::math::MakeWhiteNoiseVector(std::vector{2.0, 3.0});
-}
-
-TEST(StateSpaceUtilTest, IsStabilizable) {
-  wpi::math::Matrixd<2, 1> B{0, 1};
-
-  // First eigenvalue is uncontrollable and unstable.
-  // Second eigenvalue is controllable and stable.
-  EXPECT_FALSE((wpi::math::IsStabilizable<2, 1>(
-      wpi::math::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, B)));
-
-  // First eigenvalue is uncontrollable and marginally stable.
-  // Second eigenvalue is controllable and stable.
-  EXPECT_FALSE((wpi::math::IsStabilizable<2, 1>(
-      wpi::math::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, B)));
-
-  // First eigenvalue is uncontrollable and stable.
-  // Second eigenvalue is controllable and stable.
-  EXPECT_TRUE((wpi::math::IsStabilizable<2, 1>(
-      wpi::math::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, B)));
-
-  // First eigenvalue is uncontrollable and stable.
-  // Second eigenvalue is controllable and unstable.
-  EXPECT_TRUE((wpi::math::IsStabilizable<2, 1>(
-      wpi::math::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, B)));
-}
-
-TEST(StateSpaceUtilTest, IsDetectable) {
-  wpi::math::Matrixd<1, 2> C{0, 1};
-
-  // First eigenvalue is unobservable and unstable.
-  // Second eigenvalue is observable and stable.
-  EXPECT_FALSE((wpi::math::IsDetectable<2, 1>(
-      wpi::math::Matrixd<2, 2>{{1.2, 0}, {0, 0.5}}, C)));
-
-  // First eigenvalue is unobservable and marginally stable.
-  // Second eigenvalue is observable and stable.
-  EXPECT_FALSE((wpi::math::IsDetectable<2, 1>(
-      wpi::math::Matrixd<2, 2>{{1, 0}, {0, 0.5}}, C)));
-
-  // First eigenvalue is unobservable and stable.
-  // Second eigenvalue is observable and stable.
-  EXPECT_TRUE((wpi::math::IsDetectable<2, 1>(
-      wpi::math::Matrixd<2, 2>{{0.2, 0}, {0, 0.5}}, C)));
-
-  // First eigenvalue is unobservable and stable.
-  // Second eigenvalue is observable and unstable.
-  EXPECT_TRUE((wpi::math::IsDetectable<2, 1>(
-      wpi::math::Matrixd<2, 2>{{0.2, 0}, {0, 1.2}}, C)));
+TEST(StateSpaceUtilTest, DesaturateInputVector) {
+  constexpr Eigen::Vector2d vec1{{10.0, 12.0}};
+  EXPECT_EQ(wpi::math::DesaturateInputVector<2>(vec1, 12.0), vec1);
+  EXPECT_EQ(wpi::math::DesaturateInputVector<2>(vec1, 10.0),
+            (Eigen::Vector2d{{25.0 / 3.0}, {10.0}}));
+
+  constexpr Eigen::Vector2d vec2{{10.0, -12.0}};
+  EXPECT_EQ(wpi::math::DesaturateInputVector<2>(vec2, 12.0), vec2);
+  EXPECT_EQ(wpi::math::DesaturateInputVector<2>(vec2, 10.0),
+            (Eigen::Vector2d{{25.0 / 3.0}, {-10.0}}));
+
+  constexpr Eigen::Vector2d vec3{{0.0, 0.0}};
+  EXPECT_EQ(wpi::math::DesaturateInputVector<2>(vec3, 12.0), vec3);
 }