Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Support dynamic matrix sizes in StateSpaceUtil (#7942)

Browse Source
This commit is contained in:
Lucien Morey
2025-04-30 03:58:28 +10:00
committed by GitHub
parent 631521a980
commit f14af97dc7
3 changed files with 143 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
wpimath/src/main/native/cpp/StateSpaceUtil.cpp
View File

@@ -4,6 +4,8 @@
#include "frc/StateSpaceUtil.h"
#include <limits>
namespace frc {
template bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A,
@@ -13,4 +15,57 @@ template bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A,
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 result{costs.size(), costs.size()};
result.setZero();
for (size_t i = 0; i < costs.size(); ++i) {
if (costs[i] == std::numeric_limits<double>::infinity()) {
result(i, i) = 0.0;
} else {
result(i, i) = 1.0 / (std::pow(costs[i], 2));
}
}
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 result{stdDevs.size(), stdDevs.size()};
result.setZero();
for (size_t i = 0; i < stdDevs.size(); ++i) {
result(i, i) = std::pow(stdDevs[i], 2);
}
return result;
}
} // namespace frc

58
wpimath/src/main/native/include/frc/StateSpaceUtil.h
View File

@@ -123,6 +123,22 @@ constexpr Matrixd<N, N> MakeCostMatrix(const std::array<double, N>& costs) {
return result;
}
/**
* Creates a cost matrix from the given vector for use with LQR.
*
* The cost matrix is constructed using Bryson's rule. The inverse square of
* each element in the input is placed on the cost matrix diagonal. If a
* 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.
* @return State excursion or control effort cost matrix.
*/
WPILIB_DLLEXPORT Eigen::MatrixXd MakeCostMatrix(
const std::span<const double> costs);
/**
* Creates a covariance matrix from the given vector for use with Kalman
* filters.
@@ -152,6 +168,21 @@ constexpr Matrixd<N, N> MakeCovMatrix(const std::array<double, N>& 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 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.
* @return Process noise or measurement noise covariance matrix.
*/
WPILIB_DLLEXPORT Eigen::MatrixXd MakeCovMatrix(
const std::span<const double> stdDevs);
template <std::same_as<double>... Ts>
Vectord<sizeof...(Ts)> MakeWhiteNoiseVector(Ts... stdDevs) {
std::random_device rd;
@@ -200,6 +231,17 @@ Vectord<N> MakeWhiteNoiseVector(const std::array<double, N>& stdDevs) {
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].
*
@@ -311,6 +353,10 @@ bool IsDetectable(const Matrixd<States, States>& A,
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].
*
@@ -341,12 +387,17 @@ constexpr Vectord<Inputs> ClampInputMaxMagnitude(const Vectord<Inputs>& u,
const Vectord<Inputs>& umin,
const Vectord<Inputs>& umax) {
Vectord<Inputs> result;
for (int i = 0; i < Inputs; ++i) {
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.
@@ -366,4 +417,9 @@ Vectord<Inputs> DesaturateInputVector(const Vectord<Inputs>& u,
}
return u;
}
extern template WPILIB_DLLEXPORT Eigen::VectorXd
DesaturateInputVector<Eigen::Dynamic>(const Eigen::VectorXd& u,
double maxMagnitude);
} // namespace frc

31
wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
View File

@@ -33,6 +33,19 @@ TEST(StateSpaceUtilTest, CostArray) {
EXPECT_NEAR(mat(2, 2), 1.0 / 9.0, 1e-3);
}
TEST(StateSpaceUtilTest, CostDynamic) {
Eigen::MatrixXd mat = frc::MakeCostMatrix(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);
EXPECT_NEAR(mat(1, 0), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 1), 1.0 / 4.0, 1e-3);
EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(2, 2), 1.0 / 9.0, 1e-3);
}
TEST(StateSpaceUtilTest, CovParameterPack) {
constexpr frc::Matrixd<3, 3> mat = frc::MakeCovMatrix(1.0, 2.0, 3.0);
EXPECT_NEAR(mat(0, 0), 1.0, 1e-3);
@@ -59,6 +72,19 @@ TEST(StateSpaceUtilTest, CovArray) {
EXPECT_NEAR(mat(2, 2), 9.0, 1e-3);
}
TEST(StateSpaceUtilTest, CovDynamic) {
Eigen::MatrixXd mat = frc::MakeCovMatrix(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);
EXPECT_NEAR(mat(1, 0), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 1), 4.0, 1e-3);
EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(0, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(1, 2), 0.0, 1e-3);
EXPECT_NEAR(mat(2, 2), 9.0, 1e-3);
}
TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) {
[[maybe_unused]]
frc::Vectord<2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
@@ -69,6 +95,11 @@ TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) {
frc::Vectord<2> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
}
TEST(StateSpaceUtilTest, WhiteNoiseVectorDynamic) {
[[maybe_unused]]
Eigen::VectorXd vec = frc::MakeWhiteNoiseVector(std::vector{2.0, 3.0});
}
TEST(StateSpaceUtilTest, IsStabilizable) {
frc::Matrixd<2, 1> B{0, 1};