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

[wpimath] Allow multiple vision measurements from same timestamp (#4917)

Browse Source 
Co-authored-by: Jordan McMichael <jlmcmchl@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
2023-01-12 02:04:30 -05:00
committed by GitHub
parent befd12911c
commit f7f19207e0
13 changed files with 409 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
View File

@@ -207,3 +207,59 @@ TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) {
}
}
}
TEST(DifferentialDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
// This tests for multiple vision measurements appled at the same time.
// The expected behavior is that all measurements affect the estimated pose.
// The alternative result is that only one vision measurement affects the
// outcome. If that were the case, after 1000 measurements, the estimated
// pose would converge to that measurement.
frc::DifferentialDriveKinematics kinematics{1.0_m};
frc::DifferentialDrivePoseEstimator estimator{
kinematics,
frc::Rotation2d{},
0_m,
0_m,
frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}},
{0.02, 0.02, 0.01},
{0.1, 0.1, 0.1}};
estimator.UpdateWithTime(0_s, frc::Rotation2d{}, 0_m, 0_m);
for (int i = 0; i < 1000; i++) {
estimator.AddVisionMeasurement(
frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s);
estimator.AddVisionMeasurement(
frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m);
auto dtheta = units::math::abs(
estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m);
auto dtheta = units::math::abs(
estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
{
auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m);
auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m);
auto dtheta = units::math::abs(
estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg);
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
}