[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));
   }
 }