[wpimath] Add pose estimator overload for vision + std dev measurement (#3200)

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

@@ -256,6 +256,36 @@ public class DifferentialDrivePoseEstimator {
         timestampSeconds);
   }
 
+  /**
+   * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
+   * estimate while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
+   * DifferentialDrivePoseEstimator#update} every loop.
+   *
+   * <p>Note that the vision measurement standard deviations passed into this method will continue
+   * to apply to future measurements until a subsequent call to {@link
+   * DifferentialDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
+   *
+   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
+   *     don't use your own time source by calling {@link
+   *     DifferentialDrivePoseEstimator#updateWithTime} then you must use a timestamp with an epoch
+   *     since FPGA startup (i.e. the epoch of this timestamp is the same epoch as
+   *     Timer.getFPGATimestamp.) This means that you should use Timer.getFPGATimestamp as your time
+   *     source in this case.
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]^T, with units in meters and radians.
+   */
+  public void addVisionMeasurement(
+      Pose2d visionRobotPoseMeters,
+      double timestampSeconds,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+  }
+
   /**
    * Updates the the Unscented Kalman Filter using only wheel encoder information. Note that this
    * should be called every loop.

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

@@ -220,6 +220,35 @@ public class MecanumDrivePoseEstimator {
         timestampSeconds);
   }
 
+  /**
+   * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
+   * estimate while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
+   * MecanumDrivePoseEstimator#update} every loop.
+   *
+   * <p>Note that the vision measurement standard deviations passed into this method will continue
+   * to apply to future measurements until a subsequent call to {@link
+   * MecanumDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
+   *
+   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
+   *     don't use your own time source by calling {@link MecanumDrivePoseEstimator#updateWithTime}
+   *     then you must use a timestamp with an epoch since FPGA startup (i.e. the epoch of this
+   *     timestamp is the same epoch as Timer.getFPGATimestamp.) This means that you should use
+   *     Timer.getFPGATimestamp as your time source in this case.
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]^T, with units in meters and radians.
+   */
+  public void addVisionMeasurement(
+      Pose2d visionRobotPoseMeters,
+      double timestampSeconds,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+  }
+
   /**
    * Updates the the Unscented Kalman Filter using only wheel encoder information. This should be
    * called every loop, and the correct loop period must be passed into the constructor of this

wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java

@@ -220,6 +220,35 @@ public class SwerveDrivePoseEstimator {
         timestampSeconds);
   }
 
+  /**
+   * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
+   * estimate while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
+   * SwerveDrivePoseEstimator#update} every loop.
+   *
+   * <p>Note that the vision measurement standard deviations passed into this method will continue
+   * to apply to future measurements until a subsequent call to {@link
+   * SwerveDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
+   *
+   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
+   *     don't use your own time source by calling {@link SwerveDrivePoseEstimator#updateWithTime}
+   *     then you must use a timestamp with an epoch since FPGA startup (i.e. the epoch of this
+   *     timestamp is the same epoch as Timer.getFPGATimestamp.) This means that you should use
+   *     Timer.getFPGATimestamp as your time source in this case.
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]^T, with units in meters and radians.
+   */
+  public void addVisionMeasurement(
+      Pose2d visionRobotPoseMeters,
+      double timestampSeconds,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+  }
+
   /**
    * Updates the the Unscented Kalman Filter using only wheel encoder information. This should be
    * called every loop, and the correct loop period must be passed into the constructor of this