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

[wpimath] Remove incorrect discretization in pose estimators (#3460)

Browse Source
This commit is contained in:
Prateek Machiraju
2021-06-24 00:57:52 -04:00
committed by GitHub
parent ea0b8f48e6
commit 646ded9123
10 changed files with 22 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -212,7 +212,7 @@ class DifferentialDrivePoseEstimator {
const Eigen::Matrix<double, 3, 1>& y)>
m_visionCorrect;
Eigen::Matrix<double, 3, 3> m_visionDiscR;
Eigen::Matrix<double, 3, 3> m_visionContR;
units::second_t m_nominalDt;
units::second_t m_prevTime = -1_s;

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

@@ -208,7 +208,7 @@ class MecanumDrivePoseEstimator {
const Eigen::Matrix<double, 3, 1>& y)>
m_visionCorrect;
Eigen::Matrix3d m_visionDiscR;
Eigen::Matrix3d m_visionContR;
units::second_t m_nominalDt;
units::second_t m_prevTime = -1_s;

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

@@ -101,7 +101,7 @@ class SwerveDrivePoseEstimator {
u, y,
[](const Eigen::Matrix<double, 3, 1>& x,
const Eigen::Matrix<double, 3, 1>& 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<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);
m_visionContR = frc::MakeCovMatrix(visionMeasurementStdDevs);
}
/**
@@ -296,7 +294,7 @@ class SwerveDrivePoseEstimator {
const Eigen::Matrix<double, 3, 1>& y)>
m_visionCorrect;
Eigen::Matrix3d m_visionDiscR;
Eigen::Matrix3d m_visionContR;
units::second_t m_nominalDt;
units::second_t m_prevTime = -1_s;

4
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
View File

@@ -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 <int Rows>
void Correct(const Eigen::Matrix<double, Inputs, 1>& 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