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] Add dynamic size support for angle statistics (#7964)

Browse Source
This commit is contained in:
Lucien Morey
2025-05-16 11:43:46 +10:00
committed by GitHub
parent 79a9d7f987
commit d32e60233f
1 changed files with 31 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
View File

@@ -22,6 +22,21 @@ TEST(AngleStatisticsTest, Mean) {
.isApprox(frc::AngleMean<3, 1>(sigmas, weights, 1), 1e-3));
}
TEST(AngleStatisticsTest, Mean_DynamicSize) {
Eigen::MatrixXd sigmas{
{1, 1.2, 0},
{359 * std::numbers::pi / 180, 3 * std::numbers::pi / 180, 0},
{1, 2, 0}};
// Weights need to produce the mean of the sigmas
Eigen::VectorXd weights{3};
weights.fill(1.0 / sigmas.cols());
EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1)
.isApprox(frc::AngleMean<Eigen::Dynamic, Eigen::Dynamic>(
sigmas, weights, 1),
1e-3));
}
TEST(AngleStatisticsTest, Residual) {
Eigen::Vector3d a{1, 1 * std::numbers::pi / 180, 2};
Eigen::Vector3d b{1, 359 * std::numbers::pi / 180, 1};
@@ -30,9 +45,25 @@ TEST(AngleStatisticsTest, Residual) {
Eigen::Vector3d{0, 2 * std::numbers::pi / 180, 1}));
}
TEST(AngleStatisticsTest, Residual_DynamicSize) {
Eigen::VectorXd a{{1, 1 * std::numbers::pi / 180, 2}};
Eigen::VectorXd b{{1, 359 * std::numbers::pi / 180, 1}};
EXPECT_TRUE(frc::AngleResidual<Eigen::Dynamic>(a, b, 1).isApprox(
Eigen::VectorXd{{0, 2 * std::numbers::pi / 180, 1}}));
}
TEST(AngleStatisticsTest, Add) {
Eigen::Vector3d a{1, 1 * std::numbers::pi / 180, 2};
Eigen::Vector3d b{1, 359 * std::numbers::pi / 180, 1};
EXPECT_TRUE(frc::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d{2, 0, 3}));
}
TEST(AngleStatisticsTest, Add_DynamicSize) {
Eigen::VectorXd a{{1, 1 * std::numbers::pi / 180, 2}};
Eigen::VectorXd b{{1, 359 * std::numbers::pi / 180, 1}};
EXPECT_TRUE(frc::AngleAdd<Eigen::Dynamic>(a, b, 1).isApprox(
Eigen::VectorXd{{2, 0, 3}}));
}