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

[wpimath] Add support for varying vision standard deviations in pose estimators (#2956)

Browse Source 
Exposes the R passed to vision correct to users.
This commit is contained in:
Matt
2020-12-24 16:05:07 -08:00
committed by GitHub
parent df299d6edd
commit 6b567e0066
8 changed files with 233 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -54,15 +54,25 @@ class DifferentialDrivePoseEstimator {
*
* @param gyroAngle The gyro angle of the robot.
* @param initialPose The estimated initial pose.
* @param stateStdDevs Standard deviations of the model states.
* Increase these to trust your wheel speeds
* less.
* @param localMeasurementStdDevs Standard deviations of the encoder
* measurements. Increase these to trust
* encoder distances less.
* @param stateStdDevs Standard deviations of model states.
* Increase these numbers to trust your
* model's state estimates less. This matrix
* is in the form
* [x, y, theta, dist_l, dist_r]^T,
* with units in meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro
* measurements. Increase these numbers to
* trust sensor readings from
* encoders and gyros less.
* This matrix is in the form
* [dist_l, dist_r, theta]^T, with units in
* meters and radians.
* @param visionMeasurementStdDevs Standard deviations of the vision
* measurements. Increase these to trust
* vision less.
* 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.
* @param nominalDt The period of the loop calling Update().
*/
DifferentialDrivePoseEstimator(
@@ -72,6 +82,21 @@ class DifferentialDrivePoseEstimator {
const std::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt = 0.02_s);
/**
* Sets the pose estimator's trust of global 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.
*
* @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 SetVisionMeasurementStdDevs(
const std::array<double, 3>& visionMeasurementStdDevs);
/**
* Resets the robot's position on the field.
*

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

@@ -54,13 +54,20 @@ class MecanumDrivePoseEstimator {
* for your drivetrain.
* @param stateStdDevs Standard deviations of model states.
* Increase these numbers to trust your
* wheel and gyro velocities less.
* @param localMeasurementStdDevs Standard deviations of the gyro
* measurement. Increase this number
* to trust gyro angle measurements less.
* @param visionMeasurementStdDevs Standard deviations of the encoder
* measurements. Increase these numbers
* to trust vision less.
* model's state estimates less. This matrix
* is in the form [x, y, theta]^T, with units
* in meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro
* measurements. Increase these numbers to
* trust sensor readings from encoders
* and gyros less. This matrix is in the form
* [theta], with units in radians.
* @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.
* @param nominalDt The time in seconds between each robot
* loop.
*/
@@ -72,6 +79,21 @@ class MecanumDrivePoseEstimator {
const std::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt = 0.02_s);
/**
* Sets the pose estimator's trust of global 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.
*
* @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 SetVisionMeasurementStdDevs(
const std::array<double, 3>& visionMeasurementStdDevs);
/**
* Resets the robot's position on the field.
*

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

@@ -60,13 +60,20 @@ class SwerveDrivePoseEstimator {
* for your drivetrain.
* @param stateStdDevs Standard deviations of model states.
* Increase these numbers to trust your
* wheel and gyro velocities less.
* @param localMeasurementStdDevs Standard deviations of the gyro
* measurement. Increase this number to
* trust gyro angle measurements less.
* @param visionMeasurementStdDevs Standard deviations of the encoder
* model's state estimates less. This matrix
* is in the form [x, y, theta]^T, with units
* in meters and radians.
* @param localMeasurementStdDevs Standard deviations of the encoder and gyro
* measurements. Increase these numbers to
* trust vision less.
* trust sensor readings from encoders
* and gyros less. This matrix is in the form
* [theta], with units in radians.
* @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.
* @param nominalDt The time in seconds between each robot
* loop.
*/
@@ -89,12 +96,7 @@ class SwerveDrivePoseEstimator {
frc::AngleAdd<3>(2), nominalDt),
m_kinematics(kinematics),
m_nominalDt(nominalDt) {
// Construct R (covariances) matrix for vision measurements.
Eigen::Matrix3d visionContR =
frc::MakeCovMatrix<3>(visionMeasurementStdDevs);
// Create and store discrete covariance matrix for vision measurements.
m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt);
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
// Create correction mechanism for vision measurements.
m_visionCorrect = [&](const Eigen::Matrix<double, 3, 1>& u,
@@ -146,6 +148,26 @@ class SwerveDrivePoseEstimator {
Rotation2d(units::radian_t{m_observer.Xhat(2)}));
}
/**
* Sets the pose estimator's trust of global 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.
*
* @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 SetVisionMeasurementStdDevs(
const std::array<double, 3>& visionMeasurementStdDevs) {
// Create R (covariances) for vision measurements.
Eigen::Matrix<double, 3, 3> visionContR =
frc::MakeCovMatrix(visionMeasurementStdDevs);
m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt);
}
/**
* Add a vision measurement to the Unscented Kalman Filter. This will correct
* the odometry pose estimate while still accounting for measurement noise.