Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Refactor StateSpaceUtil into separate files (#8421)

Browse Source 
* 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()
This commit is contained in:
Tyler Veness
2025-11-29 10:28:38 -08:00
committed by GitHub
parent c8e6ce1ca4
commit a79f86ade3
51 changed files with 755 additions and 741 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
View File

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

4
wpilibc/src/main/native/include/wpi/simulation/LinearSystemSim.hpp
View File

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

4
wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.hpp
View File

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

4
wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.hpp
View File

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

4
wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.hpp
View File

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

3
wpilibj/src/main/java/org/wpilib/simulation/DifferentialDrivetrainSim.java
View File

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

3
wpilibj/src/main/java/org/wpilib/simulation/LinearSystemSim.java
View File

@@ -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<States extends Num, Inputs extends Num, Outputs ext
// Add measurement noise.
if (m_measurementStdDevs != null) {
m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs));
m_y = m_y.plus(Normal.normal(m_measurementStdDevs));
}
}

5
wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/ExampleGlobalMeasurementSensor.java
View File

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

5
wpilibjExamples/src/main/java/org/wpilib/examples/mecanumdriveposeestimator/ExampleGlobalMeasurementSensor.java
View File

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

5
wpilibjExamples/src/main/java/org/wpilib/examples/swervedriveposeestimator/ExampleGlobalMeasurementSensor.java
View File

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

2
wpimath/CMakeLists.txt
View File

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

4
wpimath/src/main/java/org/wpilib/math/controller/LTVDifferentialDriveController.java
View File

@@ -74,8 +74,8 @@ public class LTVDifferentialDriveController {
m_trackwidth = trackwidth;
m_A = plant.getA();
m_B = plant.getB();
m_Q = StateSpaceUtil.makeCostMatrix(qelems);
m_R = StateSpaceUtil.makeCostMatrix(relems);
m_Q = StateSpaceUtil.costMatrix(qelems);
m_R = StateSpaceUtil.costMatrix(relems);
m_dt = dt;
}

4
wpimath/src/main/java/org/wpilib/math/controller/LTVUnicycleController.java
View File

@@ -64,8 +64,8 @@ public class LTVUnicycleController {
* @param dt Discretization timestep in seconds.
*/
public LTVUnicycleController(Vector<N3> qelems, Vector<N2> relems, double dt) {
m_Q = StateSpaceUtil.makeCostMatrix(qelems);
m_R = StateSpaceUtil.makeCostMatrix(relems);
m_Q = StateSpaceUtil.costMatrix(qelems);
m_R = StateSpaceUtil.costMatrix(relems);
m_dt = dt;
}

6
wpimath/src/main/java/org/wpilib/math/controller/LinearQuadraticRegulator.java
View File

@@ -56,8 +56,8 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
this(
plant.getA(),
plant.getB(),
StateSpaceUtil.makeCostMatrix(qelms),
StateSpaceUtil.makeCostMatrix(relms),
StateSpaceUtil.costMatrix(qelms),
StateSpaceUtil.costMatrix(relms),
dt);
}
@@ -81,7 +81,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
Vector<States> qelms,
Vector<Inputs> relms,
double dt) {
this(A, B, StateSpaceUtil.makeCostMatrix(qelms), StateSpaceUtil.makeCostMatrix(relms), dt);
this(A, B, StateSpaceUtil.costMatrix(qelms), StateSpaceUtil.costMatrix(relms), dt);
}
/**

7
wpimath/src/main/java/org/wpilib/math/estimator/ExtendedKalmanFilter.java
View File

@@ -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 extends Num, Inputs extends Num, Output
m_residualFuncY = residualFuncY;
m_addFuncX = addFuncX;
m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
m_contQ = StateSpaceUtil.covarianceMatrix(states, stateStdDevs);
m_contR = StateSpaceUtil.covarianceMatrix(outputs, measurementStdDevs);
m_dt = dt;
reset();
@@ -156,7 +157,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
final var discR = Discretization.discretizeR(m_contR, dt);
if (StateSpaceUtil.isDetectable(discA, C) && outputs.getNum() <= states.getNum()) {
if (LinearSystemUtil.isDetectable(discA, C) && outputs.getNum() <= states.getNum()) {
m_initP = DARE.dare(discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = new Matrix<>(states, states);

4
wpimath/src/main/java/org/wpilib/math/estimator/KalmanFilter.java
View File

@@ -71,8 +71,8 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
this.m_plant = plant;
m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
m_contQ = StateSpaceUtil.covarianceMatrix(states, stateStdDevs);
m_contR = StateSpaceUtil.covarianceMatrix(outputs, measurementStdDevs);
m_dt = dt;
// Find discrete A and Q

4
wpimath/src/main/java/org/wpilib/math/estimator/SteadyStateKalmanFilter.java
View File

@@ -72,8 +72,8 @@ public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Out
this.m_plant = plant;
var contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
var contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
var contQ = StateSpaceUtil.covarianceMatrix(states, stateStdDevs);
var contR = StateSpaceUtil.covarianceMatrix(outputs, measurementStdDevs);
var pair = Discretization.discretizeAQ(plant.getA(), contQ, dt);
var discA = pair.getFirst();

4
wpimath/src/main/java/org/wpilib/math/estimator/UnscentedKalmanFilter.java
View File

@@ -167,8 +167,8 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
m_dt = nominalDt;
m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
m_contQ = StateSpaceUtil.covarianceMatrix(states, stateStdDevs);
m_contR = StateSpaceUtil.covarianceMatrix(outputs, measurementStdDevs);
m_pts = pts;

6
wpimath/src/main/java/org/wpilib/math/jni/StateSpaceUtilJNI.java → wpimath/src/main/java/org/wpilib/math/jni/LinearSystemUtilJNI.java
View File

@@ -4,8 +4,8 @@
package org.wpilib.math.jni;
/** StateSpaceUtil JNI. */
public final class StateSpaceUtilJNI extends WPIMathJNI {
/** LinearSystemUtil JNI. */
public final class LinearSystemUtilJNI extends WPIMathJNI {
/**
* Returns true if (A, B) is a stabilizable pair.
*
@@ -22,5 +22,5 @@ public final class StateSpaceUtilJNI extends WPIMathJNI {
public static native boolean isStabilizable(int states, int inputs, double[] A, double[] B);
/** Utility class. */
private StateSpaceUtilJNI() {}
private LinearSystemUtilJNI() {}
}

37
wpimath/src/main/java/org/wpilib/math/random/Normal.java Normal file
View File

@@ -0,0 +1,37 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.random;
import java.util.Random;
import org.ejml.simple.SimpleMatrix;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.numbers.N1;
import org.wpilib.math.util.Num;
/** Utilities for generating normally distributed random values. */
public final class Normal {
private static Random rand = new Random();
private Normal() {
throw new UnsupportedOperationException("This is a utility class!");
}
/**
* Creates a vector of normally distributed random values with the given standard deviations for
* each element.
*
* @param <N> Num representing the dimensionality of the noise vector to create.
* @param stdDevs A matrix whose elements are the standard deviations of each element of the
* random vector.
* @return Vector of normally distributed values.
*/
public static <N extends Num> Matrix<N, N1> normal(Matrix<N, N1> stdDevs) {
Matrix<N, N1> result = new Matrix<>(new SimpleMatrix(stdDevs.getNumRows(), 1));
for (int i = 0; i < stdDevs.getNumRows(); i++) {
result.set(i, 0, rand.nextGaussian() * stdDevs.get(i, 0));
}
return result;
}
}

54
wpimath/src/main/java/org/wpilib/math/system/LinearSystemUtil.java Normal file
View File

@@ -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.
*
* <p>(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 <States> Num representing the size of A.
* @param <Inputs> Num representing the columns of B.
* @param A System matrix.
* @param B Input matrix.
* @return If the system is stabilizable.
*/
public static <States extends Num, Inputs extends Num> boolean isStabilizable(
Matrix<States, States> A, Matrix<States, Inputs> B) {
return LinearSystemUtilJNI.isStabilizable(
A.getNumRows(), B.getNumCols(), A.getData(), B.getData());
}
/**
* Returns true if (A, C) is a detectable pair.
*
* <p>(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 <States> Num representing the size of A.
* @param <Outputs> Num representing the rows of C.
* @param A System matrix.
* @param C Output matrix.
* @return If the system is detectable.
*/
public static <States extends Num, Outputs extends Num> boolean isDetectable(
Matrix<States, States> A, Matrix<Outputs, States> C) {
return LinearSystemUtilJNI.isStabilizable(
A.getNumRows(), C.getNumRows(), A.transpose().getData(), C.transpose().getData());
}
}

164
wpimath/src/main/java/org/wpilib/math/util/StateSpaceUtil.java
View File

@@ -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.
*
* <p>Each element is squared and placed on the covariance matrix diagonal.
*
* @param <States> 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 <States extends Num> Matrix<States, States> makeCovarianceMatrix(
Nat<States> states, Matrix<States, N1> 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 <N> Num representing the dimensionality of the noise vector to create.
* @param stdDevs A matrix whose elements are the standard deviations of each element of the noise
* vector.
* @return White noise vector.
*/
public static <N extends Num> Matrix<N, N1> makeWhiteNoiseVector(Matrix<N, N1> stdDevs) {
Matrix<N, N1> result = new Matrix<>(new SimpleMatrix(stdDevs.getNumRows(), 1));
for (int i = 0; i < stdDevs.getNumRows(); i++) {
result.set(i, 0, rand.nextGaussian() * stdDevs.get(i, 0));
}
return result;
}
/**
* 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 <Elements extends Num> Matrix<Elements, Elements> makeCostMatrix(
public static <Elements extends Num> Matrix<Elements, Elements> costMatrix(
Matrix<Elements, N1> tolerances) {
Matrix<Elements, Elements> 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.
*
* <p>(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.
* <p>Each element is squared and placed on the covariance matrix diagonal.
*
* @param <States> Num representing the size of A.
* @param <Inputs> Num representing the columns of B.
* @param A System matrix.
* @param B Input matrix.
* @return If the system is stabilizable.
* @param <States> 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 <States extends Num, Inputs extends Num> boolean isStabilizable(
Matrix<States, States> A, Matrix<States, Inputs> B) {
return StateSpaceUtilJNI.isStabilizable(
A.getNumRows(), B.getNumCols(), A.getData(), B.getData());
}
/**
* Returns true if (A, C) is a detectable pair.
*
* <p>(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 <States> Num representing the size of A.
* @param <Outputs> Num representing the rows of C.
* @param A System matrix.
* @param C Output matrix.
* @return If the system is detectable.
*/
public static <States extends Num, Outputs extends Num> boolean isDetectable(
Matrix<States, States> A, Matrix<Outputs, States> 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<N3, N1> 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 <I> Number of inputs.
* @return The clamped input.
*/
public static <I extends Num> Matrix<I, N1> clampInputMaxMagnitude(
Matrix<I, N1> u, Matrix<I, N1> umin, Matrix<I, N1> umax) {
var result = new Matrix<I, N1>(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 <States extends Num> Matrix<States, States> covarianceMatrix(
Nat<States> states, Matrix<States, N1> 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 <I extends Num> Matrix<I, N1> desaturateInputVector(
Matrix<I, N1> 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<N4, N1> 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<N3, N1> 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()));
}
}

8
wpimath/src/main/native/cpp/jni/StateSpaceUtilJNI.cpp → wpimath/src/main/native/cpp/jni/LinearSystemUtilJNI.cpp
View File

@@ -6,8 +6,8 @@
#include <Eigen/Core>
#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)
{

32
wpimath/src/main/native/cpp/random/Normal.cpp Normal file
View File

@@ -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 <random>
#include <span>
#include <Eigen/Core>
namespace wpi::math {
Eigen::VectorXd Normal(const std::span<const double> 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

21
wpimath/src/main/native/cpp/util/LinearSystemUtil.cpp Normal file
View File

@@ -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 <Eigen/Core>
namespace wpi::math {
template bool IsStabilizable<1, 1>(const Eigen::Matrix<double, 1, 1>& A,
const Eigen::Matrix<double, 1, 1>& B);
template bool IsStabilizable<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B);
template bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(
const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);
template bool IsDetectable<Eigen::Dynamic, Eigen::Dynamic>(
const Eigen::MatrixXd& A, const Eigen::MatrixXd& C);
} // namespace wpi::math

43
wpimath/src/main/native/cpp/util/StateSpaceUtil.cpp
View File

@@ -5,27 +5,11 @@
#include "wpi/math/util/StateSpaceUtil.hpp"
#include <limits>
#include <span>
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<Eigen::Dynamic, Eigen::Dynamic>(
const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);
template bool IsDetectable<Eigen::Dynamic, Eigen::Dynamic>(
const Eigen::MatrixXd& A, const Eigen::MatrixXd& C);
template Eigen::VectorXd ClampInputMaxMagnitude<Eigen::Dynamic>(
const Eigen::VectorXd& u, const Eigen::VectorXd& umin,
const Eigen::VectorXd& umax);
template Eigen::VectorXd DesaturateInputVector<Eigen::Dynamic>(
const Eigen::VectorXd& u, double maxMagnitude);
Eigen::MatrixXd MakeCostMatrix(const std::span<const double> costs) {
Eigen::MatrixXd CostMatrix(const std::span<const double> costs) {
Eigen::MatrixXd result{costs.size(), costs.size()};
result.setZero();
@@ -39,25 +23,7 @@ Eigen::MatrixXd MakeCostMatrix(const std::span<const double> costs) {
return result;
}
Eigen::VectorXd MakeWhiteNoiseVector(const std::span<const double> 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<const double> stdDevs) {
Eigen::MatrixXd CovarianceMatrix(const std::span<const double> stdDevs) {
Eigen::MatrixXd result{stdDevs.size(), stdDevs.size()};
result.setZero();
@@ -68,4 +34,7 @@ Eigen::MatrixXd MakeCovMatrix(const std::span<const double> stdDevs) {
return result;
}
template Eigen::VectorXd DesaturateInputVector<Eigen::Dynamic>(
const Eigen::VectorXd& u, double maxMagnitude);
} // namespace wpi::math

4
wpimath/src/main/native/include/wpi/math/controller/LTVDifferentialDriveController.hpp
View File

@@ -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} {}
/**

4
wpimath/src/main/native/include/wpi/math/controller/LTVUnicycleController.hpp
View File

@@ -56,8 +56,8 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
LTVUnicycleController(const wpi::util::array<double, 3>& Qelems,
const wpi::util::array<double, 2>& 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} {}
/**

4
wpimath/src/main/native/include/wpi/math/controller/LinearQuadraticRegulator.hpp
View File

@@ -81,8 +81,8 @@ class LinearQuadraticRegulator {
const Matrixd<States, Inputs>& 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.

9
wpimath/src/main/native/include/wpi/math/estimator/ExtendedKalmanFilter.hpp
View File

@@ -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<States, States, Inputs>(

4
wpimath/src/main/native/include/wpi/math/estimator/KalmanFilter.hpp
View File

@@ -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

4
wpimath/src/main/native/include/wpi/math/estimator/SteadyStateKalmanFilter.hpp
View File

@@ -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<States, States> discA;
Matrixd<States, States> discQ;

8
wpimath/src/main/native/include/wpi/math/estimator/UnscentedKalmanFilter.hpp
View File

@@ -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<States, NumSigmas>& sigmas,
const Vectord<NumSigmas>& 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();

2
wpimath/src/main/native/include/wpi/math/linalg/DARE.hpp
View File

@@ -10,7 +10,7 @@
#include <Eigen/Core>
#include <Eigen/LU>
#include "wpi/math/util/StateSpaceUtil.hpp"
#include "wpi/math/system/LinearSystemUtil.hpp"
#include "wpi/util/expected"
namespace wpi::math {

85
wpimath/src/main/native/include/wpi/math/random/Normal.hpp Normal file
View File

@@ -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 <array>
#include <concepts>
#include <random>
#include <span>
#include <Eigen/Core>
#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 <std::same_as<double>... Ts>
Eigen::Vector<double, sizeof...(Ts)> Normal(Ts... stdDevs) {
std::random_device rd;
std::mt19937 gen{rd()};
Eigen::Vector<double, sizeof...(Ts)> 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 <int N>
Eigen::Vector<double, N> Normal(const std::array<double, N>& stdDevs) {
std::random_device rd;
std::mt19937 gen{rd()};
Eigen::Vector<double, N> 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<const double> stdDevs);
} // namespace wpi::math

98
wpimath/src/main/native/include/wpi/math/system/LinearSystemUtil.hpp Normal file
View File

@@ -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 <complex>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/QR>
#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 <int States, int Inputs>
bool IsStabilizable(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B) {
Eigen::EigenSolver<Eigen::Matrix<double, States, States>> 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<std::complex<double>, States, States + Inputs> E;
E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
States>::Identity() -
A,
B;
Eigen::ColPivHouseholderQR<
Eigen::Matrix<std::complex<double>, 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<Eigen::MatrixXcd> qr{E};
if (qr.rank() < A.rows()) {
return false;
}
}
}
return true;
}
extern template WPILIB_DLLEXPORT bool IsStabilizable<1, 1>(
const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B);
extern template WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B);
extern template WPILIB_DLLEXPORT bool
IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(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 <int States, int Outputs>
bool IsDetectable(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, Outputs, States>& C) {
return IsStabilizable<States, Outputs>(A.transpose(), C.transpose());
}
extern template WPILIB_DLLEXPORT bool
IsDetectable<Eigen::Dynamic, Eigen::Dynamic>(const Eigen::MatrixXd& A,
const Eigen::MatrixXd& C);
} // namespace wpi::math

348
wpimath/src/main/native/include/wpi/math/util/StateSpaceUtil.hpp
View File

@@ -4,17 +4,14 @@
#pragma once
#include <algorithm>
#include <array>
#include <cmath>
#include <concepts>
#include <limits>
#include <random>
#include <span>
#include <Eigen/Eigenvalues>
#include <Eigen/QR>
#include <Eigen/Core>
#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 <std::same_as<double>... Ts>
constexpr Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCostMatrix(
constexpr Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> CostMatrix(
Ts... tolerances) {
Matrixd<sizeof...(Ts), sizeof...(Ts)> result;
Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> result;
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
@@ -59,37 +56,6 @@ constexpr Matrixd<sizeof...(Ts), sizeof...(Ts)> 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 <std::same_as<double>... Ts>
constexpr Matrixd<sizeof...(Ts), sizeof...(Ts)> MakeCovMatrix(Ts... stdDevs) {
Matrixd<sizeof...(Ts), sizeof...(Ts)> 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<sizeof...(Ts), sizeof...(Ts)> 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 <size_t N>
constexpr Matrixd<N, N> MakeCostMatrix(const std::array<double, N>& costs) {
Matrixd<N, N> result;
constexpr Eigen::Matrix<double, N, N> CostMatrix(
const std::array<double, N>& costs) {
Eigen::Matrix<double, N, N> result;
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
@@ -132,12 +99,12 @@ constexpr Matrixd<N, N> MakeCostMatrix(const std::array<double, N>& 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<const double> 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 <std::same_as<double>... Ts>
constexpr Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> CovarianceMatrix(
Ts... stdDevs) {
Eigen::Matrix<double, sizeof...(Ts), sizeof...(Ts)> 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 <size_t N>
constexpr Matrixd<N, N> MakeCovMatrix(const std::array<double, N>& stdDevs) {
Matrixd<N, N> result;
constexpr Eigen::Matrix<double, N, N> CovarianceMatrix(
const std::array<double, N>& stdDevs) {
Eigen::Matrix<double, N, N> result;
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
@@ -176,247 +174,27 @@ constexpr Matrixd<N, N> MakeCovMatrix(const std::array<double, N>& 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<const double> stdDevs);
template <std::same_as<double>... Ts>
Vectord<sizeof...(Ts)> MakeWhiteNoiseVector(Ts... stdDevs) {
std::random_device rd;
std::mt19937 gen{rd()};
Vectord<sizeof...(Ts)> 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 <int N>
Vectord<N> MakeWhiteNoiseVector(const std::array<double, N>& stdDevs) {
std::random_device rd;
std::mt19937 gen{rd()};
Vectord<N> 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<const double> 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 <int States, int Inputs>
bool IsStabilizable(const Matrixd<States, States>& A,
const Matrixd<States, Inputs>& B) {
Eigen::EigenSolver<Matrixd<States, States>> 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<std::complex<double>, States, States + Inputs> E;
E << es.eigenvalues()[i] * Eigen::Matrix<std::complex<double>, States,
States>::Identity() -
A,
B;
Eigen::ColPivHouseholderQR<
Eigen::Matrix<std::complex<double>, 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<Eigen::MatrixXcd> 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<Eigen::Dynamic, Eigen::Dynamic>(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 <int States, int Outputs>
bool IsDetectable(const Matrixd<States, States>& A,
const Matrixd<Outputs, States>& C) {
return IsStabilizable<States, Outputs>(A.transpose(), C.transpose());
}
extern template WPILIB_DLLEXPORT bool
IsDetectable<Eigen::Dynamic, Eigen::Dynamic>(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 <int Inputs>
constexpr Vectord<Inputs> ClampInputMaxMagnitude(const Vectord<Inputs>& u,
const Vectord<Inputs>& umin,
const Vectord<Inputs>& umax) {
Vectord<Inputs> 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<Eigen::Dynamic>(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 <int Inputs>
Vectord<Inputs> DesaturateInputVector(const Vectord<Inputs>& u,
double maxMagnitude) {
double maxValue = u.template lpNorm<Eigen::Infinity>();
if (maxValue > maxMagnitude) {
return u * maxMagnitude / maxValue;
}
return u;
Eigen::Vector<double, Inputs> DesaturateInputVector(
const Eigen::Vector<double, Inputs>& u, double maxMagnitude) {
return u * std::min(1.0, maxMagnitude / u.template lpNorm<Eigen::Infinity>());
}
extern template WPILIB_DLLEXPORT Eigen::VectorXd

7
wpimath/src/main/python/pyproject.toml
View File

@@ -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"

8
wpimath/src/test/java/org/wpilib/math/estimator/ExtendedKalmanFilterTest.java
View File

@@ -17,6 +17,7 @@ import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N2;
import org.wpilib.math.numbers.N3;
import org.wpilib.math.numbers.N5;
import org.wpilib.math.random.Normal;
import org.wpilib.math.system.NumericalIntegration;
import org.wpilib.math.system.NumericalJacobian;
import org.wpilib.math.system.plant.DCMotor;
@@ -89,7 +90,7 @@ class ExtendedKalmanFilterTest {
observer.correct(u, localY);
var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
var R = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
var R = StateSpaceUtil.costMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
observer.correct(
Nat.N5(), u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R);
});
@@ -154,8 +155,7 @@ class ExtendedKalmanFilterTest {
nextR.set(4, 0, vr);
var localY = getLocalMeasurementModel(groundTruthX, u);
var whiteNoiseStdDevs = VecBuilder.fill(0.0001, 0.5, 0.5);
observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(whiteNoiseStdDevs)));
observer.correct(u, localY.plus(Normal.normal(VecBuilder.fill(0.0001, 0.5, 0.5))));
Matrix<N5, N1> rdot = nextR.minus(r).div(dt);
u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
@@ -172,7 +172,7 @@ class ExtendedKalmanFilterTest {
observer.correct(u, localY);
var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
var R = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
var R = StateSpaceUtil.costMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
observer.correct(Nat.N5(), u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R);
var finalPosition = trajectory.sample(trajectory.getTotalTime());

12
wpimath/src/test/java/org/wpilib/math/estimator/MerweUKFTest.java
View File

@@ -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 =

12
wpimath/src/test/java/org/wpilib/math/estimator/S3UKFTest.java
View File

@@ -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 =

4
wpimath/src/test/java/org/wpilib/math/jni/StateSpaceUtilJNITest.java → wpimath/src/test/java/org/wpilib/math/jni/LinearSystemUtilJNITest.java
View File

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

50
wpimath/src/test/java/org/wpilib/math/random/NormalTest.java Normal file
View File

@@ -0,0 +1,50 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.random;
import static org.junit.jupiter.api.Assertions.assertEquals;
import java.util.ArrayList;
import java.util.List;
import org.junit.jupiter.api.Test;
import org.wpilib.UtilityClassTest;
import org.wpilib.math.linalg.VecBuilder;
class NormalTest extends UtilityClassTest<Normal> {
NormalTest() {
super(Normal.class);
}
@Test
void testNormal() {
var firstData = new ArrayList<Double>();
var secondData = new ArrayList<Double>();
for (int i = 0; i < 1000; i++) {
var noiseVec = Normal.normal(VecBuilder.fill(1.0, 2.0));
firstData.add(noiseVec.get(0, 0));
secondData.add(noiseVec.get(1, 0));
}
assertEquals(1.0, calculateStandardDeviation(firstData), 0.2);
assertEquals(2.0, calculateStandardDeviation(secondData), 0.2);
}
private double calculateStandardDeviation(List<Double> numArray) {
double sum = 0.0;
double standardDeviation = 0.0;
int length = numArray.size();
for (double num : numArray) {
sum += num;
}
double mean = sum / length;
for (double num : numArray) {
standardDeviation += Math.pow(num - mean, 2);
}
return Math.sqrt(standardDeviation / length);
}
}

75
wpimath/src/test/java/org/wpilib/math/system/LinearSystemUtilTest.java Normal file
View File

@@ -0,0 +1,75 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.system;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
import org.junit.jupiter.api.Test;
import org.wpilib.UtilityClassTest;
import org.wpilib.math.linalg.MatBuilder;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N2;
import org.wpilib.math.util.Nat;
class LinearSystemUtilTest extends UtilityClassTest<LinearSystemUtil> {
LinearSystemUtilTest() {
super(LinearSystemUtil.class);
}
@Test
void testIsStabilizable() {
Matrix<N2, N2> A;
Matrix<N2, N1> B = VecBuilder.fill(0, 1);
// First eigenvalue is uncontrollable and unstable.
// Second eigenvalue is controllable and stable.
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.2, 0, 0, 0.5);
assertFalse(LinearSystemUtil.isStabilizable(A, B));
// First eigenvalue is uncontrollable and marginally stable.
// Second eigenvalue is controllable and stable.
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.5);
assertFalse(LinearSystemUtil.isStabilizable(A, B));
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and stable.
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 0.5);
assertTrue(LinearSystemUtil.isStabilizable(A, B));
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and unstable.
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 1.2);
assertTrue(LinearSystemUtil.isStabilizable(A, B));
}
@Test
void testIsDetectable() {
Matrix<N2, N2> A;
Matrix<N1, N2> C = MatBuilder.fill(Nat.N1(), Nat.N2(), 0, 1);
// First eigenvalue is unobservable and unstable.
// Second eigenvalue is observable and stable.
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.2, 0, 0, 0.5);
assertFalse(LinearSystemUtil.isDetectable(A, C));
// First eigenvalue is unobservable and marginally stable.
// Second eigenvalue is observable and stable.
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.5);
assertFalse(LinearSystemUtil.isDetectable(A, C));
// First eigenvalue is unobservable and stable.
// Second eigenvalue is observable and stable.
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 0.5);
assertTrue(LinearSystemUtil.isDetectable(A, C));
// First eigenvalue is unobservable and stable.
// Second eigenvalue is observable and unstable.
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 1.2);
assertTrue(LinearSystemUtil.isDetectable(A, C));
}
}

115
wpimath/src/test/java/org/wpilib/math/util/StateSpaceUtilTest.java
View File

@@ -5,19 +5,13 @@
package org.wpilib.math.util;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
import java.util.ArrayList;
import java.util.List;
import org.junit.jupiter.api.Test;
import org.wpilib.UtilityClassTest;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.linalg.MatBuilder;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N2;
class StateSpaceUtilTest extends UtilityClassTest<StateSpaceUtil> {
@@ -27,7 +21,7 @@ class StateSpaceUtilTest extends UtilityClassTest<StateSpaceUtil> {
@Test
void testCostArray() {
var mat = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(1.0, 2.0, 3.0));
var mat = StateSpaceUtil.costMatrix(VecBuilder.fill(1.0, 2.0, 3.0));
assertEquals(1.0, mat.get(0, 0), 1e-3);
assertEquals(0.0, mat.get(0, 1), 1e-3);
@@ -42,7 +36,7 @@ class StateSpaceUtilTest extends UtilityClassTest<StateSpaceUtil> {
@Test
void testCovArray() {
var mat = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), VecBuilder.fill(1.0, 2.0, 3.0));
var mat = StateSpaceUtil.covarianceMatrix(Nat.N3(), VecBuilder.fill(1.0, 2.0, 3.0));
assertEquals(1.0, mat.get(0, 0), 1e-3);
assertEquals(0.0, mat.get(0, 1), 1e-3);
@@ -55,89 +49,6 @@ class StateSpaceUtilTest extends UtilityClassTest<StateSpaceUtil> {
assertEquals(9.0, mat.get(2, 2), 1e-3);
}
@Test
void testIsStabilizable() {
Matrix<N2, N2> A;
Matrix<N2, N1> B = VecBuilder.fill(0, 1);
// First eigenvalue is uncontrollable and unstable.
// Second eigenvalue is controllable and stable.
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.2, 0, 0, 0.5);
assertFalse(StateSpaceUtil.isStabilizable(A, B));
// First eigenvalue is uncontrollable and marginally stable.
// Second eigenvalue is controllable and stable.
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.5);
assertFalse(StateSpaceUtil.isStabilizable(A, B));
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and stable.
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 0.5);
assertTrue(StateSpaceUtil.isStabilizable(A, B));
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and unstable.
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 1.2);
assertTrue(StateSpaceUtil.isStabilizable(A, B));
}
@Test
void testIsDetectable() {
Matrix<N2, N2> A;
Matrix<N1, N2> C = MatBuilder.fill(Nat.N1(), Nat.N2(), 0, 1);
// First eigenvalue is unobservable and unstable.
// Second eigenvalue is observable and stable.
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.2, 0, 0, 0.5);
assertFalse(StateSpaceUtil.isDetectable(A, C));
// First eigenvalue is unobservable and marginally stable.
// Second eigenvalue is observable and stable.
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.5);
assertFalse(StateSpaceUtil.isDetectable(A, C));
// First eigenvalue is unobservable and stable.
// Second eigenvalue is observable and stable.
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 0.5);
assertTrue(StateSpaceUtil.isDetectable(A, C));
// First eigenvalue is unobservable and stable.
// Second eigenvalue is observable and unstable.
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 1.2);
assertTrue(StateSpaceUtil.isDetectable(A, C));
}
@Test
void testMakeWhiteNoiseVector() {
var firstData = new ArrayList<Double>();
var secondData = new ArrayList<Double>();
for (int i = 0; i < 1000; i++) {
var noiseVec = StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(1.0, 2.0));
firstData.add(noiseVec.get(0, 0));
secondData.add(noiseVec.get(1, 0));
}
assertEquals(1.0, calculateStandardDeviation(firstData), 0.2);
assertEquals(2.0, calculateStandardDeviation(secondData), 0.2);
}
private double calculateStandardDeviation(List<Double> numArray) {
double sum = 0.0;
double standardDeviation = 0.0;
int length = numArray.size();
for (double num : numArray) {
sum += num;
}
double mean = sum / length;
for (double num : numArray) {
standardDeviation += Math.pow(num - mean, 2);
}
return Math.sqrt(standardDeviation / length);
}
@Test
void testMatrixExp() {
Matrix<N2, N2> wrappedMatrix = Matrix.eye(Nat.N2());
@@ -156,12 +67,20 @@ class StateSpaceUtilTest extends UtilityClassTest<StateSpaceUtil> {
}
@Test
@SuppressWarnings("removal")
void testPoseToVector() {
Pose2d pose = new Pose2d(1, 2, new Rotation2d(3));
var vector = StateSpaceUtil.poseToVector(pose);
assertEquals(pose.getTranslation().getX(), vector.get(0, 0), 1e-6);
assertEquals(pose.getTranslation().getY(), vector.get(1, 0), 1e-6);
assertEquals(pose.getRotation().getRadians(), vector.get(2, 0), 1e-6);
void testDesaturateInputVector() {
final var vec1 = MatBuilder.fill(Nat.N2(), Nat.N1(), 10.0, 12.0);
assertEquals(vec1, StateSpaceUtil.desaturateInputVector(vec1, 12.0));
assertEquals(
MatBuilder.fill(Nat.N2(), Nat.N1(), 25.0 / 3.0, 10.0),
StateSpaceUtil.desaturateInputVector(vec1, 10.0));
final var vec2 = MatBuilder.fill(Nat.N2(), Nat.N1(), 10.0, -12.0);
assertEquals(vec2, StateSpaceUtil.desaturateInputVector(vec2, 12.0));
assertEquals(
MatBuilder.fill(Nat.N2(), Nat.N1(), 25.0 / 3.0, -10.0),
StateSpaceUtil.desaturateInputVector(vec2, 10.0));
final var vec3 = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 0.0);
assertEquals(vec3, StateSpaceUtil.desaturateInputVector(vec3, 12.0));
}
}

9
wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
View File

@@ -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());

14
wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp
View File

@@ -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};

14
wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp
View File

@@ -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};

23
wpimath/src/test/native/cpp/random/NormalTest.cpp Normal file
View File

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

56
wpimath/src/test/native/cpp/system/LinearSystemUtilTest.cpp Normal file
View File

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

87
wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp → wpimath/src/test/native/cpp/util/StateSpaceUtilTest.cpp
View File

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