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

#include <frc/estimator/SwerveDrivePoseEstimator3d.h>

Inheritance diagram for frc::SwerveDrivePoseEstimator3d< NumModules >:

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

	SwerveDrivePoseEstimator3d (SwerveDriveKinematics< NumModules > &kinematics, const Rotation3d &gyroAngle, const wpi::array< SwerveModulePosition, NumModules > &modulePositions, const Pose3d &initialPose, const wpi::array< double, 4 > &stateStdDevs, const wpi::array< double, 4 > &visionMeasurementStdDevs)
	Constructs a SwerveDrivePoseEstimator3d.

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

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

Detailed Description

template<size_t NumModules>
class frc::SwerveDrivePoseEstimator3d< 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 SwerveDriveOdometry3d. 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 as regular encoder odometry.

Constructor & Destructor Documentation

◆ SwerveDrivePoseEstimator3d() [1/2]

template<size_t NumModules>

frc::SwerveDrivePoseEstimator3d< NumModules >::SwerveDrivePoseEstimator3d	(	SwerveDriveKinematics< NumModules > &	kinematics,
		const Rotation3d &	gyroAngle,
		const wpi::array< SwerveModulePosition, NumModules > &	modulePositions,
		const Pose3d &	initialPose )

inline

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

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.

◆ SwerveDrivePoseEstimator3d() [2/2]