[wpimath] Add constructors to pose estimators with default standard deviations (#4754)

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

@@ -51,6 +51,36 @@ public class DifferentialDrivePoseEstimator {
   private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
       TimeInterpolatableBuffer.createBuffer(1.5);
 
+  /**
+   * Constructs a DifferentialDrivePoseEstimator with default standard deviations for the model and
+   * vision measurements.
+   *
+   * <p>The default standard deviations of the model states are 0.02 meters for x, 0.02 meters for
+   * y, and 0.01 radians for heading. The default standard deviations of the vision measurements are
+   * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param leftDistanceMeters The distance traveled by the left encoder.
+   * @param rightDistanceMeters The distance traveled by the right encoder.
+   * @param initialPoseMeters The starting pose estimate.
+   */
+  public DifferentialDrivePoseEstimator(
+      DifferentialDriveKinematics kinematics,
+      Rotation2d gyroAngle,
+      double leftDistanceMeters,
+      double rightDistanceMeters,
+      Pose2d initialPoseMeters) {
+    this(
+        kinematics,
+        gyroAngle,
+        leftDistanceMeters,
+        rightDistanceMeters,
+        initialPoseMeters,
+        VecBuilder.fill(0.02, 0.02, 0.01),
+        VecBuilder.fill(0.1, 0.1, 0.1));
+  }
+
   /**
    * Constructs a DifferentialDrivePoseEstimator.
    *

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

@@ -51,6 +51,33 @@ public class MecanumDrivePoseEstimator {
   private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
       TimeInterpolatableBuffer.createBuffer(1.5);
 
+  /**
+   * Constructs a MecanumDrivePoseEstimator with default standard deviations for the model and
+   * vision measurements.
+   *
+   * <p>The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y,
+   * and 0.1 radians for heading. The default standard deviations of the vision measurements are
+   * 0.45 meters for x, 0.45 meters for y, and 0.45 radians for heading.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param wheelPositions The distances driven by each wheel.
+   * @param initialPoseMeters The starting pose estimate.
+   */
+  public MecanumDrivePoseEstimator(
+      MecanumDriveKinematics kinematics,
+      Rotation2d gyroAngle,
+      MecanumDriveWheelPositions wheelPositions,
+      Pose2d initialPoseMeters) {
+    this(
+        kinematics,
+        gyroAngle,
+        wheelPositions,
+        initialPoseMeters,
+        VecBuilder.fill(0.1, 0.1, 0.1),
+        VecBuilder.fill(0.45, 0.45, 0.45));
+  }
+
   /**
    * Constructs a MecanumDrivePoseEstimator.
    *

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

@@ -50,6 +50,33 @@ public class SwerveDrivePoseEstimator {
   private final TimeInterpolatableBuffer<InterpolationRecord> m_poseBuffer =
       TimeInterpolatableBuffer.createBuffer(1.5);
 
+  /**
+   * Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision
+   * measurements.
+   *
+   * <p>The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y,
+   * and 0.1 radians for heading. The default standard deviations of the vision measurements are 0.9
+   * meters for x, 0.9 meters for y, and 0.9 radians for heading.
+   *
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param gyroAngle The current gyro angle.
+   * @param modulePositions The current distance measurements and rotations of the swerve modules.
+   * @param initialPoseMeters The starting pose estimate.
+   */
+  public SwerveDrivePoseEstimator(
+      SwerveDriveKinematics kinematics,
+      Rotation2d gyroAngle,
+      SwerveModulePosition[] modulePositions,
+      Pose2d initialPoseMeters) {
+    this(
+        kinematics,
+        gyroAngle,
+        modulePositions,
+        initialPoseMeters,
+        VecBuilder.fill(0.1, 0.1, 0.1),
+        VecBuilder.fill(0.9, 0.9, 0.9));
+  }
+
   /**
    * Constructs a SwerveDrivePoseEstimator.
    *