diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java index 7b926be18f..6371dda2a1 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java @@ -68,18 +68,25 @@ public class DifferentialDrivePoseEstimator { private Rotation2d m_gyroOffset; private Rotation2d m_previousAngle; + private Matrix 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 visionMeasurementStdDevs) { + var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs); + m_visionDiscreteR = Discretization.discretizeR(visionContR, m_nominalDt); + } + @SuppressWarnings({"ParameterName", "MethodName"}) private Matrix f(Matrix x, Matrix u) { // Apply a rotation matrix. Note that we do *not* add x--Runge-Kutta does that for us. diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java index 322b2333b8..acfab10cc1 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java @@ -60,6 +60,8 @@ public class MecanumDrivePoseEstimator { private Rotation2d m_gyroOffset; private Rotation2d m_previousAngle; + private Matrix 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 visionMeasurementStdDevs) { + var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs); + m_visionDiscreteR = Discretization.discretizeR(visionContR, m_nominalDt); + } + /** * Resets the robot's position on the field. * diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java index 591337a1f1..491c2d0c3e 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java @@ -60,6 +60,8 @@ public class SwerveDrivePoseEstimator { private Rotation2d m_gyroOffset; private Rotation2d m_previousAngle; + private Matrix 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 visionMeasurementStdDevs) { + var visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs); + m_visionDiscreteR = Discretization.discretizeR(visionContR, m_nominalDt); + } + /** * Resets the robot's position on the field. * diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index a9e96bf698..8fac8b9b62 100644 --- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -30,10 +30,7 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( frc::AngleMean<3, 5>(2), frc::AngleResidual<5>(2), frc::AngleResidual<3>(2), frc::AngleAdd<5>(2), nominalDt), m_nominalDt(nominalDt) { - // Create R (covariances) for vision measurements. - Eigen::Matrix visionContR = - frc::MakeCovMatrix(visionMeasurmentStdDevs); - m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt); + SetVisionMeasurementStdDevs(visionMeasurmentStdDevs); // Create correction mechanism for vision measurements. m_visionCorrect = [&](const Eigen::Matrix& u, @@ -51,6 +48,14 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( m_observer.SetXhat(FillStateVector(initialPose, 0_m, 0_m)); } +void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs( + const std::array& visionMeasurmentStdDevs) { + // Create R (covariances) for vision measurements. + Eigen::Matrix visionContR = + frc::MakeCovMatrix(visionMeasurmentStdDevs); + m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt); +} + void DifferentialDrivePoseEstimator::ResetPosition( const Pose2d& pose, const Rotation2d& gyroAngle) { m_previousAngle = pose.Rotation(); diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index 1b3be996c1..6e0ba3a4a1 100644 --- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -32,11 +32,7 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( frc::AngleResidual<1>(0), 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 vision correction mechanism. m_visionCorrect = [&](const Eigen::Matrix& u, @@ -57,6 +53,14 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( m_previousAngle = initialPose.Rotation(); } +void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs( + const std::array& visionMeasurmentStdDevs) { + // Create R (covariances) for vision measurements. + Eigen::Matrix visionContR = + frc::MakeCovMatrix(visionMeasurmentStdDevs); + m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt); +} + void frc::MecanumDrivePoseEstimator::ResetPosition( const Pose2d& pose, const Rotation2d& gyroAngle) { // Set observer state. diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h index 9b58111338..f0ba454ea1 100644 --- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h @@ -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& 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& visionMeasurementStdDevs); + /** * Resets the robot's position on the field. * diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index 483bac6bef..08ebe4431c 100644 --- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -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& 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& visionMeasurementStdDevs); + /** * Resets the robot's position on the field. * diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index c8589addee..88688c7da5 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -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& 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& visionMeasurementStdDevs) { + // Create R (covariances) for vision measurements. + Eigen::Matrix 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.