Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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

6
wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
View File

@@ -36,7 +36,7 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
u, y,
[](const Eigen::Matrix<double, 5, 1>& x,
const Eigen::Matrix<double, 3, 1>&) { 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<double, 3>& visionMeasurmentStdDevs) {
// Create R (covariances) for vision measurements.
Eigen::Matrix<double, 3, 3> visionContR =
frc::MakeCovMatrix(visionMeasurmentStdDevs);
m_visionDiscR = frc::DiscretizeR<3>(visionContR, m_nominalDt);
m_visionContR = frc::MakeCovMatrix(visionMeasurmentStdDevs);
}
void DifferentialDrivePoseEstimator::ResetPosition(