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] Fix pose estimator performance (#4111)

Browse Source 
Fixes #4087.
This commit is contained in:
Tyler Veness
2022-04-26 18:43:59 -07:00
committed by GitHub
parent 51bc893bc5
commit d926dd1610
12 changed files with 159 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -56,7 +56,7 @@ void DifferentialDrivePoseEstimator::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle) {
// Reset state estimate and error covariance
m_observer.Reset();
m_latencyCompensator.Reset();
m_poseBuffer.Clear();
m_observer.SetXhat(FillStateVector(pose, 0_m, 0_m));
@@ -72,9 +72,11 @@ Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
void DifferentialDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
&m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
m_visionCorrect, timestamp);
if (auto sample = m_poseBuffer.Sample(timestamp)) {
m_visionCorrect(Eigen::Vector<double, 3>::Zero(),
PoseTo3dVector(GetEstimatedPosition().TransformBy(
visionRobotPose - sample.value())));
}
}
Pose2d DifferentialDrivePoseEstimator::Update(
@@ -103,7 +105,7 @@ Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
auto localY = Eigen::Vector<double, 3>{
leftDistance.value(), rightDistance.value(), angle.Radians().value()};
m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());
m_observer.Predict(u, dt);
m_observer.Correct(u, localY);