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