Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Use Odometry for internal state in Pose Estimation (#4668)

Browse Source 
This effectively replaces the Unscented Kalman Filter used for Pose Estimation with the Odometry model, and uses a recalculable Kalman gain matrix to update pose estimations whenever a vision measurement is added.

Notably, this change removes the need for the confusing generics used in Java, and the C++ implementation got quite a bit less complex as well.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Jordan McMichael
2022-12-02 11:36:10 -05:00
committed by GitHub
parent 68dba92630
commit e22d8cc343
35 changed files with 2288 additions and 1884 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
View File

@@ -120,6 +120,14 @@ class TimeInterpolatableBuffer {
return m_interpolatingFunc(lower_bound->second, upper_bound->second, t);
}
/**
* Grant access to the internal sample buffer. Used in Pose Estimation to
* replay odometry inputs stored within this buffer.
*/
std::vector<std::pair<units::second_t, T>>& GetInternalBuffer() {
return m_pastSnapshots;
}
private:
units::second_t m_historySize;
std::vector<std::pair<units::second_t, T>> m_pastSnapshots;