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] Reset error covariance in pose estimator ResetPosition() (#3418)

Browse Source 
This also fixes a member function name inconsistency between languages
and adds missing documentation to C++'s KalmanFilterLatencyCompensator.

Fixes #3229.
This commit is contained in:
Tyler Veness
2021-06-08 21:20:43 -07:00
committed by GitHub
parent 0773f4033e
commit f9c3d54bd1
8 changed files with 76 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -55,9 +55,14 @@ void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
void DifferentialDrivePoseEstimator::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle) {
m_previousAngle = pose.Rotation();
// Reset state estimate and error covariance
m_observer.Reset();
m_latencyCompensator.Reset();
m_observer.SetXhat(FillStateVector(pose, 0_m, 0_m));
m_gyroOffset = GetEstimatedPosition().Rotation() - gyroAngle;
m_previousAngle = pose.Rotation();
}
Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
@@ -68,9 +73,9 @@ Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
void DifferentialDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
m_latencyCompensator.ApplyPastMeasurement<3>(&m_observer, m_nominalDt,
PoseTo3dVector(visionRobotPose),
m_visionCorrect, timestamp);
m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
&m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
m_visionCorrect, timestamp);
}
Pose2d DifferentialDrivePoseEstimator::Update(

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

@@ -60,10 +60,12 @@ void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs(
void frc::MecanumDrivePoseEstimator::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle) {
// Set observer state.
// Reset state estimate and error covariance
m_observer.Reset();
m_latencyCompensator.Reset();
m_observer.SetXhat(PoseTo3dVector(pose));
// Calculate offsets.
m_gyroOffset = pose.Rotation() - gyroAngle;
m_previousAngle = pose.Rotation();
}
@@ -75,9 +77,9 @@ Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
m_latencyCompensator.ApplyPastMeasurement<3>(&m_observer, m_nominalDt,
PoseTo3dVector(visionRobotPose),
m_visionCorrect, timestamp);
m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
&m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
m_visionCorrect, timestamp);
}
Pose2d frc::MecanumDrivePoseEstimator::Update(