[wpimath] Refactor StateSpaceUtil into separate files

* 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()

Author: calcmogul

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),