diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java index ada6fc72d03..c78716a9104 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java @@ -40,8 +40,10 @@ public class DifferentialDrivePoseEstimator { private final Matrix m_q = new Matrix<>(Nat.N3(), Nat.N1()); private Matrix m_visionK = new Matrix<>(Nat.N3(), Nat.N3()); + private static final double kBufferDuration = 1.5; + private final TimeInterpolatableBuffer m_poseBuffer = - TimeInterpolatableBuffer.createBuffer(1.5); + TimeInterpolatableBuffer.createBuffer(kBufferDuration); /** * Constructs a DifferentialDrivePoseEstimator with default standard deviations for the model and @@ -187,6 +189,11 @@ public Pose2d getEstimatedPosition() { * or sync the epochs. */ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { + // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip. + if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) { + return; + } + // Step 1: Get the pose odometry measured at the moment the vision measurement was made. var sample = m_poseBuffer.getSample(timestampSeconds); diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java index 7c8b0a7c410..b6f11c44a48 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java @@ -39,8 +39,10 @@ public class MecanumDrivePoseEstimator { private final Matrix m_q = new Matrix<>(Nat.N3(), Nat.N1()); private Matrix m_visionK = new Matrix<>(Nat.N3(), Nat.N3()); + private static final double kBufferDuration = 1.5; + private final TimeInterpolatableBuffer m_poseBuffer = - TimeInterpolatableBuffer.createBuffer(1.5); + TimeInterpolatableBuffer.createBuffer(kBufferDuration); /** * Constructs a MecanumDrivePoseEstimator with default standard deviations for the model and @@ -175,6 +177,11 @@ public Pose2d getEstimatedPosition() { * your time source or sync the epochs. */ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { + // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip. + if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) { + return; + } + // Step 1: Get the pose odometry measured at the moment the vision measurement was made. var sample = m_poseBuffer.getSample(timestampSeconds); diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java index 3a57ff09ddf..a36bf71e15f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java @@ -40,8 +40,10 @@ public class SwerveDrivePoseEstimator { private final int m_numModules; private Matrix m_visionK = new Matrix<>(Nat.N3(), Nat.N3()); + private static final double kBufferDuration = 1.5; + private final TimeInterpolatableBuffer m_poseBuffer = - TimeInterpolatableBuffer.createBuffer(1.5); + TimeInterpolatableBuffer.createBuffer(kBufferDuration); /** * Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision @@ -177,6 +179,11 @@ public Pose2d getEstimatedPosition() { * or sync the epochs. */ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { + // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip. + if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) { + return; + } + // Step 1: Get the pose odometry measured at the moment the vision measurement was made. var sample = m_poseBuffer.getSample(timestampSeconds); diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index 27d745cdfd4..590991cd03a 100644 --- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -94,6 +94,13 @@ Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const { void DifferentialDrivePoseEstimator::AddVisionMeasurement( const Pose2d& visionRobotPose, units::second_t timestamp) { + // Step 0: If this measurement is old enough to be outside the pose buffer's + // timespan, skip. + if (m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration > + timestamp) { + return; + } + // Step 1: Get the estimated pose from when the vision measurement was made. auto sample = m_poseBuffer.Sample(timestamp); diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index a7fa523f2cb..dcb668fcd57 100644 --- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -105,6 +105,13 @@ Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const { void frc::MecanumDrivePoseEstimator::AddVisionMeasurement( const Pose2d& visionRobotPose, units::second_t timestamp) { + // Step 0: If this measurement is old enough to be outside the pose buffer's + // timespan, skip. + if (m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration > + timestamp) { + return; + } + // Step 1: Get the estimated pose from when the vision measurement was made. auto sample = m_poseBuffer.Sample(timestamp); diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h index 339ccc98d32..49f00e39e4e 100644 --- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h @@ -240,14 +240,16 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator { double i) const; }; + static constexpr units::second_t kBufferDuration = 1.5_s; + DifferentialDriveKinematics& m_kinematics; DifferentialDriveOdometry m_odometry; wpi::array m_q{wpi::empty_array}; Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero(); TimeInterpolatableBuffer m_poseBuffer{ - 1.5_s, [this](const InterpolationRecord& start, - const InterpolationRecord& end, double t) { + kBufferDuration, [this](const InterpolationRecord& start, + const InterpolationRecord& end, double t) { return start.Interpolate(this->m_kinematics, end, t); }}; }; diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index d1967e9db91..10fa4e9836d 100644 --- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -232,14 +232,16 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator { double i) const; }; + static constexpr units::second_t kBufferDuration = 1.5_s; + MecanumDriveKinematics& m_kinematics; MecanumDriveOdometry m_odometry; wpi::array m_q{wpi::empty_array}; Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero(); TimeInterpolatableBuffer m_poseBuffer{ - 1.5_s, [this](const InterpolationRecord& start, - const InterpolationRecord& end, double t) { + kBufferDuration, [this](const InterpolationRecord& start, + const InterpolationRecord& end, double t) { return start.Interpolate(this->m_kinematics, end, t); }}; }; diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index ea282fdb516..bff45cc05f4 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -170,6 +170,13 @@ class SwerveDrivePoseEstimator { */ void AddVisionMeasurement(const Pose2d& visionRobotPose, units::second_t timestamp) { + // Step 0: If this measurement is old enough to be outside the pose buffer's + // timespan, skip. + if (m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration > + timestamp) { + return; + } + // Step 1: Get the estimated pose from when the vision measurement was made. auto sample = m_poseBuffer.Sample(timestamp); @@ -374,14 +381,16 @@ class SwerveDrivePoseEstimator { } }; + static constexpr units::second_t kBufferDuration = 1.5_s; + SwerveDriveKinematics& m_kinematics; SwerveDriveOdometry m_odometry; wpi::array m_q{wpi::empty_array}; Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero(); TimeInterpolatableBuffer m_poseBuffer{ - 1.5_s, [this](const InterpolationRecord& start, - const InterpolationRecord& end, double t) { + kBufferDuration, [this](const InterpolationRecord& start, + const InterpolationRecord& end, double t) { return start.Interpolate(this->m_kinematics, end, t); }}; }; diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java index a353bc94a96..e0e202f98c3 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java @@ -266,4 +266,41 @@ void testSimultaneousVisionMeasurements() { assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog); } } + + @Test + void testDiscardsStaleVisionMeasurements() { + var kinematics = new DifferentialDriveKinematics(1); + var estimator = + new DifferentialDrivePoseEstimator( + kinematics, + new Rotation2d(), + 0, + 0, + new Pose2d(), + VecBuilder.fill(0.1, 0.1, 0.1), + VecBuilder.fill(0.9, 0.9, 0.9)); + + double time = 0; + + // Add enough measurements to fill up the buffer + for (; time < 4; time += 0.02) { + estimator.updateWithTime(time, new Rotation2d(), 0, 0); + } + + var odometryPose = estimator.getEstimatedPosition(); + + // Apply a vision measurement made 3 seconds ago + // This test passes if this does not cause a ConcurrentModificationException. + estimator.addVisionMeasurement( + new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)), + 1, + VecBuilder.fill(0.1, 0.1, 0.1)); + + assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X"); + assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y"); + assertEquals( + odometryPose.getRotation().getRadians(), + estimator.getEstimatedPosition().getRotation().getRadians(), + "Incorrect Final Theta"); + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java index 3c99e91e52c..d844c5d86ae 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java @@ -279,4 +279,45 @@ void testSimultaneousVisionMeasurements() { assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog); } } + + @Test + void testDiscardsOldVisionMeasurements() { + var kinematics = + new MecanumDriveKinematics( + new Translation2d(1, 1), + new Translation2d(-1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1)); + var estimator = + new MecanumDrivePoseEstimator( + kinematics, + new Rotation2d(), + new MecanumDriveWheelPositions(), + new Pose2d(), + VecBuilder.fill(0.1, 0.1, 0.1), + VecBuilder.fill(0.9, 0.9, 0.9)); + + double time = 0; + + // Add enough measurements to fill up the buffer + for (; time < 4; time += 0.02) { + estimator.updateWithTime(time, new Rotation2d(), new MecanumDriveWheelPositions()); + } + + var odometryPose = estimator.getEstimatedPosition(); + + // Apply a vision measurement made 3 seconds ago + // This test passes if this does not cause a ConcurrentModificationException. + estimator.addVisionMeasurement( + new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)), + 1, + VecBuilder.fill(0.1, 0.1, 0.1)); + + assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X"); + assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y"); + assertEquals( + odometryPose.getRotation().getRadians(), + estimator.getEstimatedPosition().getRotation().getRadians(), + "Incorrect Final Theta"); + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java index c4ac274c09b..9fcd8525db8 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java @@ -300,4 +300,58 @@ void testSimultaneousVisionMeasurements() { assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog); } } + + @Test + void testDiscardsOldVisionMeasurements() { + var kinematics = + new SwerveDriveKinematics( + new Translation2d(1, 1), + new Translation2d(-1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1)); + var estimator = + new SwerveDrivePoseEstimator( + kinematics, + new Rotation2d(), + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }, + new Pose2d(), + VecBuilder.fill(0.1, 0.1, 0.1), + VecBuilder.fill(0.9, 0.9, 0.9)); + + double time = 0; + + // Add enough measurements to fill up the buffer + for (; time < 4; time += 0.02) { + estimator.updateWithTime( + time, + new Rotation2d(), + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }); + } + + var odometryPose = estimator.getEstimatedPosition(); + + // Apply a vision measurement made 3 seconds ago + // This test passes if this does not cause a ConcurrentModificationException. + estimator.addVisionMeasurement( + new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)), + 1, + VecBuilder.fill(0.1, 0.1, 0.1)); + + assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X"); + assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y"); + assertEquals( + odometryPose.getRotation().getRadians(), + estimator.getEstimatedPosition().getRotation().getRadians(), + "Incorrect Final Theta"); + } } diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp index 4c91c137beb..1e79b0b6e8e 100644 --- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp @@ -263,3 +263,31 @@ TEST(DifferentialDrivePoseEstimatorTest, SimultaneousVisionMeasurements) { EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); } } + +TEST(DifferentialDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) { + frc::DifferentialDriveKinematics kinematics{1_m}; + + frc::DifferentialDrivePoseEstimator estimator{ + kinematics, frc::Rotation2d{}, 0_m, 0_m, 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{}, 0_m, 0_m); + } + + 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); +} diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp index 79c2cb859e0..acb32c187bb 100644 --- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp @@ -262,3 +262,34 @@ TEST(MecanumDrivePoseEstimatorTest, SimultaneousVisionMeasurements) { EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); } } + +TEST(MecanumDrivePoseEstimatorTest, TestDiscardStaleVisionMeasurements) { + frc::MecanumDriveKinematics 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::MecanumDrivePoseEstimator estimator{ + kinematics, frc::Rotation2d{}, frc::MecanumDriveWheelPositions{}, + 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{}, + frc::MecanumDriveWheelPositions{}); + } + + 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); +} diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp index 6d5c61f0cad..a4388f3e25c 100644 --- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp @@ -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); +}