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] 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

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

@@ -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<double, 3, 3> 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<double, 3, 1>& u,
@@ -51,6 +48,14 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
m_observer.SetXhat(FillStateVector(initialPose, 0_m, 0_m));
}
void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
const std::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);
}
void DifferentialDrivePoseEstimator::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle) {
m_previousAngle = pose.Rotation();