Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +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

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