This class wraps Differential Drive Odometry to fuse latency-compensated vision measurements with differential drive encoder measurements. More...

#include <frc/estimator/DifferentialDrivePoseEstimator3d.h>

Inheritance diagram for frc::DifferentialDrivePoseEstimator3d:

Public Member Functions
	DifferentialDrivePoseEstimator3d (DifferentialDriveKinematics &kinematics, const Rotation3d &gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, const Pose3d &initialPose)
	Constructs a DifferentialDrivePoseEstimator3d with default standard deviations for the model and vision measurements.

	DifferentialDrivePoseEstimator3d (DifferentialDriveKinematics &kinematics, const Rotation3d &gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, const Pose3d &initialPose, const wpi::array< double, 4 > &stateStdDevs, const wpi::array< double, 4 > &visionMeasurementStdDevs)
	Constructs a DifferentialDrivePoseEstimator3d.

void	ResetPosition (const Rotation3d &gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, const Pose3d &pose)
	Resets the robot's position on the field.

Pose3d	Update (const Rotation3d &gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance)
	Updates the pose estimator with wheel encoder and gyro information.

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

Public Member Functions inherited from frc::PoseEstimator3d< DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions >
	PoseEstimator3d (Kinematics< DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions > &kinematics, Odometry3d< DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions > &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 DifferentialDriveWheelPositions &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 DifferentialDriveWheelPositions &wheelPositions)
	Updates the pose estimator with wheel encoder and gyro information.

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

Detailed Description

This class wraps Differential Drive Odometry to fuse latency-compensated vision measurements with differential drive encoder measurements.

It will correct for noisy vision measurements and encoder drift. It is intended to be an easy drop-in for DifferentialDriveOdometry3d. In fact, if you never call AddVisionMeasurement(), and only call Update(), this will behave exactly the same as DifferentialDriveOdometry3d. 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 (if your robot loops are faster or slower than the default of 20 ms, then you should change the nominal delta time via the constructor).

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

Constructor & Destructor Documentation

◆ DifferentialDrivePoseEstimator3d() [1/2]

frc::DifferentialDrivePoseEstimator3d::DifferentialDrivePoseEstimator3d	(	DifferentialDriveKinematics &	kinematics,
		const Rotation3d &	gyroAngle,
		units::meter_t	leftDistance,
		units::meter_t	rightDistance,
		const Pose3d &	initialPose )

Constructs a DifferentialDrivePoseEstimator3d 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, 0.02 meters for z, and 0.01 radians for angle. The default standard deviations of the vision measurements are 0.1 meters for x, 0.1 meters for y, 0.1 meters for z, and 0.1 radians for angle.

Parameters

kinematics	A correctly-configured kinematics object for your drivetrain.
gyroAngle	The gyro angle of the robot.
leftDistance	The distance traveled by the left encoder.
rightDistance	The distance traveled by the right encoder.
initialPose	The estimated initial pose.

◆ DifferentialDrivePoseEstimator3d() [2/2]

frc::DifferentialDrivePoseEstimator3d::DifferentialDrivePoseEstimator3d	(	DifferentialDriveKinematics &	kinematics,
		const Rotation3d &	gyroAngle,
		units::meter_t	leftDistance,
		units::meter_t	rightDistance,
		const Pose3d &	initialPose,
		const wpi::array< double, 4 > &	stateStdDevs,
		const wpi::array< double, 4 > &	visionMeasurementStdDevs )

Constructs a DifferentialDrivePoseEstimator3d.

Parameters

kinematics	A correctly-configured kinematics object for your drivetrain.
gyroAngle	The gyro angle of the robot.
leftDistance	The distance traveled by the left encoder.
rightDistance	The distance traveled by the right encoder.
initialPose	The estimated initial pose.
stateStdDevs	Standard deviations of the pose estimate (x position in meters, y position in meters, z position in meters, and angle 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, z position in meters, and angle in radians). Increase these numbers to trust the vision pose measurement less.

Member Function Documentation

◆ ResetPosition()

void frc::DifferentialDrivePoseEstimator3d::ResetPosition	(	const Rotation3d &	gyroAngle,
		units::meter_t	leftDistance,
		units::meter_t	rightDistance,
		const Pose3d &	pose )

inline

Resets the robot's position on the field.

Parameters

gyroAngle	The current gyro angle.
leftDistance	The distance traveled by the left encoder.
rightDistance	The distance traveled by the right encoder.
pose	The estimated pose of the robot on the field.

◆ Update()

Pose3d frc::DifferentialDrivePoseEstimator3d::Update	(	const Rotation3d &	gyroAngle,
		units::meter_t	leftDistance,
		units::meter_t	rightDistance )

inline

Updates the pose estimator with wheel encoder and gyro information.

This should be called every loop.

Parameters

gyroAngle	The current gyro angle.
leftDistance	The distance traveled by the left encoder.
rightDistance	The distance traveled by the right encoder.

Returns: The estimated pose of the robot.

◆ UpdateWithTime()

Pose3d frc::DifferentialDrivePoseEstimator3d::UpdateWithTime	(	units::second_t	currentTime,
		const Rotation3d &	gyroAngle,
		units::meter_t	leftDistance,
		units::meter_t	rightDistance )

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.
leftDistance	The distance traveled by the left encoder.
rightDistance	The distance traveled by the right encoder.

Returns: The estimated pose of the robot.

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

frc/estimator/DifferentialDrivePoseEstimator3d.h

Public Member Functions

Detailed Description

Constructor & Destructor Documentation

◆ DifferentialDrivePoseEstimator3d() [1/2]

◆ DifferentialDrivePoseEstimator3d() [2/2]

Member Function Documentation

◆ ResetPosition()

◆ Update()

◆ UpdateWithTime()