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

wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp

@@ -38,6 +38,14 @@ DifferentialDrivePoseEstimator::InterpolationRecord::Interpolate(
   }
 }
 
+DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
+    DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
+    units::meter_t leftDistance, units::meter_t rightDistance,
+    const Pose2d& initialPose)
+    : DifferentialDrivePoseEstimator{
+          kinematics,  gyroAngle,          leftDistance,   rightDistance,
+          initialPose, {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}} {}
+
 DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
     DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
     units::meter_t leftDistance, units::meter_t rightDistance,

wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp

@@ -51,6 +51,13 @@ frc::MecanumDrivePoseEstimator::InterpolationRecord::Interpolate(
   }
 }
 
+frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
+    MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
+    const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose)
+    : MecanumDrivePoseEstimator{kinematics,      gyroAngle,
+                                wheelPositions,  initialPose,
+                                {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}} {}
+
 frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
     MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
     const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose,

wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h

@@ -44,6 +44,28 @@ namespace frc {
  */
 class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
  public:
+  /**
+   * Constructs a DifferentialDrivePoseEstimator with default standard
+   * deviations for the model and vision measurements.
+   *
+   * 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 gyro angle of the robot.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   * @param initialPose              The estimated initial pose.
+   */
+  DifferentialDrivePoseEstimator(DifferentialDriveKinematics& kinematics,
+                                 const Rotation2d& gyroAngle,
+                                 units::meter_t leftDistance,
+                                 units::meter_t rightDistance,
+                                 const Pose2d& initialPose);
+
   /**
    * Constructs a DifferentialDrivePoseEstimator.
    *

wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h

@@ -44,6 +44,26 @@ namespace frc {
  */
 class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
  public:
+  /**
+   * Constructs a MecanumDrivePoseEstimator with default standard deviations
+   * for the model and vision measurements.
+   *
+   * 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 distance measured by each wheel.
+   * @param initialPose              The starting pose estimate.
+   */
+  MecanumDrivePoseEstimator(MecanumDriveKinematics& kinematics,
+                            const Rotation2d& gyroAngle,
+                            const MecanumDriveWheelPositions& wheelPositions,
+                            const Pose2d& initialPose);
+
   /**
    * Constructs a MecanumDrivePoseEstimator.
    *

wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h

@@ -44,6 +44,31 @@ namespace frc {
 template <size_t NumModules>
 class SwerveDrivePoseEstimator {
  public:
+  /**
+   * Constructs a SwerveDrivePoseEstimator with default standard deviations
+   * for the model and vision measurements.
+   *
+   * 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 and rotation
+   *                                 measurements of the swerve modules.
+   * @param initialPose              The starting pose estimate.
+   */
+  SwerveDrivePoseEstimator(
+      SwerveDriveKinematics<NumModules>& kinematics,
+      const Rotation2d& gyroAngle,
+      const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
+      const Pose2d& initialPose)
+      : SwerveDrivePoseEstimator{kinematics,      gyroAngle,
+                                 modulePositions, initialPose,
+                                 {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}} {}
+
   /**
    * Constructs a SwerveDrivePoseEstimator.
    *