diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java index 6a4bd6ea58..6bfb3e2199 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java @@ -4,7 +4,6 @@ package edu.wpi.first.math.estimator; -import edu.wpi.first.math.Discretization; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -59,7 +58,7 @@ public class DifferentialDrivePoseEstimator { private Rotation2d m_gyroOffset; private Rotation2d m_previousAngle; - private Matrix m_visionDiscreteR; + private Matrix m_visionContR; /** * Constructs a DifferentialDrivePoseEstimator. @@ -143,7 +142,7 @@ public class DifferentialDrivePoseEstimator { u, y, (x, u1) -> new Matrix<>(x.getStorage().extractMatrix(0, 3, 0, 1)), - m_visionDiscreteR, + m_visionContR, AngleStatistics.angleMean(2), AngleStatistics.angleResidual(2), AngleStatistics.angleResidual(2), @@ -164,8 +163,7 @@ public class DifferentialDrivePoseEstimator { * 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); + m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs); } @SuppressWarnings({"ParameterName", "MethodName"}) diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java index 0cf7367fb1..c460bd3a52 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java @@ -4,7 +4,6 @@ package edu.wpi.first.math.estimator; -import edu.wpi.first.math.Discretization; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.StateSpaceUtil; @@ -54,7 +53,7 @@ public class MecanumDrivePoseEstimator { private Rotation2d m_gyroOffset; private Rotation2d m_previousAngle; - private Matrix m_visionDiscreteR; + private Matrix m_visionContR; /** * Constructs a MecanumDrivePoseEstimator. @@ -144,7 +143,7 @@ public class MecanumDrivePoseEstimator { u, y, (x, u1) -> x, - m_visionDiscreteR, + m_visionContR, AngleStatistics.angleMean(2), AngleStatistics.angleResidual(2), AngleStatistics.angleResidual(2), @@ -165,8 +164,7 @@ public class MecanumDrivePoseEstimator { * 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); + m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java index 65077cba19..d1b457958d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java @@ -4,7 +4,6 @@ package edu.wpi.first.math.estimator; -import edu.wpi.first.math.Discretization; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.StateSpaceUtil; @@ -54,7 +53,7 @@ public class SwerveDrivePoseEstimator { private Rotation2d m_gyroOffset; private Rotation2d m_previousAngle; - private Matrix m_visionDiscreteR; + private Matrix m_visionContR; /** * Constructs a SwerveDrivePoseEstimator. @@ -144,7 +143,7 @@ public class SwerveDrivePoseEstimator { u, y, (x, u1) -> x, - m_visionDiscreteR, + m_visionContR, AngleStatistics.angleMean(2), AngleStatistics.angleResidual(2), AngleStatistics.angleResidual(2), @@ -165,8 +164,7 @@ public class SwerveDrivePoseEstimator { * 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); + m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java index b1bc4f19cc..ec163a0e8d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java @@ -334,7 +334,7 @@ public class UnscentedKalmanFilter void correct( @@ -364,7 +364,7 @@ public class UnscentedKalmanFilter& x, const Eigen::Matrix&) { return x.block<3, 1>(0, 0); }, - m_visionDiscR, frc::AngleMean<3, 5>(2), frc::AngleResidual<3>(2), + m_visionContR, frc::AngleMean<3, 5>(2), frc::AngleResidual<3>(2), frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)); }; @@ -48,9 +48,7 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs( const wpi::array& visionMeasurmentStdDevs) { // Create R (covariances) for vision measurements. - Eigen::Matrix visionContR = - frc::MakeCovMatrix(visionMeasurmentStdDevs); - m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt); + m_visionContR = frc::MakeCovMatrix(visionMeasurmentStdDevs); } void DifferentialDrivePoseEstimator::ResetPosition( diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index 5d6967e3ea..ef086bd73e 100644 --- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -38,7 +38,7 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( u, y, [](const Eigen::Matrix& x, const Eigen::Matrix&) { return x; }, - m_visionDiscR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2), + m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2), frc::AngleResidual<3>(2), frc::AngleAdd<3>(2)); }; @@ -53,9 +53,7 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs( const wpi::array& visionMeasurmentStdDevs) { // Create R (covariances) for vision measurements. - Eigen::Matrix visionContR = - frc::MakeCovMatrix(visionMeasurmentStdDevs); - m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt); + m_visionContR = frc::MakeCovMatrix(visionMeasurmentStdDevs); } void frc::MecanumDrivePoseEstimator::ResetPosition( diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h index bef76b973e..3a244c300e 100644 --- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h @@ -212,7 +212,7 @@ class DifferentialDrivePoseEstimator { const Eigen::Matrix& y)> m_visionCorrect; - Eigen::Matrix m_visionDiscR; + Eigen::Matrix m_visionContR; units::second_t m_nominalDt; units::second_t m_prevTime = -1_s; diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index 07568c27f6..0db20c916e 100644 --- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -208,7 +208,7 @@ class MecanumDrivePoseEstimator { const Eigen::Matrix& y)> m_visionCorrect; - Eigen::Matrix3d m_visionDiscR; + Eigen::Matrix3d m_visionContR; units::second_t m_nominalDt; units::second_t m_prevTime = -1_s; diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index e0f0ae2286..6914086faa 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -101,7 +101,7 @@ class SwerveDrivePoseEstimator { u, y, [](const Eigen::Matrix& x, const Eigen::Matrix& u) { return x; }, - m_visionDiscR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2), + m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2), frc::AngleResidual<3>(2), frc::AngleAdd<3>(2)); }; @@ -161,9 +161,7 @@ class SwerveDrivePoseEstimator { void SetVisionMeasurementStdDevs( const wpi::array& visionMeasurementStdDevs) { // Create R (covariances) for vision measurements. - Eigen::Matrix visionContR = - frc::MakeCovMatrix(visionMeasurementStdDevs); - m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt); + m_visionContR = frc::MakeCovMatrix(visionMeasurementStdDevs); } /** @@ -296,7 +294,7 @@ class SwerveDrivePoseEstimator { const Eigen::Matrix& y)> m_visionCorrect; - Eigen::Matrix3d m_visionDiscR; + Eigen::Matrix3d m_visionContR; units::second_t m_nominalDt; units::second_t m_prevTime = -1_s; diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h index 71eac27221..7ad9c24a2a 100644 --- a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h +++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h @@ -251,7 +251,7 @@ class UnscentedKalmanFilter { * @param y Measurement vector. * @param h A vector-valued function of x and u that returns the measurement * vector. - * @param R Measurement noise covariance matrix. + * @param R Measurement noise covariance matrix (continuous-time). */ template void Correct(const Eigen::Matrix& u, @@ -290,7 +290,7 @@ class UnscentedKalmanFilter { * @param y Measurement vector. * @param h A vector-valued function of x and u that returns the * measurement vector. - * @param R Measurement noise covariance matrix. + * @param R Measurement noise covariance matrix (continuous-time). * @param meanFuncY A function that computes the mean of 2 * States + 1 * measurement vectors using a given set of weights. * @param residualFuncY A function that computes the residual of two