diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp index e70c3fca0c..a307cbbd3f 100644 --- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp @@ -6,6 +6,7 @@ #include +#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 { diff --git a/wpilibc/src/main/native/include/wpi/simulation/LinearSystemSim.hpp b/wpilibc/src/main/native/include/wpi/simulation/LinearSystemSim.hpp index 69708cbb88..55a97c2e0d 100644 --- a/wpilibc/src/main/native/include/wpi/simulation/LinearSystemSim.hpp +++ b/wpilibc/src/main/native/include/wpi/simulation/LinearSystemSim.hpp @@ -7,9 +7,9 @@ #include #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(m_measurementStdDevs); + m_y += wpi::math::Normal(m_measurementStdDevs); } /** diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.hpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.hpp index 6c34d126d3..a004c8e5df 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.hpp +++ b/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)}, diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.hpp b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.hpp index 6c34d126d3..a004c8e5df 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.hpp +++ b/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)}, diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.hpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.hpp index 6c34d126d3..a004c8e5df 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.hpp +++ b/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)}, diff --git a/wpilibj/src/main/java/org/wpilib/simulation/DifferentialDrivetrainSim.java b/wpilibj/src/main/java/org/wpilib/simulation/DifferentialDrivetrainSim.java index d49b6b1973..c5371a3035 100644 --- a/wpilibj/src/main/java/org/wpilib/simulation/DifferentialDrivetrainSim.java +++ b/wpilibj/src/main/java/org/wpilib/simulation/DifferentialDrivetrainSim.java @@ -11,6 +11,7 @@ import org.wpilib.math.linalg.VecBuilder; 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 class DifferentialDrivetrainSim { 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)); } } diff --git a/wpilibj/src/main/java/org/wpilib/simulation/LinearSystemSim.java b/wpilibj/src/main/java/org/wpilib/simulation/LinearSystemSim.java index 43f517b5a9..c20de19c4c 100644 --- a/wpilibj/src/main/java/org/wpilib/simulation/LinearSystemSim.java +++ b/wpilibj/src/main/java/org/wpilib/simulation/LinearSystemSim.java @@ -8,6 +8,7 @@ import org.ejml.MatrixDimensionException; 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 class LinearSystemSim qelems, Vector 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; } diff --git a/wpimath/src/main/java/org/wpilib/math/controller/LinearQuadraticRegulator.java b/wpimath/src/main/java/org/wpilib/math/controller/LinearQuadraticRegulator.java index 3143b2eefc..bc43dba74f 100644 --- a/wpimath/src/main/java/org/wpilib/math/controller/LinearQuadraticRegulator.java +++ b/wpimath/src/main/java/org/wpilib/math/controller/LinearQuadraticRegulator.java @@ -56,8 +56,8 @@ public class LinearQuadraticRegulator qelms, Vector relms, double dt) { - this(A, B, StateSpaceUtil.makeCostMatrix(qelms), StateSpaceUtil.makeCostMatrix(relms), dt); + this(A, B, StateSpaceUtil.costMatrix(qelms), StateSpaceUtil.costMatrix(relms), dt); } /** diff --git a/wpimath/src/main/java/org/wpilib/math/estimator/ExtendedKalmanFilter.java b/wpimath/src/main/java/org/wpilib/math/estimator/ExtendedKalmanFilter.java index 0286551035..63bad3206f 100644 --- a/wpimath/src/main/java/org/wpilib/math/estimator/ExtendedKalmanFilter.java +++ b/wpimath/src/main/java/org/wpilib/math/estimator/ExtendedKalmanFilter.java @@ -9,6 +9,7 @@ import org.wpilib.math.linalg.DARE; 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 class ExtendedKalmanFilter(states, states); diff --git a/wpimath/src/main/java/org/wpilib/math/estimator/KalmanFilter.java b/wpimath/src/main/java/org/wpilib/math/estimator/KalmanFilter.java index 7440e0c221..6f1f61bc81 100644 --- a/wpimath/src/main/java/org/wpilib/math/estimator/KalmanFilter.java +++ b/wpimath/src/main/java/org/wpilib/math/estimator/KalmanFilter.java @@ -71,8 +71,8 @@ public class KalmanFilter 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 Matrix normal(Matrix stdDevs) { + Matrix 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; + } +} diff --git a/wpimath/src/main/java/org/wpilib/math/system/LinearSystemUtil.java b/wpimath/src/main/java/org/wpilib/math/system/LinearSystemUtil.java new file mode 100644 index 0000000000..57fedbc6f8 --- /dev/null +++ b/wpimath/src/main/java/org/wpilib/math/system/LinearSystemUtil.java @@ -0,0 +1,54 @@ +// 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 org.wpilib.math.jni.LinearSystemUtilJNI; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.util.Num; + +/** Linear system utilities. */ +public final class LinearSystemUtil { + private LinearSystemUtil() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + /** + * Returns true if (A, B) is a stabilizable pair. + * + *

(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have + * absolute values less than one, where an eigenvalue is uncontrollable if rank([λI - A, B]) %3C n + * where n is the number of states. + * + * @param Num representing the size of A. + * @param Num representing the columns of B. + * @param A System matrix. + * @param B Input matrix. + * @return If the system is stabilizable. + */ + public static boolean isStabilizable( + Matrix A, Matrix B) { + return LinearSystemUtilJNI.isStabilizable( + A.getNumRows(), B.getNumCols(), A.getData(), B.getData()); + } + + /** + * Returns true if (A, C) is a detectable pair. + * + *

(A, C) is detectable if and only if the unobservable eigenvalues of A, if any, have absolute + * values less than one, where an eigenvalue is unobservable if rank([λI - A; C]) %3C n where n is + * the number of states. + * + * @param Num representing the size of A. + * @param Num representing the rows of C. + * @param A System matrix. + * @param C Output matrix. + * @return If the system is detectable. + */ + public static boolean isDetectable( + Matrix A, Matrix C) { + return LinearSystemUtilJNI.isStabilizable( + A.getNumRows(), C.getNumRows(), A.transpose().getData(), C.transpose().getData()); + } +} diff --git a/wpimath/src/main/java/org/wpilib/math/util/StateSpaceUtil.java b/wpimath/src/main/java/org/wpilib/math/util/StateSpaceUtil.java index 78d9f12900..c9921830e8 100644 --- a/wpimath/src/main/java/org/wpilib/math/util/StateSpaceUtil.java +++ b/wpimath/src/main/java/org/wpilib/math/util/StateSpaceUtil.java @@ -4,62 +4,16 @@ package org.wpilib.math.util; -import java.util.Random; import org.ejml.simple.SimpleMatrix; -import org.wpilib.math.geometry.Pose2d; -import org.wpilib.math.jni.StateSpaceUtilJNI; import org.wpilib.math.linalg.Matrix; -import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.numbers.N1; -import org.wpilib.math.numbers.N3; -import org.wpilib.math.numbers.N4; /** State-space utilities. */ public final class StateSpaceUtil { - private static Random rand = new Random(); - private StateSpaceUtil() { throw new UnsupportedOperationException("This is a utility class!"); } - /** - * Creates a covariance matrix from the given vector for use with Kalman filters. - * - *

Each element is squared and placed on the covariance matrix diagonal. - * - * @param Num representing the states of the system. - * @param states A Nat representing the states of the system. - * @param stdDevs For a Q matrix, its elements are the standard deviations of each state from how - * the model behaves. For an R matrix, its elements are the standard deviations for each - * output measurement. - * @return Process noise or measurement noise covariance matrix. - */ - public static Matrix makeCovarianceMatrix( - Nat states, Matrix stdDevs) { - var result = new Matrix<>(states, states); - for (int i = 0; i < states.getNum(); i++) { - result.set(i, i, Math.pow(stdDevs.get(i, 0), 2)); - } - return result; - } - - /** - * Creates a vector of normally distributed white noise with the given noise intensities for each - * element. - * - * @param 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 noise - * vector. - * @return White noise vector. - */ - public static Matrix makeWhiteNoiseVector(Matrix stdDevs) { - Matrix 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; - } - /** * Creates a cost matrix from the given vector for use with LQR. * @@ -73,7 +27,7 @@ public final class StateSpaceUtil { * excursions of the control inputs from no actuation. * @return State excursion or control effort cost matrix. */ - public static Matrix makeCostMatrix( + public static Matrix costMatrix( Matrix tolerances) { Matrix result = new Matrix<>(new SimpleMatrix(tolerances.getNumRows(), tolerances.getNumRows())); @@ -91,70 +45,22 @@ public final class StateSpaceUtil { } /** - * Returns true if (A, B) is a stabilizable pair. + * Creates a covariance matrix from the given vector for use with Kalman filters. * - *

(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have - * absolute values less than one, where an eigenvalue is uncontrollable if rank([λI - A, B]) %3C n - * where n is the number of states. + *

Each element is squared and placed on the covariance matrix diagonal. * - * @param Num representing the size of A. - * @param Num representing the columns of B. - * @param A System matrix. - * @param B Input matrix. - * @return If the system is stabilizable. + * @param Num representing the states of the system. + * @param states A Nat representing the states of the system. + * @param stdDevs For a Q matrix, its elements are the standard deviations of each state from how + * the model behaves. For an R matrix, its elements are the standard deviations for each + * output measurement. + * @return Process noise or measurement noise covariance matrix. */ - public static boolean isStabilizable( - Matrix A, Matrix B) { - return StateSpaceUtilJNI.isStabilizable( - A.getNumRows(), B.getNumCols(), A.getData(), B.getData()); - } - - /** - * Returns true if (A, C) is a detectable pair. - * - *

(A, C) is detectable if and only if the unobservable eigenvalues of A, if any, have absolute - * values less than one, where an eigenvalue is unobservable if rank([λI - A; C]) %3C n where n is - * the number of states. - * - * @param Num representing the size of A. - * @param Num representing the rows of C. - * @param A System matrix. - * @param C Output matrix. - * @return If the system is detectable. - */ - public static boolean isDetectable( - Matrix A, Matrix C) { - return StateSpaceUtilJNI.isStabilizable( - A.getNumRows(), C.getNumRows(), A.transpose().getData(), C.transpose().getData()); - } - - /** - * Convert a {@link Pose2d} to a vector of [x, y, theta], where theta is in radians. - * - * @param pose A pose to convert to a vector. - * @return The given pose in vector form, with the third element, theta, in radians. - * @deprecated Create the vector manually instead. If you were using this as an intermediate step - * for constructing affine transformations, use {@link Pose2d#toMatrix()} instead. - */ - @Deprecated(forRemoval = true, since = "2025") - public static Matrix poseToVector(Pose2d pose) { - return VecBuilder.fill(pose.getX(), pose.getY(), pose.getRotation().getRadians()); - } - - /** - * Clamp the input u to the min and max. - * - * @param u The input to clamp. - * @param umin The minimum input magnitude. - * @param umax The maximum input magnitude. - * @param Number of inputs. - * @return The clamped input. - */ - public static Matrix clampInputMaxMagnitude( - Matrix u, Matrix umin, Matrix umax) { - var result = new Matrix(new SimpleMatrix(u.getNumRows(), 1)); - for (int i = 0; i < u.getNumRows(); i++) { - result.set(i, 0, Math.clamp(u.get(i, 0), umin.get(i, 0), umax.get(i, 0))); + public static Matrix covarianceMatrix( + Nat states, Matrix stdDevs) { + var result = new Matrix<>(states, states); + for (int i = 0; i < states.getNum(); i++) { + result.set(i, i, Math.pow(stdDevs.get(i, 0), 2)); } return result; } @@ -170,46 +76,6 @@ public final class StateSpaceUtil { */ public static Matrix desaturateInputVector( Matrix u, double maxMagnitude) { - double maxValue = u.maxAbs(); - boolean isCapped = maxValue > maxMagnitude; - - if (isCapped) { - return u.times(maxMagnitude / maxValue); - } - return u; - } - - /** - * Convert a {@link Pose2d} to a vector of [x, y, cos(theta), sin(theta)], where theta is in - * radians. - * - * @param pose A pose to convert to a vector. - * @return The given pose in as a 4x1 vector of x, y, cos(theta), and sin(theta). - * @deprecated Create the vector manually instead. If you were using this as an intermediate step - * for constructing affine transformations, use {@link Pose2d#toMatrix()} instead. - */ - @Deprecated(forRemoval = true, since = "2025") - public static Matrix poseTo4dVector(Pose2d pose) { - return VecBuilder.fill( - pose.getTranslation().getX(), - pose.getTranslation().getY(), - pose.getRotation().getCos(), - pose.getRotation().getSin()); - } - - /** - * Convert a {@link Pose2d} to a vector of [x, y, theta], where theta is in radians. - * - * @param pose A pose to convert to a vector. - * @return The given pose in vector form, with the third element, theta, in radians. - * @deprecated Create the vector manually instead. If you were using this as an intermediate step - * for constructing affine transformations, use {@link Pose2d#toMatrix()} instead. - */ - @Deprecated(forRemoval = true, since = "2025") - public static Matrix poseTo3dVector(Pose2d pose) { - return VecBuilder.fill( - pose.getTranslation().getX(), - pose.getTranslation().getY(), - pose.getRotation().getRadians()); + return u.times(Math.min(1.0, maxMagnitude / u.maxAbs())); } } diff --git a/wpimath/src/main/native/cpp/jni/StateSpaceUtilJNI.cpp b/wpimath/src/main/native/cpp/jni/LinearSystemUtilJNI.cpp similarity index 83% rename from wpimath/src/main/native/cpp/jni/StateSpaceUtilJNI.cpp rename to wpimath/src/main/native/cpp/jni/LinearSystemUtilJNI.cpp index 86176f1f60..ded5b735f4 100644 --- a/wpimath/src/main/native/cpp/jni/StateSpaceUtilJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/LinearSystemUtilJNI.cpp @@ -6,8 +6,8 @@ #include -#include "org_wpilib_math_jni_StateSpaceUtilJNI.h" -#include "wpi/math/util/StateSpaceUtil.hpp" +#include "org_wpilib_math_jni_LinearSystemUtilJNI.h" +#include "wpi/math/system/LinearSystemUtil.hpp" #include "wpi/util/jni_util.hpp" using namespace wpi::util::java; @@ -15,12 +15,12 @@ using namespace wpi::util::java; extern "C" { /* - * Class: org_wpilib_math_jni_StateSpaceUtilJNI + * Class: org_wpilib_math_jni_LinearSystemUtilJNI * Method: isStabilizable * Signature: (II[D[D)Z */ JNIEXPORT jboolean JNICALL -Java_org_wpilib_math_jni_StateSpaceUtilJNI_isStabilizable +Java_org_wpilib_math_jni_LinearSystemUtilJNI_isStabilizable (JNIEnv* env, jclass, jint states, jint inputs, jdoubleArray aSrc, jdoubleArray bSrc) { diff --git a/wpimath/src/main/native/cpp/random/Normal.cpp b/wpimath/src/main/native/cpp/random/Normal.cpp new file mode 100644 index 0000000000..5944235325 --- /dev/null +++ b/wpimath/src/main/native/cpp/random/Normal.cpp @@ -0,0 +1,32 @@ +// 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 +#include + +#include + +namespace wpi::math { + +Eigen::VectorXd Normal(const std::span stdDevs) { + std::random_device rd; + std::mt19937 gen{rd()}; + + Eigen::VectorXd result{stdDevs.size()}; + for (size_t i = 0; i < stdDevs.size(); ++i) { + // Passing a standard deviation of 0.0 to std::normal_distribution is + // undefined behavior + if (stdDevs[i] == 0.0) { + result(i) = 0.0; + } else { + std::normal_distribution distr{0.0, stdDevs[i]}; + result(i) = distr(gen); + } + } + return result; +} + +} // namespace wpi::math diff --git a/wpimath/src/main/native/cpp/util/LinearSystemUtil.cpp b/wpimath/src/main/native/cpp/util/LinearSystemUtil.cpp new file mode 100644 index 0000000000..db4aa784dc --- /dev/null +++ b/wpimath/src/main/native/cpp/util/LinearSystemUtil.cpp @@ -0,0 +1,21 @@ +// 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 + +namespace wpi::math { + +template bool IsStabilizable<1, 1>(const Eigen::Matrix& A, + const Eigen::Matrix& B); +template bool IsStabilizable<2, 1>(const Eigen::Matrix& A, + const Eigen::Matrix& B); +template bool IsStabilizable( + const Eigen::MatrixXd& A, const Eigen::MatrixXd& B); + +template bool IsDetectable( + const Eigen::MatrixXd& A, const Eigen::MatrixXd& C); + +} // namespace wpi::math diff --git a/wpimath/src/main/native/cpp/util/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/util/StateSpaceUtil.cpp index 52eb462dbc..af4c36d586 100644 --- a/wpimath/src/main/native/cpp/util/StateSpaceUtil.cpp +++ b/wpimath/src/main/native/cpp/util/StateSpaceUtil.cpp @@ -5,27 +5,11 @@ #include "wpi/math/util/StateSpaceUtil.hpp" #include +#include namespace wpi::math { -template bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A, - const Matrixd<1, 1>& B); -template bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, - const Matrixd<2, 1>& B); -template bool IsStabilizable( - const Eigen::MatrixXd& A, const Eigen::MatrixXd& B); - -template bool IsDetectable( - const Eigen::MatrixXd& A, const Eigen::MatrixXd& C); - -template Eigen::VectorXd ClampInputMaxMagnitude( - const Eigen::VectorXd& u, const Eigen::VectorXd& umin, - const Eigen::VectorXd& umax); - -template Eigen::VectorXd DesaturateInputVector( - const Eigen::VectorXd& u, double maxMagnitude); - -Eigen::MatrixXd MakeCostMatrix(const std::span costs) { +Eigen::MatrixXd CostMatrix(const std::span costs) { Eigen::MatrixXd result{costs.size(), costs.size()}; result.setZero(); @@ -39,25 +23,7 @@ Eigen::MatrixXd MakeCostMatrix(const std::span costs) { return result; } -Eigen::VectorXd MakeWhiteNoiseVector(const std::span stdDevs) { - std::random_device rd; - std::mt19937 gen{rd()}; - - Eigen::VectorXd result{stdDevs.size()}; - for (size_t i = 0; i < stdDevs.size(); ++i) { - // Passing a standard deviation of 0.0 to std::normal_distribution is - // undefined behavior - if (stdDevs[i] == 0.0) { - result(i) = 0.0; - } else { - std::normal_distribution distr{0.0, stdDevs[i]}; - result(i) = distr(gen); - } - } - return result; -} - -Eigen::MatrixXd MakeCovMatrix(const std::span stdDevs) { +Eigen::MatrixXd CovarianceMatrix(const std::span stdDevs) { Eigen::MatrixXd result{stdDevs.size(), stdDevs.size()}; result.setZero(); @@ -68,4 +34,7 @@ Eigen::MatrixXd MakeCovMatrix(const std::span stdDevs) { return result; } +template Eigen::VectorXd DesaturateInputVector( + const Eigen::VectorXd& u, double maxMagnitude); + } // namespace wpi::math diff --git a/wpimath/src/main/native/include/wpi/math/controller/LTVDifferentialDriveController.hpp b/wpimath/src/main/native/include/wpi/math/controller/LTVDifferentialDriveController.hpp index 035e08f5d9..723ce9f1f4 100644 --- a/wpimath/src/main/native/include/wpi/math/controller/LTVDifferentialDriveController.hpp +++ b/wpimath/src/main/native/include/wpi/math/controller/LTVDifferentialDriveController.hpp @@ -62,8 +62,8 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController { : m_trackwidth{trackwidth}, m_A{plant.A()}, m_B{plant.B()}, - m_Q{wpi::math::MakeCostMatrix(Qelems)}, - m_R{wpi::math::MakeCostMatrix(Relems)}, + m_Q{wpi::math::CostMatrix(Qelems)}, + m_R{wpi::math::CostMatrix(Relems)}, m_dt{dt} {} /** diff --git a/wpimath/src/main/native/include/wpi/math/controller/LTVUnicycleController.hpp b/wpimath/src/main/native/include/wpi/math/controller/LTVUnicycleController.hpp index 5b13f0f31e..4bb1eb2df9 100644 --- a/wpimath/src/main/native/include/wpi/math/controller/LTVUnicycleController.hpp +++ b/wpimath/src/main/native/include/wpi/math/controller/LTVUnicycleController.hpp @@ -56,8 +56,8 @@ class WPILIB_DLLEXPORT LTVUnicycleController { LTVUnicycleController(const wpi::util::array& Qelems, const wpi::util::array& Relems, wpi::units::second_t dt) - : m_Q{wpi::math::MakeCostMatrix(Qelems)}, - m_R{wpi::math::MakeCostMatrix(Relems)}, + : m_Q{wpi::math::CostMatrix(Qelems)}, + m_R{wpi::math::CostMatrix(Relems)}, m_dt{dt} {} /** diff --git a/wpimath/src/main/native/include/wpi/math/controller/LinearQuadraticRegulator.hpp b/wpimath/src/main/native/include/wpi/math/controller/LinearQuadraticRegulator.hpp index 2f53b06cda..5a849cfd17 100644 --- a/wpimath/src/main/native/include/wpi/math/controller/LinearQuadraticRegulator.hpp +++ b/wpimath/src/main/native/include/wpi/math/controller/LinearQuadraticRegulator.hpp @@ -81,8 +81,8 @@ class LinearQuadraticRegulator { const Matrixd& B, const StateArray& Qelems, const InputArray& Relems, wpi::units::second_t dt) - : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems), - MakeCostMatrix(Relems), dt) {} + : LinearQuadraticRegulator(A, B, CostMatrix(Qelems), CostMatrix(Relems), + dt) {} /** * Constructs a controller with the given coefficients and plant. diff --git a/wpimath/src/main/native/include/wpi/math/estimator/ExtendedKalmanFilter.hpp b/wpimath/src/main/native/include/wpi/math/estimator/ExtendedKalmanFilter.hpp index 15a69198e1..3748cdad00 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/ExtendedKalmanFilter.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/ExtendedKalmanFilter.hpp @@ -16,6 +16,7 @@ #include "wpi/math/system/Discretization.hpp" #include "wpi/math/system/NumericalIntegration.hpp" #include "wpi/math/system/NumericalJacobian.hpp" +#include "wpi/math/util/MathShared.hpp" #include "wpi/math/util/StateSpaceUtil.hpp" #include "wpi/units/time.hpp" #include "wpi/util/array.hpp" @@ -79,8 +80,8 @@ class ExtendedKalmanFilter { const StateArray& stateStdDevs, const OutputArray& measurementStdDevs, wpi::units::second_t dt) : m_f(std::move(f)), m_h(std::move(h)) { - m_contQ = MakeCovMatrix(stateStdDevs); - m_contR = MakeCovMatrix(measurementStdDevs); + m_contQ = CovarianceMatrix(stateStdDevs); + m_contR = CovarianceMatrix(measurementStdDevs); m_residualFuncY = [](const OutputVector& a, const OutputVector& b) -> OutputVector { return a - b; @@ -170,8 +171,8 @@ class ExtendedKalmanFilter { m_h(std::move(h)), m_residualFuncY(std::move(residualFuncY)), m_addFuncX(std::move(addFuncX)) { - m_contQ = MakeCovMatrix(stateStdDevs); - m_contR = MakeCovMatrix(measurementStdDevs); + m_contQ = CovarianceMatrix(stateStdDevs); + m_contR = CovarianceMatrix(measurementStdDevs); m_dt = dt; StateMatrix contA = NumericalJacobianX( diff --git a/wpimath/src/main/native/include/wpi/math/estimator/KalmanFilter.hpp b/wpimath/src/main/native/include/wpi/math/estimator/KalmanFilter.hpp index 94a1dbf70d..9fac41cf62 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/KalmanFilter.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/KalmanFilter.hpp @@ -72,8 +72,8 @@ class KalmanFilter { const OutputArray& measurementStdDevs, wpi::units::second_t dt) { m_plant = &plant; - m_contQ = MakeCovMatrix(stateStdDevs); - m_contR = MakeCovMatrix(measurementStdDevs); + m_contQ = CovarianceMatrix(stateStdDevs); + m_contR = CovarianceMatrix(measurementStdDevs); m_dt = dt; // Find discrete A and Q diff --git a/wpimath/src/main/native/include/wpi/math/estimator/SteadyStateKalmanFilter.hpp b/wpimath/src/main/native/include/wpi/math/estimator/SteadyStateKalmanFilter.hpp index d692ac8dda..c21cebba5f 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/SteadyStateKalmanFilter.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/SteadyStateKalmanFilter.hpp @@ -76,8 +76,8 @@ class SteadyStateKalmanFilter { wpi::units::second_t dt) { m_plant = &plant; - auto contQ = MakeCovMatrix(stateStdDevs); - auto contR = MakeCovMatrix(measurementStdDevs); + auto contQ = CovarianceMatrix(stateStdDevs); + auto contR = CovarianceMatrix(measurementStdDevs); Matrixd discA; Matrixd discQ; diff --git a/wpimath/src/main/native/include/wpi/math/estimator/UnscentedKalmanFilter.hpp b/wpimath/src/main/native/include/wpi/math/estimator/UnscentedKalmanFilter.hpp index 7a7ce432ab..75a953ce34 100644 --- a/wpimath/src/main/native/include/wpi/math/estimator/UnscentedKalmanFilter.hpp +++ b/wpimath/src/main/native/include/wpi/math/estimator/UnscentedKalmanFilter.hpp @@ -93,8 +93,8 @@ class UnscentedKalmanFilter { const StateArray& stateStdDevs, const OutputArray& measurementStdDevs, wpi::units::second_t dt) : m_f(std::move(f)), m_h(std::move(h)) { - m_contQ = MakeCovMatrix(stateStdDevs); - m_contR = MakeCovMatrix(measurementStdDevs); + m_contQ = CovarianceMatrix(stateStdDevs); + m_contR = CovarianceMatrix(measurementStdDevs); m_meanFuncX = [](const Matrixd& sigmas, const Vectord& Wm) -> StateVector { return sigmas * Wm; @@ -169,8 +169,8 @@ class UnscentedKalmanFilter { m_residualFuncX(std::move(residualFuncX)), m_residualFuncY(std::move(residualFuncY)), m_addFuncX(std::move(addFuncX)) { - m_contQ = MakeCovMatrix(stateStdDevs); - m_contR = MakeCovMatrix(measurementStdDevs); + m_contQ = CovarianceMatrix(stateStdDevs); + m_contR = CovarianceMatrix(measurementStdDevs); m_dt = dt; Reset(); diff --git a/wpimath/src/main/native/include/wpi/math/linalg/DARE.hpp b/wpimath/src/main/native/include/wpi/math/linalg/DARE.hpp index 1d6af51401..ec5a27606c 100644 --- a/wpimath/src/main/native/include/wpi/math/linalg/DARE.hpp +++ b/wpimath/src/main/native/include/wpi/math/linalg/DARE.hpp @@ -10,7 +10,7 @@ #include #include -#include "wpi/math/util/StateSpaceUtil.hpp" +#include "wpi/math/system/LinearSystemUtil.hpp" #include "wpi/util/expected" namespace wpi::math { diff --git a/wpimath/src/main/native/include/wpi/math/random/Normal.hpp b/wpimath/src/main/native/include/wpi/math/random/Normal.hpp new file mode 100644 index 0000000000..6a6bfd0142 --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/random/Normal.hpp @@ -0,0 +1,85 @@ +// 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. + +#pragma once + +#include +#include +#include +#include + +#include + +#include "wpi/util/Algorithm.hpp" +#include "wpi/util/SymbolExports.hpp" + +namespace wpi::math { + +/** + * Creates a vector of normally distributed random values with the given + * standard deviations for each element. + * + * @param stdDevs An array whose elements are the standard deviations of each + * element of the random vector. + * @return Vector of normally distributed values. + */ +template ... Ts> +Eigen::Vector Normal(Ts... stdDevs) { + std::random_device rd; + std::mt19937 gen{rd()}; + + Eigen::Vector result; + wpi::util::for_each( + [&](int i, double stdDev) { + // Passing a standard deviation of 0.0 to std::normal_distribution is + // undefined behavior + if (stdDev == 0.0) { + result(i) = 0.0; + } else { + std::normal_distribution distr{0.0, stdDev}; + result(i) = distr(gen); + } + }, + stdDevs...); + return result; +} + +/** + * Creates a vector of normally distributed random values with the given + * standard deviations for each element. + * + * @param stdDevs An array whose elements are the standard deviations of each + * element of the random vector. + * @return Vector of normally distributed values. + */ +template +Eigen::Vector Normal(const std::array& stdDevs) { + std::random_device rd; + std::mt19937 gen{rd()}; + + Eigen::Vector result; + for (size_t i = 0; i < stdDevs.size(); ++i) { + // Passing a standard deviation of 0.0 to std::normal_distribution is + // undefined behavior + if (stdDevs[i] == 0.0) { + result(i) = 0.0; + } else { + std::normal_distribution distr{0.0, stdDevs[i]}; + result(i) = distr(gen); + } + } + return result; +} + +/** + * Creates a vector of normally distributed random values with the given + * standard deviations for each element. + * + * @param stdDevs A possibly variable length container whose elements are the + * standard deviations of each element of the random vector. + * @return Vector of normally distributed values. + */ +WPILIB_DLLEXPORT Eigen::VectorXd Normal(const std::span stdDevs); + +} // namespace wpi::math diff --git a/wpimath/src/main/native/include/wpi/math/system/LinearSystemUtil.hpp b/wpimath/src/main/native/include/wpi/math/system/LinearSystemUtil.hpp new file mode 100644 index 0000000000..c5aa4533ce --- /dev/null +++ b/wpimath/src/main/native/include/wpi/math/system/LinearSystemUtil.hpp @@ -0,0 +1,98 @@ +// 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. + +#pragma once + +#include + +#include +#include +#include + +#include "wpi/util/SymbolExports.hpp" + +namespace wpi::math { + +/** + * Returns true if (A, B) is a stabilizable pair. + * + * (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if + * any, have absolute values less than one, where an eigenvalue is + * uncontrollable if rank([λI - A, B]) < n where n is the number of states. + * + * @tparam States Number of states. + * @tparam Inputs Number of inputs. + * @param A System matrix. + * @param B Input matrix. + */ +template +bool IsStabilizable(const Eigen::Matrix& A, + const Eigen::Matrix& B) { + Eigen::EigenSolver> es{A, false}; + + for (int i = 0; i < A.rows(); ++i) { + if (std::norm(es.eigenvalues()[i]) < 1) { + continue; + } + + if constexpr (States != Eigen::Dynamic && Inputs != Eigen::Dynamic) { + Eigen::Matrix, States, States + Inputs> E; + E << es.eigenvalues()[i] * Eigen::Matrix, States, + States>::Identity() - + A, + B; + + Eigen::ColPivHouseholderQR< + Eigen::Matrix, States, States + Inputs>> + qr{E}; + if (qr.rank() < States) { + return false; + } + } else { + Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()}; + E << es.eigenvalues()[i] * + Eigen::MatrixXcd::Identity(A.rows(), A.rows()) - + A, + B; + + Eigen::ColPivHouseholderQR qr{E}; + if (qr.rank() < A.rows()) { + return false; + } + } + } + return true; +} + +extern template WPILIB_DLLEXPORT bool IsStabilizable<1, 1>( + const Eigen::Matrix& A, const Eigen::Matrix& B); +extern template WPILIB_DLLEXPORT bool IsStabilizable<2, 1>( + const Eigen::Matrix& A, const Eigen::Matrix& B); +extern template WPILIB_DLLEXPORT bool +IsStabilizable(const Eigen::MatrixXd& A, + const Eigen::MatrixXd& B); + +/** + * Returns true if (A, C) is a detectable pair. + * + * (A, C) is detectable if and only if the unobservable eigenvalues of A, if + * any, have absolute values less than one, where an eigenvalue is unobservable + * if rank([λI - A; C]) < n where n is the number of states. + * + * @tparam States Number of states. + * @tparam Outputs Number of outputs. + * @param A System matrix. + * @param C Output matrix. + */ +template +bool IsDetectable(const Eigen::Matrix& A, + const Eigen::Matrix& C) { + return IsStabilizable(A.transpose(), C.transpose()); +} + +extern template WPILIB_DLLEXPORT bool +IsDetectable(const Eigen::MatrixXd& A, + const Eigen::MatrixXd& C); + +} // namespace wpi::math diff --git a/wpimath/src/main/native/include/wpi/math/util/StateSpaceUtil.hpp b/wpimath/src/main/native/include/wpi/math/util/StateSpaceUtil.hpp index 6c8a07a989..5b89efe601 100644 --- a/wpimath/src/main/native/include/wpi/math/util/StateSpaceUtil.hpp +++ b/wpimath/src/main/native/include/wpi/math/util/StateSpaceUtil.hpp @@ -4,17 +4,14 @@ #pragma once +#include #include -#include #include #include -#include +#include -#include -#include +#include -#include "wpi/math/geometry/Pose2d.hpp" -#include "wpi/math/linalg/EigenCore.hpp" #include "wpi/util/Algorithm.hpp" #include "wpi/util/SymbolExports.hpp" @@ -28,15 +25,15 @@ namespace wpi::math { * infinity, its cost matrix entry is set to zero. * * @param tolerances An array. For a Q matrix, its elements are the maximum - * allowed excursions of the states from the reference. For an - * R matrix, its elements are the maximum allowed excursions - * of the control inputs from no actuation. + * allowed excursions of the states from the reference. For an R matrix, its + * elements are the maximum allowed excursions of the control inputs from no + * actuation. * @return State excursion or control effort cost matrix. */ template ... Ts> -constexpr Matrixd MakeCostMatrix( +constexpr Eigen::Matrix CostMatrix( Ts... tolerances) { - Matrixd result; + Eigen::Matrix result; for (int row = 0; row < result.rows(); ++row) { for (int col = 0; col < result.cols(); ++col) { @@ -59,37 +56,6 @@ constexpr Matrixd MakeCostMatrix( return result; } -/** - * Creates a covariance matrix from the given vector for use with Kalman - * filters. - * - * Each element is squared and placed on the covariance matrix diagonal. - * - * @param stdDevs An array. For a Q matrix, its elements are the standard - * deviations of each state from how the model behaves. For an R - * matrix, its elements are the standard deviations for each - * output measurement. - * @return Process noise or measurement noise covariance matrix. - */ -template ... Ts> -constexpr Matrixd MakeCovMatrix(Ts... stdDevs) { - Matrixd result; - - for (int row = 0; row < result.rows(); ++row) { - for (int col = 0; col < result.cols(); ++col) { - if (row != col) { - result(row, col) = 0.0; - } - } - } - - wpi::util::for_each( - [&](int i, double stdDev) { result(i, i) = stdDev * stdDev; }, - stdDevs...); - - return result; -} - /** * Creates a cost matrix from the given vector for use with LQR. * @@ -98,14 +64,15 @@ constexpr Matrixd MakeCovMatrix(Ts... stdDevs) { * tolerance is infinity, its cost matrix entry is set to zero. * * @param costs An array. For a Q matrix, its elements are the maximum allowed - * excursions of the states from the reference. For an R matrix, - * its elements are the maximum allowed excursions of the control - * inputs from no actuation. + * excursions of the states from the reference. For an R matrix, its + * elements are the maximum allowed excursions of the control inputs from no + * actuation. * @return State excursion or control effort cost matrix. */ template -constexpr Matrixd MakeCostMatrix(const std::array& costs) { - Matrixd result; +constexpr Eigen::Matrix CostMatrix( + const std::array& costs) { + Eigen::Matrix result; for (int row = 0; row < result.rows(); ++row) { for (int col = 0; col < result.cols(); ++col) { @@ -132,12 +99,12 @@ constexpr Matrixd MakeCostMatrix(const std::array& costs) { * tolerance is infinity, its cost matrix entry is set to zero. * * @param costs A possibly variable length container. For a Q matrix, its - * elements are the maximum allowed excursions of the states from - * the reference. For an R matrix, its elements are the maximum - * allowed excursions of the control inputs from no actuation. + * elements are the maximum allowed excursions of the states from the + * reference. For an R matrix, its elements are the maximum allowed + * excursions of the control inputs from no actuation. * @return State excursion or control effort cost matrix. */ -WPILIB_DLLEXPORT Eigen::MatrixXd MakeCostMatrix( +WPILIB_DLLEXPORT Eigen::MatrixXd CostMatrix( const std::span costs); /** @@ -147,14 +114,45 @@ WPILIB_DLLEXPORT Eigen::MatrixXd MakeCostMatrix( * Each element is squared and placed on the covariance matrix diagonal. * * @param stdDevs An array. For a Q matrix, its elements are the standard - * deviations of each state from how the model behaves. For an R - * matrix, its elements are the standard deviations for each - * output measurement. + * deviations of each state from how the model behaves. For an R matrix, its + * elements are the standard deviations for each output measurement. + * @return Process noise or measurement noise covariance matrix. + */ +template ... Ts> +constexpr Eigen::Matrix CovarianceMatrix( + Ts... stdDevs) { + Eigen::Matrix result; + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { + if (row != col) { + result(row, col) = 0.0; + } + } + } + + wpi::util::for_each( + [&](int i, double stdDev) { result(i, i) = stdDev * stdDev; }, + stdDevs...); + + return result; +} + +/** + * Creates a covariance matrix from the given vector for use with Kalman + * filters. + * + * Each element is squared and placed on the covariance matrix diagonal. + * + * @param stdDevs An array. For a Q matrix, its elements are the standard + * deviations of each state from how the model behaves. For an R matrix, its + * elements are the standard deviations for each output measurement. * @return Process noise or measurement noise covariance matrix. */ template -constexpr Matrixd MakeCovMatrix(const std::array& stdDevs) { - Matrixd result; +constexpr Eigen::Matrix CovarianceMatrix( + const std::array& stdDevs) { + Eigen::Matrix result; for (int row = 0; row < result.rows(); ++row) { for (int col = 0; col < result.cols(); ++col) { @@ -176,247 +174,27 @@ constexpr Matrixd MakeCovMatrix(const std::array& stdDevs) { * Each element is squared and placed on the covariance matrix diagonal. * * @param stdDevs A possibly variable length container. For a Q matrix, its - * elements are the standard deviations of each state from how - * the model behaves. For an R matrix, its elements are the - * standard deviations for each output measurement. + * elements are the standard deviations of each state from how the model + * behaves. For an R matrix, its elements are the standard deviations for + * each output measurement. * @return Process noise or measurement noise covariance matrix. */ -WPILIB_DLLEXPORT Eigen::MatrixXd MakeCovMatrix( +WPILIB_DLLEXPORT Eigen::MatrixXd CovarianceMatrix( const std::span stdDevs); -template ... Ts> -Vectord MakeWhiteNoiseVector(Ts... stdDevs) { - std::random_device rd; - std::mt19937 gen{rd()}; - - Vectord result; - wpi::util::for_each( - [&](int i, double stdDev) { - // Passing a standard deviation of 0.0 to std::normal_distribution is - // undefined behavior - if (stdDev == 0.0) { - result(i) = 0.0; - } else { - std::normal_distribution distr{0.0, stdDev}; - result(i) = distr(gen); - } - }, - stdDevs...); - return result; -} - -/** - * Creates a vector of normally distributed white noise with the given noise - * intensities for each element. - * - * @param stdDevs An array whose elements are the standard deviations of each - * element of the noise vector. - * @return White noise vector. - */ -template -Vectord MakeWhiteNoiseVector(const std::array& stdDevs) { - std::random_device rd; - std::mt19937 gen{rd()}; - - Vectord result; - for (size_t i = 0; i < stdDevs.size(); ++i) { - // Passing a standard deviation of 0.0 to std::normal_distribution is - // undefined behavior - if (stdDevs[i] == 0.0) { - result(i) = 0.0; - } else { - std::normal_distribution distr{0.0, stdDevs[i]}; - result(i) = distr(gen); - } - } - return result; -} - -/** - * Creates a vector of normally distributed white noise with the given noise - * intensities for each element. - * - * @param stdDevs A possibly variable length container whose elements are the - * standard deviations of each element of the noise vector. - * @return White noise vector. - */ -WPILIB_DLLEXPORT Eigen::VectorXd MakeWhiteNoiseVector( - const std::span stdDevs); - -/** - * Converts a Pose2d into a vector of [x, y, theta]. - * - * @param pose The pose that is being represented. - * - * @return The vector. - * @deprecated Create the vector manually instead. If you were using this as an - * intermediate step for constructing affine transformations, use - * Pose2d.ToMatrix() instead. - */ -[[deprecated("Use Pose2d.ToMatrix() instead.")]] -WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) { - return Eigen::Vector3d{{pose.Translation().X().value(), - pose.Translation().Y().value(), - pose.Rotation().Radians().value()}}; -} - -/** - * Converts a Pose2d into a vector of [x, y, cos(theta), sin(theta)]. - * - * @param pose The pose that is being represented. - * - * @return The vector. - * @deprecated Create the vector manually instead. If you were using this as an - * intermediate step for constructing affine transformations, use - * Pose2d.ToMatrix() instead. - */ -[[deprecated("Use Pose2d.ToMatrix() instead.")]] -WPILIB_DLLEXPORT constexpr Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) { - return Eigen::Vector4d{{pose.Translation().X().value(), - pose.Translation().Y().value(), pose.Rotation().Cos(), - pose.Rotation().Sin()}}; -} - -/** - * Returns true if (A, B) is a stabilizable pair. - * - * (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if - * any, have absolute values less than one, where an eigenvalue is - * uncontrollable if rank([λI - A, B]) < n where n is the number of states. - * - * @tparam States Number of states. - * @tparam Inputs Number of inputs. - * @param A System matrix. - * @param B Input matrix. - */ -template -bool IsStabilizable(const Matrixd& A, - const Matrixd& B) { - Eigen::EigenSolver> es{A, false}; - - for (int i = 0; i < A.rows(); ++i) { - if (std::norm(es.eigenvalues()[i]) < 1) { - continue; - } - - if constexpr (States != Eigen::Dynamic && Inputs != Eigen::Dynamic) { - Eigen::Matrix, States, States + Inputs> E; - E << es.eigenvalues()[i] * Eigen::Matrix, States, - States>::Identity() - - A, - B; - - Eigen::ColPivHouseholderQR< - Eigen::Matrix, States, States + Inputs>> - qr{E}; - if (qr.rank() < States) { - return false; - } - } else { - Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()}; - E << es.eigenvalues()[i] * - Eigen::MatrixXcd::Identity(A.rows(), A.rows()) - - A, - B; - - Eigen::ColPivHouseholderQR qr{E}; - if (qr.rank() < A.rows()) { - return false; - } - } - } - return true; -} - -extern template WPILIB_DLLEXPORT bool IsStabilizable<1, 1>( - const Matrixd<1, 1>& A, const Matrixd<1, 1>& B); -extern template WPILIB_DLLEXPORT bool IsStabilizable<2, 1>( - const Matrixd<2, 2>& A, const Matrixd<2, 1>& B); -extern template WPILIB_DLLEXPORT bool -IsStabilizable(const Eigen::MatrixXd& A, - const Eigen::MatrixXd& B); - -/** - * Returns true if (A, C) is a detectable pair. - * - * (A, C) is detectable if and only if the unobservable eigenvalues of A, if - * any, have absolute values less than one, where an eigenvalue is unobservable - * if rank([λI - A; C]) < n where n is the number of states. - * - * @tparam States Number of states. - * @tparam Outputs Number of outputs. - * @param A System matrix. - * @param C Output matrix. - */ -template -bool IsDetectable(const Matrixd& A, - const Matrixd& C) { - return IsStabilizable(A.transpose(), C.transpose()); -} - -extern template WPILIB_DLLEXPORT bool -IsDetectable(const Eigen::MatrixXd& A, - const Eigen::MatrixXd& C); - -/** - * Converts a Pose2d into a vector of [x, y, theta]. - * - * @param pose The pose that is being represented. - * - * @return The vector. - * @deprecated Create the vector manually instead. If you were using this as an - * intermediate step for constructing affine transformations, use - * Pose2d.ToMatrix() instead. - */ -[[deprecated("Use Pose2d.ToMatrix() instead.")]] -WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseToVector(const Pose2d& pose) { - return Eigen::Vector3d{ - {pose.X().value(), pose.Y().value(), pose.Rotation().Radians().value()}}; -} - -/** - * Clamps input vector between system's minimum and maximum allowable input. - * - * @tparam Inputs Number of inputs. - * @param u Input vector to clamp. - * @param umin The minimum input magnitude. - * @param umax The maximum input magnitude. - * @return Clamped input vector. - */ -template -constexpr Vectord ClampInputMaxMagnitude(const Vectord& u, - const Vectord& umin, - const Vectord& umax) { - Vectord result; - for (int i = 0; i < u.rows(); ++i) { - result(i) = std::clamp(u(i), umin(i), umax(i)); - } - return result; -} - -extern template WPILIB_DLLEXPORT Eigen::VectorXd -ClampInputMaxMagnitude(const Eigen::VectorXd& u, - const Eigen::VectorXd& umin, - const Eigen::VectorXd& umax); - /** * Renormalize all inputs if any exceeds the maximum magnitude. Useful for * systems such as differential drivetrains. * - * @tparam Inputs Number of inputs. - * @param u The input vector. + * @tparam Inputs Number of inputs. + * @param u The input vector. * @param maxMagnitude The maximum magnitude any input can have. * @return The normalizedInput */ template -Vectord DesaturateInputVector(const Vectord& u, - double maxMagnitude) { - double maxValue = u.template lpNorm(); - - if (maxValue > maxMagnitude) { - return u * maxMagnitude / maxValue; - } - return u; +Eigen::Vector DesaturateInputVector( + const Eigen::Vector& u, double maxMagnitude) { + return u * std::min(1.0, maxMagnitude / u.template lpNorm()); } extern template WPILIB_DLLEXPORT Eigen::VectorXd diff --git a/wpimath/src/main/python/pyproject.toml b/wpimath/src/main/python/pyproject.toml index 3a36a798a0..ec9c249dbb 100644 --- a/wpimath/src/main/python/pyproject.toml +++ b/wpimath/src/main/python/pyproject.toml @@ -60,6 +60,7 @@ scan_headers_ignore = [ "wpi/math/linalg/ct_matrix.hpp", "wpi/math/linalg/DARE.hpp", "wpi/math/linalg/EigenCore.hpp", + "wpi/math/system/LinearSystemUtil.hpp", "wpi/math/util/StateSpaceUtil.hpp", "wpi/math/fmt/Eigen.hpp", @@ -77,6 +78,8 @@ scan_headers_ignore = [ "wpi/math/geometry/detail/RotationVectorToMatrix.hpp", + "wpi/math/random/Normal.hpp", + "wpi/math/system/Discretization.hpp", "wpi/math/system/NumericalIntegration.hpp", "wpi/math/system/NumericalJacobian.hpp", @@ -1605,6 +1608,9 @@ SwerveDrivePoseEstimator3d = "wpi/math/estimator/SwerveDrivePoseEstimator3d.hpp" # wpi/math/optimization SimulatedAnnealing = "wpi/math/optimization/SimulatedAnnealing.hpp" +# wpi/math/random +# Normal = "wpi/math/random/Normal.hpp" + # wpi/math/path TravelingSalesman = "wpi/math/path/TravelingSalesman.hpp" @@ -1612,6 +1618,7 @@ TravelingSalesman = "wpi/math/path/TravelingSalesman.hpp" # Discretization = "wpi/math/system/Discretization.hpp" LinearSystem = "wpi/math/system/LinearSystem.hpp" LinearSystemLoop = "wpi/math/system/LinearSystemLoop.hpp" +# LinearSystemUtil = "wpi/math/system/LinearSystemUtil.hpp" # NumericalIntegration = "wpi/math/system/NumericalIntegration.hpp" # NumericalJacobian = "wpi/math/system/NumericalJacobian.hpp" diff --git a/wpimath/src/test/java/org/wpilib/math/estimator/ExtendedKalmanFilterTest.java b/wpimath/src/test/java/org/wpilib/math/estimator/ExtendedKalmanFilterTest.java index ecf433653b..1a3e551b46 100644 --- a/wpimath/src/test/java/org/wpilib/math/estimator/ExtendedKalmanFilterTest.java +++ b/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 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()); diff --git a/wpimath/src/test/java/org/wpilib/math/estimator/MerweUKFTest.java b/wpimath/src/test/java/org/wpilib/math/estimator/MerweUKFTest.java index ed83da53e4..709b1ee4d1 100644 --- a/wpimath/src/test/java/org/wpilib/math/estimator/MerweUKFTest.java +++ b/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 = diff --git a/wpimath/src/test/java/org/wpilib/math/estimator/S3UKFTest.java b/wpimath/src/test/java/org/wpilib/math/estimator/S3UKFTest.java index dbbf9271f2..8efca89c87 100644 --- a/wpimath/src/test/java/org/wpilib/math/estimator/S3UKFTest.java +++ b/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 = diff --git a/wpimath/src/test/java/org/wpilib/math/jni/StateSpaceUtilJNITest.java b/wpimath/src/test/java/org/wpilib/math/jni/LinearSystemUtilJNITest.java similarity index 79% rename from wpimath/src/test/java/org/wpilib/math/jni/StateSpaceUtilJNITest.java rename to wpimath/src/test/java/org/wpilib/math/jni/LinearSystemUtilJNITest.java index bc9298eff9..2b13ddd2f4 100644 --- a/wpimath/src/test/java/org/wpilib/math/jni/StateSpaceUtilJNITest.java +++ b/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); } } diff --git a/wpimath/src/test/java/org/wpilib/math/random/NormalTest.java b/wpimath/src/test/java/org/wpilib/math/random/NormalTest.java new file mode 100644 index 0000000000..bda6702e71 --- /dev/null +++ b/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 { + NormalTest() { + super(Normal.class); + } + + @Test + void testNormal() { + var firstData = new ArrayList(); + var secondData = new ArrayList(); + 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 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); + } +} diff --git a/wpimath/src/test/java/org/wpilib/math/system/LinearSystemUtilTest.java b/wpimath/src/test/java/org/wpilib/math/system/LinearSystemUtilTest.java new file mode 100644 index 0000000000..b0a20c3bda --- /dev/null +++ b/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 { + LinearSystemUtilTest() { + super(LinearSystemUtil.class); + } + + @Test + void testIsStabilizable() { + Matrix A; + Matrix 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 A; + Matrix 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)); + } +} diff --git a/wpimath/src/test/java/org/wpilib/math/util/StateSpaceUtilTest.java b/wpimath/src/test/java/org/wpilib/math/util/StateSpaceUtilTest.java index 8f8e448f7b..7993ffa852 100644 --- a/wpimath/src/test/java/org/wpilib/math/util/StateSpaceUtilTest.java +++ b/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 { @@ -27,7 +21,7 @@ class StateSpaceUtilTest extends UtilityClassTest { @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 { @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 { assertEquals(9.0, mat.get(2, 2), 1e-3); } - @Test - void testIsStabilizable() { - Matrix A; - Matrix 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 A; - Matrix 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(); - var secondData = new ArrayList(); - 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 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 wrappedMatrix = Matrix.eye(Nat.N2()); @@ -156,12 +67,20 @@ class StateSpaceUtilTest extends UtilityClassTest { } @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)); } } diff --git a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp index 85b2e20ccb..623a8b2a92 100644 --- a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp @@ -11,10 +11,10 @@ #include #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()); diff --git a/wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp b/wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp index b61c147aa2..99718c1a0a 100644 --- a/wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp +++ b/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}; diff --git a/wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp b/wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp index c03fb1007f..2ae02dcede 100644 --- a/wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp +++ b/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}; diff --git a/wpimath/src/test/native/cpp/random/NormalTest.cpp b/wpimath/src/test/native/cpp/random/NormalTest.cpp new file mode 100644 index 0000000000..d30537de94 --- /dev/null +++ b/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 +#include + +TEST(NormalTest, NormalParameterPack) { + [[maybe_unused]] + Eigen::Vector vec = wpi::math::Normal(2.0, 3.0); +} + +TEST(NormalTest, NormalArray) { + [[maybe_unused]] + Eigen::Vector 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}); +} diff --git a/wpimath/src/test/native/cpp/system/LinearSystemUtilTest.cpp b/wpimath/src/test/native/cpp/system/LinearSystemUtilTest.cpp new file mode 100644 index 0000000000..8dc52d2c8c --- /dev/null +++ b/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 +#include + +TEST(LinearSystemUtilTest, IsStabilizable) { + Eigen::Matrix B{0, 1}; + + // First eigenvalue is uncontrollable and unstable. + // Second eigenvalue is controllable and stable. + EXPECT_FALSE((wpi::math::IsStabilizable<2, 1>( + Eigen::Matrix{{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{{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{{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{{0.2, 0}, {0, 1.2}}, B))); +} + +TEST(LinearSystemUtilTest, IsDetectable) { + Eigen::Matrix C{0, 1}; + + // First eigenvalue is unobservable and unstable. + // Second eigenvalue is observable and stable. + EXPECT_FALSE((wpi::math::IsDetectable<2, 1>( + Eigen::Matrix{{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{{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{{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{{0.2, 0}, {0, 1.2}}, C))); +} diff --git a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp b/wpimath/src/test/native/cpp/util/StateSpaceUtilTest.cpp similarity index 50% rename from wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp rename to wpimath/src/test/native/cpp/util/StateSpaceUtilTest.cpp index bc564d6520..7f60dc0ae2 100644 --- a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp +++ b/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); }