Class MecanumDrivePoseEstimator
public class MecanumDrivePoseEstimator extends Object
Mecanum Drive Odometry
to fuse latencycompensated
vision measurements with mecanum drive encoder distance measurements. It will correct for noisy
measurements and encoder drift. It is intended to be a dropin replacement for MecanumDriveOdometry
.
update(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.MecanumDriveWheelPositions)
should be called every robot loop.
addVisionMeasurement(edu.wpi.first.math.geometry.Pose2d, double)
can be called as infrequently as you
want; if you never call it, then this class will behave mostly like regular encoder odometry.

Constructor Summary
Constructors Constructor Description MecanumDrivePoseEstimator(MecanumDriveKinematics kinematics, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d initialPoseMeters)
Constructs a MecanumDrivePoseEstimator with default standard deviations for the model and vision measurements.MecanumDrivePoseEstimator(MecanumDriveKinematics kinematics, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d initialPoseMeters, Matrix<N3,N1> stateStdDevs, Matrix<N3,N1> visionMeasurementStdDevs)
Constructs a MecanumDrivePoseEstimator. 
Method Summary
Modifier and Type Method Description void
addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds)
Adds a vision measurement to the Kalman Filter.void
addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix<N3,N1> visionMeasurementStdDevs)
Adds a vision measurement to the Kalman Filter.Pose2d
getEstimatedPosition()
Gets the estimated robot pose.void
resetPosition(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters)
Resets the robot's position on the field.void
setVisionMeasurementStdDevs(Matrix<N3,N1> visionMeasurementStdDevs)
Sets the pose estimator's trust of global measurements.Pose2d
update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions)
Updates the pose estimator with wheel encoder and gyro information.Pose2d
updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions)
Updates the pose estimator with wheel encoder and gyro information.

Constructor Details

MecanumDrivePoseEstimator
public MecanumDrivePoseEstimator(MecanumDriveKinematics kinematics, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d initialPoseMeters)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.
 Parameters:
kinematics
 A correctlyconfigured kinematics object for your drivetrain.gyroAngle
 The current gyro angle.wheelPositions
 The distances driven by each wheel.initialPoseMeters
 The starting pose estimate.

MecanumDrivePoseEstimator
public MecanumDrivePoseEstimator(MecanumDriveKinematics kinematics, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d initialPoseMeters, Matrix<N3,N1> stateStdDevs, Matrix<N3,N1> visionMeasurementStdDevs)Constructs a MecanumDrivePoseEstimator. Parameters:
kinematics
 A correctlyconfigured kinematics object for your drivetrain.gyroAngle
 The current gyro angle.wheelPositions
 The distance measured by each wheel.initialPoseMeters
 The starting pose estimate.stateStdDevs
 Standard deviations of the pose estimate (x position in meters, y position in meters, and heading in radians). Increase these numbers to trust your state estimate less.visionMeasurementStdDevs
 Standard deviations of the vision pose measurement (x position in meters, y position in meters, and heading in radians). Increase these numbers to trust the vision pose measurement less.


Method Details

setVisionMeasurementStdDevs
Sets the pose estimator's trust of global measurements. This might be used to change trust in vision measurements after the autonomous period, or to change trust as distance to a vision target increases. Parameters:
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]ᵀ, with units in meters and radians.

resetPosition
public void resetPosition(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters)Resets the robot's position on the field.The gyroscope angle does not need to be reset in the user's robot code. The library automatically takes care of offsetting the gyro angle.
 Parameters:
gyroAngle
 The angle reported by the gyroscope.wheelPositions
 The distances driven by each wheel.poseMeters
 The position on the field that your robot is at.

getEstimatedPosition
Gets the estimated robot pose. Returns:
 The estimated robot pose in meters.

addVisionMeasurement
Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate while still accounting for measurement noise.This method can be called as infrequently as you want, as long as you are calling
update(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.MecanumDriveWheelPositions)
every loop.To promote stability of the pose estimate and make it robust to bad vision data, we recommend only adding vision measurements that are already within one meter or so of the current pose estimate.
 Parameters:
visionRobotPoseMeters
 The pose of the robot as measured by the vision camera.timestampSeconds
 The timestamp of the vision measurement in seconds. Note that if you don't use your own time source by callingupdateWithTime(double,Rotation2d,MecanumDriveWheelPositions)
then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch asTimer.getFPGATimestamp()
.) This means that you should useTimer.getFPGATimestamp()
as your time source or sync the epochs.

addVisionMeasurement
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix<N3,N1> visionMeasurementStdDevs)Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate while still accounting for measurement noise.This method can be called as infrequently as you want, as long as you are calling
update(edu.wpi.first.math.geometry.Rotation2d, edu.wpi.first.math.kinematics.MecanumDriveWheelPositions)
every loop.To promote stability of the pose estimate and make it robust to bad vision data, we recommend only adding vision measurements that are already within one meter or so of the current pose estimate.
Note that the vision measurement standard deviations passed into this method will continue to apply to future measurements until a subsequent call to
setVisionMeasurementStdDevs(Matrix)
or this method. Parameters:
visionRobotPoseMeters
 The pose of the robot as measured by the vision camera.timestampSeconds
 The timestamp of the vision measurement in seconds. Note that if you don't use your own time source by callingupdateWithTime(double,Rotation2d,MecanumDriveWheelPositions)
, then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch asTimer.getFPGATimestamp()
). This means that you should useTimer.getFPGATimestamp()
as your time source in this case.visionMeasurementStdDevs
 Standard deviations of the vision pose measurement (x position in meters, y position in meters, and heading in radians). Increase these numbers to trust the vision pose measurement less.

update
Updates the pose estimator with wheel encoder and gyro information. This should be called every loop. Parameters:
gyroAngle
 The current gyro angle.wheelPositions
 The distances driven by each wheel. Returns:
 The estimated pose of the robot in meters.

updateWithTime
public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions)Updates the pose estimator with wheel encoder and gyro information. This should be called every loop. Parameters:
currentTimeSeconds
 Time at which this method was called, in seconds.gyroAngle
 The current gyroscope angle.wheelPositions
 The distances driven by each wheel. Returns:
 The estimated pose of the robot in meters.
