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] Discard stale pose estimates (#5045)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Jordan McMichael
2023-02-04 01:04:30 -05:00
committed by GitHub
parent fe5d226a19
commit 4079eabe9b
14 changed files with 283 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
View File

@@ -275,3 +275,38 @@ TEST(SwerveDrivePoseEstimatorTest, SimultaneousVisionMeasurements) {
EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad);
}
}
TEST(SwerveDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) {
frc::SwerveDriveKinematics<4> kinematics{
frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
frc::SwerveModulePosition fl;
frc::SwerveModulePosition fr;
frc::SwerveModulePosition bl;
frc::SwerveModulePosition br;
frc::SwerveDrivePoseEstimator<4> estimator{
kinematics, frc::Rotation2d{}, {fl, fr, bl, br},
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}};
// Add enough measurements to fill up the bufer
for (auto time = 0.0_s; time < 4_s; time += 0.02_s) {
estimator.UpdateWithTime(time, frc::Rotation2d{}, {fl, fr, bl, br});
}
auto odometryPose = estimator.GetEstimatedPosition();
// Apply a vision measurement from 3 seconds ago
estimator.AddVisionMeasurement(
frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}},
1_s, {0.1, 0.1, 0.1});
EXPECT_NEAR(odometryPose.X().value(),
estimator.GetEstimatedPosition().X().value(), 1e-6);
EXPECT_NEAR(odometryPose.Y().value(),
estimator.GetEstimatedPosition().Y().value(), 1e-6);
EXPECT_NEAR(odometryPose.Rotation().Radians().value(),
estimator.GetEstimatedPosition().Rotation().Radians().value(),
1e-6);
}