This class wraps Mecanum Drive Odometry to fuse latency-compensated vision measurements with mecanum drive encoder velocity measurements. More...

#include <frc/estimator/MecanumDrivePoseEstimator.h>

Public Member Functions
	MecanumDrivePoseEstimator (MecanumDriveKinematics &kinematics, const Rotation2d &gyroAngle, const MecanumDriveWheelPositions &wheelPositions, const Pose2d &initialPose)
	Constructs a MecanumDrivePoseEstimator with default standard deviations for the model and vision measurements. More...

	MecanumDrivePoseEstimator (MecanumDriveKinematics &kinematics, const Rotation2d &gyroAngle, const MecanumDriveWheelPositions &wheelPositions, const Pose2d &initialPose, const wpi::array< double, 3 > &stateStdDevs, const wpi::array< double, 3 > &visionMeasurementStdDevs)
	Constructs a MecanumDrivePoseEstimator. More...

void	SetVisionMeasurementStdDevs (const wpi::array< double, 3 > &visionMeasurementStdDevs)
	Sets the pose estimator's trust in vision measurements. More...

void	ResetPosition (const Rotation2d &gyroAngle, const MecanumDriveWheelPositions &wheelPositions, const Pose2d &pose)
	Resets the robot's position on the field. More...

Pose2d	GetEstimatedPosition () const
	Gets the estimated robot pose. More...

void	AddVisionMeasurement (const Pose2d &visionRobotPose, units::second_t timestamp)
	Add a vision measurement to the Kalman Filter. More...

void	AddVisionMeasurement (const Pose2d &visionRobotPose, units::second_t timestamp, const wpi::array< double, 3 > &visionMeasurementStdDevs)
	Adds a vision measurement to the Kalman Filter. More...

Pose2d	Update (const Rotation2d &gyroAngle, const MecanumDriveWheelPositions &wheelPositions)
	Updates the pose estimator with wheel encoder and gyro information. More...

Pose2d	UpdateWithTime (units::second_t currentTime, const Rotation2d &gyroAngle, const MecanumDriveWheelPositions &wheelPositions)
	Updates the pose estimator with wheel encoder and gyro information. More...

Detailed Description

This class wraps Mecanum Drive Odometry to fuse latency-compensated vision measurements with mecanum drive encoder velocity measurements.

It will correct for noisy measurements and encoder drift. It is intended to be an easy drop-in for MecanumDriveOdometry.

Update() should be called every robot loop. If your loops are faster or slower than the default of 20 ms, then you should change the nominal delta time by specifying it in the constructor.

AddVisionMeasurement() can be called as infrequently as you want; if you never call it, then this class will behave mostly like regular encoder odometry.

Constructor & Destructor Documentation

◆ MecanumDrivePoseEstimator() [1/2]

frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator	(	MecanumDriveKinematics &	kinematics,
		const Rotation2d &	gyroAngle,
		const MecanumDriveWheelPositions &	wheelPositions,
		const Pose2d &	initialPose
	)

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 distance measured by each wheel.
initialPose	The starting pose estimate.

◆ MecanumDrivePoseEstimator() [2/2]

frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator	(	MecanumDriveKinematics &	kinematics,
		const Rotation2d &	gyroAngle,
		const MecanumDriveWheelPositions &	wheelPositions,
		const Pose2d &	initialPose,
		const wpi::array< double, 3 > &	stateStdDevs,
		const wpi::array< double, 3 > &	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.
initialPose	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.

Member Function Documentation

◆ AddVisionMeasurement() [1/2]

void frc::MecanumDrivePoseEstimator::AddVisionMeasurement	(	const Pose2d &	visionRobotPose,
		units::second_t	timestamp
	)

Add 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() 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

visionRobotPose	The pose of the robot as measured by the vision camera.
timestamp	The timestamp of the vision measurement in seconds. Note that if you don't use your own time source by calling UpdateWithTime() then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as frc::Timer::GetFPGATimestamp().) This means that you should use frc::Timer::GetFPGATimestamp() as your time source or sync the epochs.

◆ AddVisionMeasurement() [2/2]

void frc::MecanumDrivePoseEstimator::AddVisionMeasurement	(	const Pose2d &	visionRobotPose,
		units::second_t	timestamp,
		const wpi::array< double, 3 > &	visionMeasurementStdDevs
	)

inline

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() 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() or this method.

Parameters

visionRobotPose	The pose of the robot as measured by the vision camera.
timestamp	The timestamp of the vision measurement in seconds. Note that if you don't use your own time source by calling UpdateWithTime(), then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as frc::Timer::GetFPGATimestamp(). This means that you should use frc::Timer::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.

◆ GetEstimatedPosition()

Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition ( ) const

Gets the estimated robot pose.

Returns: The estimated robot pose in meters.

◆ ResetPosition()

void frc::MecanumDrivePoseEstimator::ResetPosition	(	const Rotation2d &	gyroAngle,
		const MecanumDriveWheelPositions &	wheelPositions,
		const Pose2d &	pose
	)

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 measured at each wheel.
pose	The position on the field that your robot is at.

◆ SetVisionMeasurementStdDevs()

void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs ( const wpi::array< double, 3 > & visionMeasurementStdDevs )

Sets the pose estimator's trust in vision 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 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()

Pose2d frc::MecanumDrivePoseEstimator::Update	(	const Rotation2d &	gyroAngle,
		const MecanumDriveWheelPositions &	wheelPositions
	)

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 measured at each wheel.

Returns: The estimated pose of the robot in meters.

◆ UpdateWithTime()

Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime	(	units::second_t	currentTime,
		const Rotation2d &	gyroAngle,
		const MecanumDriveWheelPositions &	wheelPositions
	)

Updates the pose estimator with wheel encoder and gyro information.

This should be called every loop.

Parameters

currentTime	Time at which this method was called, in seconds.
gyroAngle	The current gyroscope angle.
wheelPositions	The distances measured at each wheel.

Returns: The estimated pose of the robot in meters.

The documentation for this class was generated from the following file:

frc/estimator/MecanumDrivePoseEstimator.h

Public Member Functions

Detailed Description

Constructor & Destructor Documentation

◆ MecanumDrivePoseEstimator() [1/2]

◆ MecanumDrivePoseEstimator() [2/2]

Member Function Documentation

◆ AddVisionMeasurement() [1/2]

◆ AddVisionMeasurement() [2/2]

◆ GetEstimatedPosition()

◆ ResetPosition()

◆ SetVisionMeasurementStdDevs()

◆ Update()

◆ UpdateWithTime()