diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java index 5b15760832..8db6a3540f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java @@ -51,6 +51,36 @@ public class DifferentialDrivePoseEstimator { private final TimeInterpolatableBuffer m_poseBuffer = TimeInterpolatableBuffer.createBuffer(1.5); + /** + * 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 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. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java index 5b9b23cf2f..6702142ac2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java @@ -51,6 +51,33 @@ public class MecanumDrivePoseEstimator { private final TimeInterpolatableBuffer m_poseBuffer = TimeInterpolatableBuffer.createBuffer(1.5); + /** + * 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 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. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java index ed3e02bf42..bc01f28b4a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java @@ -50,6 +50,33 @@ public class SwerveDrivePoseEstimator { private final TimeInterpolatableBuffer m_poseBuffer = TimeInterpolatableBuffer.createBuffer(1.5); + /** + * 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 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. * diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index dde169ba69..39e0d8cb65 100644 --- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/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, diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index f381435d71..9642e08f98 100644 --- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/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, diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h index d8502bf6a3..b74d2bb841 100644 --- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h +++ b/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. * diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index 4d42782bda..d07d77095b 100644 --- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/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. * diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index fcf2187e16..f8d3461780 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -44,6 +44,31 @@ namespace frc { template 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& kinematics, + const Rotation2d& gyroAngle, + const wpi::array& modulePositions, + const Pose2d& initialPose) + : SwerveDrivePoseEstimator{kinematics, gyroAngle, + modulePositions, initialPose, + {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}} {} + /** * Constructs a SwerveDrivePoseEstimator. *