[wpimath] Discard stale pose estimates (#5045)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java

@@ -40,8 +40,10 @@ public class DifferentialDrivePoseEstimator {
   private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
   private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
 
+  private static final double kBufferDuration = 1.5;
+
   private final TimeInterpolatableBuffer<InterpolationRecord> 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 class DifferentialDrivePoseEstimator {
    *     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);
 

wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java

@@ -39,8 +39,10 @@ public class MecanumDrivePoseEstimator {
   private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
   private Matrix<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
 
+  private static final double kBufferDuration = 1.5;
+
   private final TimeInterpolatableBuffer<InterpolationRecord> 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 class MecanumDrivePoseEstimator {
    *     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);
 

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<N3, N3> m_visionK = new Matrix<>(Nat.N3(), Nat.N3());
 
+  private static final double kBufferDuration = 1.5;
+
   private final TimeInterpolatableBuffer<InterpolationRecord> 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 class SwerveDrivePoseEstimator {
    *     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);
 

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);
 

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);
 

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<double, 3> m_q{wpi::empty_array};
   Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
 
   TimeInterpolatableBuffer<InterpolationRecord> 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);
       }};
 };

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<double, 3> m_q{wpi::empty_array};
   Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
 
   TimeInterpolatableBuffer<InterpolationRecord> 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);
       }};
 };

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<NumModules>& m_kinematics;
   SwerveDriveOdometry<NumModules> m_odometry;
   wpi::array<double, 3> m_q{wpi::empty_array};
   Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
 
   TimeInterpolatableBuffer<InterpolationRecord> 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);
       }};
 };

wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java

@@ -266,4 +266,41 @@ class DifferentialDrivePoseEstimatorTest {
       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");
+  }
 }

wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java

@@ -279,4 +279,45 @@ class MecanumDrivePoseEstimatorTest {
       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");
+  }
 }

wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java

@@ -300,4 +300,58 @@ class SwerveDrivePoseEstimatorTest {
       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");
+  }
 }

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);
+}

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);
+}

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);
+}