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

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.
    *