This class wraps Swerve Drive Odometry to fuse latency-compensated vision measurements with swerve drive encoder distance measurements. More...

#include <frc/estimator/SwerveDrivePoseEstimator.h>

Inheritance diagram for frc::SwerveDrivePoseEstimator< NumModules >:

Public Member Functions
	SwerveDrivePoseEstimator (SwerveDriveKinematics< NumModules > &kinematics, const Rotation2d &gyroAngle, const wpi::array< SwerveModulePosition, NumModules > &modulePositions, const Pose2d &initialPose)
	Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision measurements. More...

	SwerveDrivePoseEstimator (SwerveDriveKinematics< NumModules > &kinematics, const Rotation2d &gyroAngle, const wpi::array< SwerveModulePosition, NumModules > &modulePositions, const Pose2d &initialPose, const wpi::array< double, 3 > &stateStdDevs, const wpi::array< double, 3 > &visionMeasurementStdDevs)
	Constructs a SwerveDrivePoseEstimator. More...

void	ResetPosition (const Rotation2d &gyroAngle, const wpi::array< SwerveModulePosition, NumModules > &modulePositions, const Pose2d &pose)
	Resets the robot's position on the field. More...

Pose2d	Update (const Rotation2d &gyroAngle, const wpi::array< SwerveModulePosition, NumModules > &modulePositions)
	Updates the pose estimator with wheel encoder and gyro information. More...

Pose2d	UpdateWithTime (units::second_t currentTime, const Rotation2d &gyroAngle, const wpi::array< SwerveModulePosition, NumModules > &modulePositions)
	Updates the pose estimator with wheel encoder and gyro information. More...

Public Member Functions inherited from frc::PoseEstimator< SwerveDriveWheelSpeeds< NumModules >, SwerveDriveWheelPositions< NumModules > >
	PoseEstimator (Kinematics< SwerveDriveWheelSpeeds< NumModules >, WheelPositions > &kinematics, Odometry< SwerveDriveWheelSpeeds< NumModules >, WheelPositions > &odometry, const wpi::array< double, 3 > &stateStdDevs, const wpi::array< double, 3 > &visionMeasurementStdDevs)
	Constructs a PoseEstimator. 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 WheelPositions &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)
	Adds 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 WheelPositions &wheelPositions)
	Updates the pose estimator with wheel encoder and gyro information. More...

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

Detailed Description

template<size_t NumModules>
class frc::SwerveDrivePoseEstimator< NumModules >

This class wraps Swerve Drive Odometry to fuse latency-compensated vision measurements with swerve drive encoder distance measurements.

It is intended to be a drop-in for SwerveDriveOdometry.

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 as regular encoder odometry.

Constructor & Destructor Documentation

◆ SwerveDrivePoseEstimator() [1/2]

template<size_t NumModules>

frc::SwerveDrivePoseEstimator< NumModules >::SwerveDrivePoseEstimator	(	SwerveDriveKinematics< NumModules > &	kinematics,
		const Rotation2d &	gyroAngle,
		const wpi::array< SwerveModulePosition, NumModules > &	modulePositions,
		const Pose2d &	initialPose
	)

inline

Constructs a SwerveDrivePoseEstimator 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.9 meters for x, 0.9 meters for y, and 0.9 radians for heading.

Parameters

kinematics	A correctly-configured kinematics object for your drivetrain.
gyroAngle	The current gyro angle.
modulePositions	The current distance and rotation measurements of the swerve modules.
initialPose	The starting pose estimate.

◆ SwerveDrivePoseEstimator() [2/2]

template<size_t NumModules>

frc::SwerveDrivePoseEstimator< NumModules >::SwerveDrivePoseEstimator	(	SwerveDriveKinematics< NumModules > &	kinematics,
		const Rotation2d &	gyroAngle,
		const wpi::array< SwerveModulePosition, NumModules > &	modulePositions,
		const Pose2d &	initialPose,
		const wpi::array< double, 3 > &	stateStdDevs,
		const wpi::array< double, 3 > &	visionMeasurementStdDevs
	)

inline

Constructs a SwerveDrivePoseEstimator.

Parameters

kinematics	A correctly-configured kinematics object for your drivetrain.
gyroAngle	The current gyro angle.
modulePositions	The current distance and rotation measurements of the swerve modules.
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

◆ ResetPosition()

template<size_t NumModules>

void frc::SwerveDrivePoseEstimator< NumModules >::ResetPosition	(	const Rotation2d &	gyroAngle,
		const wpi::array< SwerveModulePosition, NumModules > &	modulePositions,
		const Pose2d &	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 angle reported by the gyroscope.
modulePositions	The current distance and rotation measurements of the swerve modules.
pose	The position on the field that your robot is at.

◆ Update()

template<size_t NumModules>

Pose2d frc::SwerveDrivePoseEstimator< NumModules >::Update	(	const Rotation2d &	gyroAngle,
		const wpi::array< SwerveModulePosition, NumModules > &	modulePositions
	)

inline

Updates the pose estimator with wheel encoder and gyro information.

This should be called every loop.

Parameters

gyroAngle	The current gyro angle.
modulePositions	The current distance and rotation measurements of the swerve modules.

Returns: The estimated robot pose in meters.

◆ UpdateWithTime()

template<size_t NumModules>

Pose2d frc::SwerveDrivePoseEstimator< NumModules >::UpdateWithTime	(	units::second_t	currentTime,
		const Rotation2d &	gyroAngle,
		const wpi::array< SwerveModulePosition, NumModules > &	modulePositions
	)

inline

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 gyro angle.
modulePositions	The current distance traveled and rotations of the swerve modules.

Returns: The estimated robot pose in meters.

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

frc/estimator/SwerveDrivePoseEstimator.h

Public Member Functions

Detailed Description

Constructor & Destructor Documentation

◆ SwerveDrivePoseEstimator() [1/2]

◆ SwerveDrivePoseEstimator() [2/2]

Member Function Documentation

◆ ResetPosition()

◆ Update()

◆ UpdateWithTime()