Package edu.wpi.first.math.estimator

Class MecanumDrivePoseEstimator

java.lang.Object

edu.wpi.first.math.estimator.PoseEstimator<MecanumDriveWheelPositions>

edu.wpi.first.math.estimator.MecanumDrivePoseEstimator

public class MecanumDrivePoseEstimator
extends PoseEstimator<MecanumDriveWheelPositions>

This class wraps Mecanum Drive Odometry to fuse latency-compensated vision measurements with mecanum drive encoder distance measurements. It will correct for noisy measurements and encoder drift. It is intended to be a drop-in replacement for MecanumDriveOdometry.

PoseEstimator.update(edu.wpi.first.math.geometry.Rotation2d, T) should be called every robot loop.

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

Methods inherited from class edu.wpi.first.math.estimator.PoseEstimator

addVisionMeasurement, addVisionMeasurement, getEstimatedPosition, resetPosition, setVisionMeasurementStdDevs, update, updateWithTime

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

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 correctly-configured 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 correctly-configured 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.