Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add pose estimator overload for vision + std dev measurement (#3200)

Browse Source
This commit is contained in:
Prateek Machiraju
2021-03-04 02:37:18 -05:00
committed by GitHub
parent 1a2680b9e5
commit f3f86b8e78
6 changed files with 202 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
View File

@@ -135,6 +135,44 @@ class DifferentialDrivePoseEstimator {
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp);
/**
* Adds a vision measurement to the Unscented 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.
*
* 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.
*
* @param visionRobotPose The pose of the robot as measured by the
* vision camera.
* @param 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
* frc2::Timer::GetFPGATimestamp(). This means
* that you should use
* frc2::Timer::GetFPGATimestamp() as your
* time source in this case.
* @param visionMeasurementStdDevs Standard deviations of the vision
* measurements. Increase these numbers to
* trust global measurements from vision
* less. This matrix is in the form
* [x, y, theta]^T, with units in meters and
* radians.
*/
void AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp,
const wpi::array<double, 3>& visionMeasurementStdDevs) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
AddVisionMeasurement(visionRobotPose, timestamp);
}
/**
* Updates the Unscented Kalman Filter using only wheel encoder information.
* Note that this should be called every loop iteration.

38
wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
View File

@@ -135,6 +135,44 @@ class MecanumDrivePoseEstimator {
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp);
/**
* Adds a vision measurement to the Unscented 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.
*
* 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.
*
* @param visionRobotPose The pose of the robot as measured by the
* vision camera.
* @param 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
* frc2::Timer::GetFPGATimestamp(). This means
* that you should use
* frc2::Timer::GetFPGATimestamp() as your
* time source in this case.
* @param visionMeasurementStdDevs Standard deviations of the vision
* measurements. Increase these numbers to
* trust global measurements from vision
* less. This matrix is in the form
* [x, y, theta]^T, with units in meters and
* radians.
*/
void AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp,
const wpi::array<double, 3>& visionMeasurementStdDevs) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
AddVisionMeasurement(visionRobotPose, timestamp);
}
/**
* Updates the the Unscented Kalman Filter using only wheel encoder
* information. This should be called every loop, and the correct loop period

38
wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
View File

@@ -189,6 +189,44 @@ class SwerveDrivePoseEstimator {
m_visionCorrect, timestamp);
}
/**
* Adds a vision measurement to the Unscented 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.
*
* 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.
*
* @param visionRobotPose The pose of the robot as measured by the
* vision camera.
* @param 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
* frc2::Timer::GetFPGATimestamp(). This means
* that you should use
* frc2::Timer::GetFPGATimestamp() as your
* time source in this case.
* @param visionMeasurementStdDevs Standard deviations of the vision
* measurements. Increase these numbers to
* trust global measurements from vision
* less. This matrix is in the form
* [x, y, theta]^T, with units in meters and
* radians.
*/
void AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp,
const wpi::array<double, 3>& visionMeasurementStdDevs) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
AddVisionMeasurement(visionRobotPose, timestamp);
}
/**
* Updates the the Unscented Kalman Filter using only wheel encoder
* information. This should be called every loop, and the correct loop period