This class wraps odometry to fuse latency-compensated vision measurements with encoder measurements. More...

#include <frc/estimator/PoseEstimator3d.h>

Public Member Functions
	PoseEstimator3d (Kinematics< WheelSpeeds, WheelPositions > &kinematics, Odometry3d< WheelSpeeds, WheelPositions > &odometry, const wpi::array< double, 4 > &stateStdDevs, const wpi::array< double, 4 > &visionMeasurementStdDevs)
	Constructs a PoseEstimator3d.

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

void	ResetPosition (const Rotation3d &gyroAngle, const WheelPositions &wheelPositions, const Pose3d &pose)
	Resets the robot's position on the field.

void	ResetPose (const Pose3d &pose)
	Resets the robot's pose.

void	ResetTranslation (const Translation3d &translation)
	Resets the robot's translation.

void	ResetRotation (const Rotation3d &rotation)
	Resets the robot's rotation.

Pose3d	GetEstimatedPosition () const
	Gets the estimated robot pose.

std::optional< Pose3d >	SampleAt (units::second_t timestamp) const
	Return the pose at a given timestamp, if the buffer is not empty.

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

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

Pose3d	Update (const Rotation3d &gyroAngle, const WheelPositions &wheelPositions)
	Updates the pose estimator with wheel encoder and gyro information.

Pose3d	UpdateWithTime (units::second_t currentTime, const Rotation3d &gyroAngle, const WheelPositions &wheelPositions)
	Updates the pose estimator with wheel encoder and gyro information.

Detailed Description

template<typename WheelSpeeds, typename WheelPositions>
class frc::PoseEstimator3d< WheelSpeeds, WheelPositions >

This class wraps odometry to fuse latency-compensated vision measurements with encoder measurements.

Robot code should not use this directly- Instead, use the particular type for your drivetrain (e.g., DifferentialDrivePoseEstimator3d). It will correct for noisy vision measurements and encoder drift. It is intended to be an easy drop-in for Odometry3d. It is also intended to be an easy replacement for PoseEstimator, only requiring the addition of a standard deviation for Z and appropriate conversions between 2D and 3D versions of geometry classes. (See Pose3d(Pose2d), Rotation3d(Rotation2d), Translation3d(Translation2d), and Pose3d.ToPose2d().)

Update() should be called every robot loop.

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

Template Parameters

WheelSpeeds	Wheel speeds type.
WheelPositions	Wheel positions type.

Constructor & Destructor Documentation

◆ PoseEstimator3d()

template<typename WheelSpeeds , typename WheelPositions >

frc::PoseEstimator3d< WheelSpeeds, WheelPositions >::PoseEstimator3d	(	Kinematics< WheelSpeeds, WheelPositions > &	kinematics,
		Odometry3d< WheelSpeeds, WheelPositions > &	odometry,
		const wpi::array< double, 4 > &	stateStdDevs,
		const wpi::array< double, 4 > &	visionMeasurementStdDevs )

inline

Constructs a PoseEstimator3d.

Warning: The initial pose estimate will always be the default pose, regardless of the odometry's current pose.

Parameters

kinematics	A correctly-configured kinematics object for your drivetrain.
odometry	A correctly-configured odometry object for your drivetrain.
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 z position in meters, and angle in radians). Increase these numbers to trust the vision pose measurement less.

Member Function Documentation

◆ AddVisionMeasurement() [1/2]

template<typename WheelSpeeds , typename WheelPositions >

void frc::PoseEstimator3d< WheelSpeeds, WheelPositions >::AddVisionMeasurement	(	const Pose3d &	visionRobotPose,
		units::second_t	timestamp )

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.

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.

◆ AddVisionMeasurement() [2/2]

template<typename WheelSpeeds , typename WheelPositions >

void frc::PoseEstimator3d< WheelSpeeds, WheelPositions >::AddVisionMeasurement	(	const Pose3d &	visionRobotPose,
		units::second_t	timestamp,
		const wpi::array< double, 4 > &	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()

template<typename WheelSpeeds , typename WheelPositions >

Pose3d frc::PoseEstimator3d< WheelSpeeds, WheelPositions >::GetEstimatedPosition ( ) const

inline

Gets the estimated robot pose.

Returns: The estimated robot pose in meters.

◆ ResetPose()

template<typename WheelSpeeds , typename WheelPositions >

void frc::PoseEstimator3d< WheelSpeeds, WheelPositions >::ResetPose ( const Pose3d & pose )

inline

Resets the robot's pose.

Parameters

pose	The pose to reset to.

◆ ResetPosition()

template<typename WheelSpeeds , typename WheelPositions >

void frc::PoseEstimator3d< WheelSpeeds, WheelPositions >::ResetPosition	(	const Rotation3d &	gyroAngle,
		const WheelPositions &	wheelPositions,
		const Pose3d &	pose )

inline

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 current gyro angle.
wheelPositions	The distances traveled by the encoders.
pose	The estimated pose of the robot on the field.

◆ ResetRotation()

template<typename WheelSpeeds , typename WheelPositions >

void frc::PoseEstimator3d< WheelSpeeds, WheelPositions >::ResetRotation ( const Rotation3d & rotation )

inline

Resets the robot's rotation.

Parameters

rotation The rotation to reset to.

◆ ResetTranslation()

template<typename WheelSpeeds , typename WheelPositions >

void frc::PoseEstimator3d< WheelSpeeds, WheelPositions >::ResetTranslation ( const Translation3d & translation )

inline

Resets the robot's translation.

Parameters

translation The pose to translation to.

◆ SampleAt()

template<typename WheelSpeeds , typename WheelPositions >

std::optional< Pose3d > frc::PoseEstimator3d< WheelSpeeds, WheelPositions >::SampleAt ( units::second_t timestamp ) const

inline

Return the pose at a given timestamp, if the buffer is not empty.

Parameters

timestamp The pose's timestamp.

Returns: The pose at the given timestamp (or std::nullopt if the buffer is empty).

◆ SetVisionMeasurementStdDevs()

template<typename WheelSpeeds , typename WheelPositions >

void frc::PoseEstimator3d< WheelSpeeds, WheelPositions >::SetVisionMeasurementStdDevs ( const wpi::array< double, 4 > & visionMeasurementStdDevs )

inline

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

template<typename WheelSpeeds , typename WheelPositions >

Pose3d frc::PoseEstimator3d< WheelSpeeds, WheelPositions >::Update	(	const Rotation3d &	gyroAngle,
		const WheelPositions &	wheelPositions )

inline

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 traveled by the encoders.

Returns: The estimated pose of the robot in meters.

◆ UpdateWithTime()

template<typename WheelSpeeds , typename WheelPositions >

Pose3d frc::PoseEstimator3d< WheelSpeeds, WheelPositions >::UpdateWithTime	(	units::second_t	currentTime,
		const Rotation3d &	gyroAngle,
		const WheelPositions &	wheelPositions )

inline

Updates the pose estimator with wheel encoder and gyro information.

This should be called every loop.

Parameters

currentTime	The time at which this method was called.
gyroAngle	The current gyro angle.
wheelPositions	The distances traveled by the encoders.

Returns: The estimated pose of the robot in meters.

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

frc/estimator/PoseEstimator3d.h

Public Member Functions

Detailed Description

Constructor & Destructor Documentation

◆ PoseEstimator3d()

Member Function Documentation

◆ AddVisionMeasurement() [1/2]

◆ AddVisionMeasurement() [2/2]

◆ GetEstimatedPosition()

◆ ResetPose()

◆ ResetPosition()

◆ ResetRotation()

◆ ResetTranslation()

◆ SampleAt()

◆ SetVisionMeasurementStdDevs()

◆ Update()

◆ UpdateWithTime()