Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +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

26
wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
View File

@@ -34,6 +34,19 @@ class KalmanFilterLatencyCompensator {
localMeasurements(localY) {}
};
/**
* Clears the observer snapshot buffer.
*/
void Reset() { m_pastObserverSnapshots.clear(); }
/**
* Add past observer states to the observer snapshots list.
*
* @param observer The observer.
* @param u The input at the timestamp.
* @param localY The local output at the timestamp
* @param timestamp The timesnap of the state.
*/
void AddObserverState(const KalmanFilterType& observer,
Eigen::Matrix<double, Inputs, 1> u,
Eigen::Matrix<double, Outputs, 1> localY,
@@ -48,8 +61,19 @@ class KalmanFilterLatencyCompensator {
}
}
/**
* Add past global measurements (such as from vision)to the estimator.
*
* @param observer The observer to apply the past global
* measurement.
* @param nominalDt The nominal timestep.
* @param y The measurement.
* @param globalMeasurementCorrect The function take calls correct() on the
* observer.
* @param timestamp The timestamp of the measurement.
*/
template <int Rows>
void ApplyPastMeasurement(
void ApplyPastGlobalMeasurement(
KalmanFilterType* observer, units::second_t nominalDt,
Eigen::Matrix<double, Rows, 1> y,
std::function<void(const Eigen::Matrix<double, Inputs, 1>& u,

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

@@ -125,10 +125,12 @@ class SwerveDrivePoseEstimator {
* @param gyroAngle The angle reported by the gyroscope.
*/
void 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();
}
@@ -184,7 +186,7 @@ class SwerveDrivePoseEstimator {
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp) {
m_latencyCompensator.ApplyPastMeasurement<3>(
m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
&m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
m_visionCorrect, timestamp);
}