 |
WPILibC++ 2024.3.2
|
|
WPILibC++ 2024.3.2
|
This class wraps odometry to fuse latency-compensated vision measurements with encoder measurements. More...
#include <frc/estimator/PoseEstimator.h>
Public Member Functions | |
| PoseEstimator (Kinematics< WheelSpeeds, WheelPositions > &kinematics, Odometry< WheelSpeeds, 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... | |
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., DifferentialDrivePoseEstimator). It will correct for noisy vision measurements and encoder drift. It is intended to be an easy drop-in for Odometry.
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.
| WheelSpeeds | Wheel speeds type. |
| WheelPositions | Wheel positions type. |
| frc::PoseEstimator< WheelSpeeds, WheelPositions >::PoseEstimator | ( | Kinematics< WheelSpeeds, WheelPositions > & | kinematics, |
| Odometry< WheelSpeeds, WheelPositions > & | odometry, | ||
| const wpi::array< double, 3 > & | stateStdDevs, | ||
| const wpi::array< double, 3 > & | visionMeasurementStdDevs | ||
| ) |
Constructs a PoseEstimator.
| 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 heading in radians). Increase these numbers to trust the vision pose measurement less. |
| void frc::PoseEstimator< WheelSpeeds, WheelPositions >::AddVisionMeasurement | ( | const Pose2d & | visionRobotPose, |
| units::second_t | timestamp | ||
| ) |
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.
| 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. |
|
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.
| 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. |
| Pose2d frc::PoseEstimator< WheelSpeeds, WheelPositions >::GetEstimatedPosition |
Gets the estimated robot pose.
| void frc::PoseEstimator< WheelSpeeds, WheelPositions >::ResetPosition | ( | const Rotation2d & | gyroAngle, |
| const WheelPositions & | 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.
| gyroAngle | The current gyro angle. |
| wheelPositions | The distances traveled by the encoders. |
| pose | The estimated pose of the robot on the field. |
| void frc::PoseEstimator< WheelSpeeds, WheelPositions >::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.
| 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. |
| Pose2d frc::PoseEstimator< WheelSpeeds, WheelPositions >::Update | ( | const Rotation2d & | gyroAngle, |
| const WheelPositions & | wheelPositions | ||
| ) |
Updates the pose estimator with wheel encoder and gyro information.
This should be called every loop.
| gyroAngle | The current gyro angle. |
| wheelPositions | The distances traveled by the encoders. |
| Pose2d frc::PoseEstimator< WheelSpeeds, WheelPositions >::UpdateWithTime | ( | units::second_t | currentTime, |
| const Rotation2d & | gyroAngle, | ||
| const WheelPositions & | wheelPositions | ||
| ) |
Updates the pose estimator with wheel encoder and gyro information.
This should be called every loop.
| currentTime | The time at which this method was called. |
| gyroAngle | The current gyro angle. |
| wheelPositions | The distances traveled by the encoders. |
1.9.4