[wpimath] Refactor StateSpaceUtil into separate files

wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp

@@ -6,6 +6,7 @@
 
 #include <utility>
 
+#include "wpi/math/random/Normal.hpp"
 #include "wpi/math/system/NumericalIntegration.hpp"
 #include "wpi/math/system/plant/LinearSystemId.hpp"
 #include "wpi/math/util/StateSpaceUtil.hpp"
@@ -61,7 +62,7 @@ void DifferentialDrivetrainSim::SetGearing(double newGearing) {
 void DifferentialDrivetrainSim::Update(wpi::units::second_t dt) {
   m_x = wpi::math::RKDP([this](auto& x, auto& u) { return Dynamics(x, u); },
                         m_x, m_u, dt);
-  m_y = m_x + wpi::math::MakeWhiteNoiseVector<7>(m_measurementStdDevs);
+  m_y = m_x + wpi::math::Normal<7>(m_measurementStdDevs);
 }
 
 double DifferentialDrivetrainSim::GetGearing() const {

wpilibc/src/main/native/include/wpi/simulation/LinearSystemSim.hpp

@@ -7,9 +7,9 @@
 #include <array>
 
 #include "wpi/math/linalg/EigenCore.hpp"
+#include "wpi/math/random/Normal.hpp"
 #include "wpi/math/system/LinearSystem.hpp"
 #include "wpi/math/util/StateSpaceUtil.hpp"
-#include "wpi/units/current.hpp"
 #include "wpi/units/time.hpp"
 
 namespace wpi::sim {
@@ -61,7 +61,7 @@ class LinearSystemSim {
     // Add noise. If the user did not pass a noise vector to the
     // constructor, then this method will not do anything because
     // the standard deviations default to zero.
-    m_y += wpi::math::MakeWhiteNoiseVector<Outputs>(m_measurementStdDevs);
+    m_y += wpi::math::Normal<Outputs>(m_measurementStdDevs);
   }
 
   /**

wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.hpp

@@ -5,7 +5,7 @@
 #pragma once
 
 #include "wpi/math/geometry/Pose2d.hpp"
-#include "wpi/math/util/StateSpaceUtil.hpp"
+#include "wpi/math/random/Normal.hpp"
 
 /**
  * This dummy class represents a global measurement sensor, such as a computer
@@ -15,7 +15,7 @@ class ExampleGlobalMeasurementSensor {
  public:
   static wpi::math::Pose2d GetEstimatedGlobalPose(
       const wpi::math::Pose2d& estimatedRobotPose) {
-    auto randVec = wpi::math::MakeWhiteNoiseVector(0.1, 0.1, 0.1);
+    auto randVec = wpi::math::Normal(0.1, 0.1, 0.1);
     return wpi::math::Pose2d{
         estimatedRobotPose.X() + wpi::units::meter_t{randVec(0)},
         estimatedRobotPose.Y() + wpi::units::meter_t{randVec(1)},

wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.hpp

@@ -5,7 +5,7 @@
 #pragma once
 
 #include "wpi/math/geometry/Pose2d.hpp"
-#include "wpi/math/util/StateSpaceUtil.hpp"
+#include "wpi/math/random/Normal.hpp"
 
 /**
  * This dummy class represents a global measurement sensor, such as a computer
@@ -15,7 +15,7 @@ class ExampleGlobalMeasurementSensor {
  public:
   static wpi::math::Pose2d GetEstimatedGlobalPose(
       const wpi::math::Pose2d& estimatedRobotPose) {
-    auto randVec = wpi::math::MakeWhiteNoiseVector(0.1, 0.1, 0.1);
+    auto randVec = wpi::math::Normal(0.1, 0.1, 0.1);
     return wpi::math::Pose2d{
         estimatedRobotPose.X() + wpi::units::meter_t{randVec(0)},
         estimatedRobotPose.Y() + wpi::units::meter_t{randVec(1)},

wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.hpp

@@ -5,7 +5,7 @@
 #pragma once
 
 #include "wpi/math/geometry/Pose2d.hpp"
-#include "wpi/math/util/StateSpaceUtil.hpp"
+#include "wpi/math/random/Normal.hpp"
 
 /**
  * This dummy class represents a global measurement sensor, such as a computer
@@ -15,7 +15,7 @@ class ExampleGlobalMeasurementSensor {
  public:
   static wpi::math::Pose2d GetEstimatedGlobalPose(
       const wpi::math::Pose2d& estimatedRobotPose) {
-    auto randVec = wpi::math::MakeWhiteNoiseVector(0.1, 0.1, 0.1);
+    auto randVec = wpi::math::Normal(0.1, 0.1, 0.1);
     return wpi::math::Pose2d{
         estimatedRobotPose.X() + wpi::units::meter_t{randVec(0)},
         estimatedRobotPose.Y() + wpi::units::meter_t{randVec(1)},

wpilibj/src/main/java/org/wpilib/simulation/DifferentialDrivetrainSim.java

@@ -11,6 +11,7 @@
 import org.wpilib.math.numbers.N1;
 import org.wpilib.math.numbers.N2;
 import org.wpilib.math.numbers.N7;
+import org.wpilib.math.random.Normal;
 import org.wpilib.math.system.LinearSystem;
 import org.wpilib.math.system.NumericalIntegration;
 import org.wpilib.math.system.plant.DCMotor;
@@ -146,7 +147,7 @@ public void update(double dt) {
     m_x = NumericalIntegration.rkdp(this::getDynamics, m_x, m_u, dt);
     m_y = m_x;
     if (m_measurementStdDevs != null) {
-      m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs));
+      m_y = m_y.plus(Normal.normal(m_measurementStdDevs));
     }
   }
 

wpilibj/src/main/java/org/wpilib/simulation/LinearSystemSim.java

@@ -8,6 +8,7 @@
 import org.ejml.simple.SimpleMatrix;
 import org.wpilib.math.linalg.Matrix;
 import org.wpilib.math.numbers.N1;
+import org.wpilib.math.random.Normal;
 import org.wpilib.math.system.LinearSystem;
 import org.wpilib.math.util.Num;
 import org.wpilib.math.util.StateSpaceUtil;
@@ -83,7 +84,7 @@ public void update(double dt) {
 
     // Add measurement noise.
     if (m_measurementStdDevs != null) {
-      m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs));
+      m_y = m_y.plus(Normal.normal(m_measurementStdDevs));
     }
   }
 

wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/ExampleGlobalMeasurementSensor.java

@@ -7,7 +7,7 @@
 import org.wpilib.math.geometry.Pose2d;
 import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.linalg.VecBuilder;
-import org.wpilib.math.util.StateSpaceUtil;
+import org.wpilib.math.random.Normal;
 import org.wpilib.math.util.Units;
 
 /** This dummy class represents a global measurement sensor, such as a computer vision solution. */
@@ -22,8 +22,7 @@ private ExampleGlobalMeasurementSensor() {
    * @param estimatedRobotPose The robot pose.
    */
   public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) {
-    var rand =
-        StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+    var rand = Normal.normal(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
     return new Pose2d(
         estimatedRobotPose.getX() + rand.get(0, 0),
         estimatedRobotPose.getY() + rand.get(1, 0),

wpilibjExamples/src/main/java/org/wpilib/examples/mecanumdriveposeestimator/ExampleGlobalMeasurementSensor.java

@@ -7,7 +7,7 @@
 import org.wpilib.math.geometry.Pose2d;
 import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.linalg.VecBuilder;
-import org.wpilib.math.util.StateSpaceUtil;
+import org.wpilib.math.random.Normal;
 import org.wpilib.math.util.Units;
 
 /** This dummy class represents a global measurement sensor, such as a computer vision solution. */
@@ -22,8 +22,7 @@ private ExampleGlobalMeasurementSensor() {
    * @param estimatedRobotPose The robot pose.
    */
   public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) {
-    var rand =
-        StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+    var rand = Normal.normal(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
     return new Pose2d(
         estimatedRobotPose.getX() + rand.get(0, 0),
         estimatedRobotPose.getY() + rand.get(1, 0),

wpilibjExamples/src/main/java/org/wpilib/examples/swervedriveposeestimator/ExampleGlobalMeasurementSensor.java

@@ -7,7 +7,7 @@
 import org.wpilib.math.geometry.Pose2d;
 import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.linalg.VecBuilder;
-import org.wpilib.math.util.StateSpaceUtil;
+import org.wpilib.math.random.Normal;
 import org.wpilib.math.util.Units;
 
 /** This dummy class represents a global measurement sensor, such as a computer vision solution. */
@@ -22,8 +22,7 @@ private ExampleGlobalMeasurementSensor() {
    * @param estimatedRobotPose The robot pose.
    */
   public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) {
-    var rand =
-        StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+    var rand = Normal.normal(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
     return new Pose2d(
         estimatedRobotPose.getX() + rand.get(0, 0),
         estimatedRobotPose.getY() + rand.get(1, 0),

wpimath/CMakeLists.txt

@@ -12,7 +12,7 @@ file(
     src/main/native/cpp/jni/EigenJNI.cpp
     src/main/native/cpp/jni/Ellipse2dJNI.cpp
     src/main/native/cpp/jni/Exceptions.cpp
-    src/main/native/cpp/jni/StateSpaceUtilJNI.cpp
+    src/main/native/cpp/jni/LinearSystemUtilJNI.cpp
     src/main/native/cpp/jni/Transform3dJNI.cpp
     src/main/native/cpp/jni/Twist3dJNI.cpp
 )

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

@@ -74,8 +74,8 @@ public 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 LTVUnicycleController(double dt) {
    * @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 LinearQuadraticRegulator(
     this(
         plant.getA(),
         plant.getB(),
-        StateSpaceUtil.makeCostMatrix(qelms),
-        StateSpaceUtil.makeCostMatrix(relms),
+        StateSpaceUtil.costMatrix(qelms),
+        StateSpaceUtil.costMatrix(relms),
         dt);
   }
 
@@ -81,7 +81,7 @@ public LinearQuadraticRegulator(
       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.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 ExtendedKalmanFilter(
     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 ExtendedKalmanFilter(
 
     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 KalmanFilter(
 
     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 SteadyStateKalmanFilter(
 
     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 UnscentedKalmanFilter(
 
     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/StateSpaceUtilJNI.java → 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;
+  }
+}