Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-01 02:41:48 +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

30
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java
View File

@@ -256,6 +256,36 @@ public class DifferentialDrivePoseEstimator {
timestampSeconds);
}
/**
* Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
* estimate while still accounting for measurement noise.
*
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* DifferentialDrivePoseEstimator#update} every loop.
*
* <p>Note that the vision measurement standard deviations passed into this method will continue
* to apply to future measurements until a subsequent call to {@link
* DifferentialDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link
* DifferentialDrivePoseEstimator#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
* Timer.getFPGATimestamp.) This means that you should use 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.
*/
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,
double timestampSeconds,
Matrix<N3, N1> visionMeasurementStdDevs) {
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
}
/**
* Updates the the Unscented Kalman Filter using only wheel encoder information. Note that this
* should be called every loop.

29
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java
View File

@@ -220,6 +220,35 @@ public class MecanumDrivePoseEstimator {
timestampSeconds);
}
/**
* Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
* estimate while still accounting for measurement noise.
*
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* MecanumDrivePoseEstimator#update} every loop.
*
* <p>Note that the vision measurement standard deviations passed into this method will continue
* to apply to future measurements until a subsequent call to {@link
* MecanumDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link MecanumDrivePoseEstimator#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 Timer.getFPGATimestamp.) This means that you should use
* 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.
*/
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,
double timestampSeconds,
Matrix<N3, N1> visionMeasurementStdDevs) {
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
}
/**
* Updates the the Unscented Kalman Filter using only wheel encoder information. This should be
* called every loop, and the correct loop period must be passed into the constructor of this

29
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java
View File

@@ -220,6 +220,35 @@ public class SwerveDrivePoseEstimator {
timestampSeconds);
}
/**
* Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
* estimate while still accounting for measurement noise.
*
* <p>This method can be called as infrequently as you want, as long as you are calling {@link
* SwerveDrivePoseEstimator#update} every loop.
*
* <p>Note that the vision measurement standard deviations passed into this method will continue
* to apply to future measurements until a subsequent call to {@link
* SwerveDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link SwerveDrivePoseEstimator#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 Timer.getFPGATimestamp.) This means that you should use
* 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.
*/
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,
double timestampSeconds,
Matrix<N3, N1> visionMeasurementStdDevs) {
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
}
/**
* Updates the the Unscented Kalman Filter using only wheel encoder information. This should be
* called every loop, and the correct loop period must be passed into the constructor of this

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