Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-06 03:31:43 +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

105
wpimath/src/test/native/cpp/util/StateSpaceUtilTest.cpp Normal file
View File

@@ -0,0 +1,105 @@
// 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/util/StateSpaceUtil.hpp"
#include <gtest/gtest.h>
#include "wpi/math/linalg/EigenCore.hpp"
TEST(StateSpaceUtilTest, CostParameterPack) {
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);
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, CostArray) {
constexpr wpi::math::Matrixd<3, 3> mat =
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);
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, CostDynamic) {
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);
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 wpi::math::Matrixd<3, 3> mat =
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);
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, CovArray) {
constexpr wpi::math::Matrixd<3, 3> mat =
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);
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, CovDynamic) {
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);
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, 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);
}