Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +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

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

@@ -68,18 +68,25 @@ public class DifferentialDrivePoseEstimator {
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
private Matrix<N3, N3> m_visionDiscreteR;
/**
* Constructs a DifferentialDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to
* trust your wheel and gyro velocities less.
* 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 encoder distances and gyro
* angle less.
* 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 numbers to trust vision less.
* 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 DifferentialDrivePoseEstimator(
Rotation2d gyroAngle, Pose2d initialPoseMeters,
@@ -96,12 +103,17 @@ public class DifferentialDrivePoseEstimator {
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to
* trust your wheel and gyro velocities less.
* 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 encoder distances and gyro
* angle less.
* 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 numbers to trust vision less.
* 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 nominalDtSeconds The time in seconds between each robot loop.
*/
@SuppressWarnings("ParameterName")
@@ -127,12 +139,13 @@ public class DifferentialDrivePoseEstimator {
);
m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
var visionDiscR = Discretization.discretizeR(visionContR, m_nominalDt);
// Initialize vision R
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
m_visionCorrect = (u, y) -> m_observer.correct(
Nat.N3(), u, y,
(x, u_) -> new Matrix<>(x.getStorage().extractMatrix(0, 3, 0, 1)),
visionDiscR,
m_visionDiscreteR,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
@@ -144,6 +157,21 @@ public class DifferentialDrivePoseEstimator {
m_observer.setXhat(fillStateVector(initialPoseMeters, 0.0, 0.0));
}
/**
* 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.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
m_visionDiscreteR = Discretization.discretizeR(visionContR, m_nominalDt);
}
@SuppressWarnings({"ParameterName", "MethodName"})
private Matrix<N5, N1> f(Matrix<N5, N1> x, Matrix<N3, N1> u) {
// Apply a rotation matrix. Note that we do *not* add x--Runge-Kutta does that for us.

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

@@ -60,6 +60,8 @@ public class MecanumDrivePoseEstimator {
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
private Matrix<N3, N3> m_visionDiscreteR;
/**
* Constructs a MecanumDrivePoseEstimator.
*
@@ -67,11 +69,17 @@ public class MecanumDrivePoseEstimator {
* @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object 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.
* trust your 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 vision less.
* 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 MecanumDrivePoseEstimator(
Rotation2d gyroAngle, Pose2d initialPoseMeters, MecanumDriveKinematics kinematics,
@@ -89,11 +97,17 @@ public class MecanumDrivePoseEstimator {
* @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object 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.
* trust your 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 vision less.
* 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 nominalDtSeconds The time in seconds between each robot loop.
*/
@SuppressWarnings("ParameterName")
@@ -120,13 +134,13 @@ public class MecanumDrivePoseEstimator {
m_kinematics = kinematics;
m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
var visionDiscR = Discretization.discretizeR(visionContR, m_nominalDt);
// Initialize vision R
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
m_visionCorrect = (u, y) -> m_observer.correct(
Nat.N3(), u, y,
(x, u_) -> x,
visionDiscR,
m_visionDiscreteR,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
@@ -138,6 +152,21 @@ public class MecanumDrivePoseEstimator {
m_observer.setXhat(StateSpaceUtil.poseTo3dVector(initialPoseMeters));
}
/**
* 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.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
m_visionDiscreteR = Discretization.discretizeR(visionContR, m_nominalDt);
}
/**
* Resets the robot's position on the field.
*

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

@@ -60,6 +60,8 @@ public class SwerveDrivePoseEstimator {
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
private Matrix<N3, N3> m_visionDiscreteR;
/**
* Constructs a SwerveDrivePoseEstimator.
*
@@ -67,11 +69,17 @@ public class SwerveDrivePoseEstimator {
* @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object 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.
* trust your 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 vision less.
* 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 SwerveDrivePoseEstimator(
Rotation2d gyroAngle, Pose2d initialPoseMeters, SwerveDriveKinematics kinematics,
@@ -89,11 +97,17 @@ public class SwerveDrivePoseEstimator {
* @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object 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.
* trust your 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 vision less.
* 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 nominalDtSeconds The time in seconds between each robot loop.
*/
@SuppressWarnings("ParameterName")
@@ -120,13 +134,13 @@ public class SwerveDrivePoseEstimator {
m_kinematics = kinematics;
m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
var visionDiscR = Discretization.discretizeR(visionContR, m_nominalDt);
// Initialize vision R
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
m_visionCorrect = (u, y) -> m_observer.correct(
Nat.N3(), u, y,
(x, u_) -> x,
visionDiscR,
m_visionDiscreteR,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(2),
@@ -138,6 +152,21 @@ public class SwerveDrivePoseEstimator {
m_observer.setXhat(StateSpaceUtil.poseTo3dVector(initialPoseMeters));
}
/**
* 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.
*/
public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
m_visionDiscreteR = Discretization.discretizeR(visionContR, m_nominalDt);
}
/**
* Resets the robot's position on the field.
*