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

[wpimath] Fix crash in KF latency compensator (#3888)

Browse Source 
It would crash in C++ if the global measurement was sooner than all the
snapshots.

Align Java with the changes and better document computation approach.
This commit is contained in:
Tyler Veness
2022-01-09 23:01:04 -08:00
committed by GitHub
parent a3a0334fad
commit ba0908216c
2 changed files with 75 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -86,26 +86,46 @@ class KalmanFilterLatencyCompensator {
return;
}
// We will perform a binary search to find the index of the element in the
// vector that has a timestamp that is equal to or greater than the vision
// measurement timestamp.
auto lowerBoundIter = std::lower_bound(
// Perform a binary search to find the index of first snapshot whose
// timestamp is greater than or equal to the global measurement timestamp
auto it = std::lower_bound(
m_pastObserverSnapshots.cbegin(), m_pastObserverSnapshots.cend(),
timestamp,
[](const auto& entry, const auto& ts) { return entry.first < ts; });
int index = std::distance(m_pastObserverSnapshots.cbegin(), lowerBoundIter);
// High and Low should be the same. The sampled timestamp is greater than or
// equal to the vision pose timestamp. We will now find the entry which is
// closest in time to the requested timestamp.
size_t indexOfClosestEntry;
size_t indexOfClosestEntry =
units::math::abs(
timestamp - m_pastObserverSnapshots[std::max(index - 1, 0)].first) <
units::math::abs(timestamp -
m_pastObserverSnapshots[index].first)
? index - 1
: index;
if (it == m_pastObserverSnapshots.cbegin()) {
// If the global measurement is older than any snapshot, throw out the
// measurement because there's no state estimate into which to incorporate
// the measurement
if (timestamp < it->first) {
return;
}
// If the first snapshot has same timestamp as the global measurement, use
// that snapshot
indexOfClosestEntry = 0;
} else if (it == m_pastObserverSnapshots.cend()) {
// If all snapshots are older than the global measurement, use the newest
// snapshot
indexOfClosestEntry = m_pastObserverSnapshots.size() - 1;
} else {
// Index of snapshot taken after the global measurement
int nextIdx = std::distance(m_pastObserverSnapshots.cbegin(), it);
// Index of snapshot taken before the global measurement. Since we already
// handled the case where the index points to the first snapshot, this
// computation is guaranteed to be nonnegative.
int prevIdx = nextIdx - 1;
// Find the snapshot closest in time to global measurement
units::second_t prevTimeDiff =
units::math::abs(timestamp - m_pastObserverSnapshots[prevIdx].first);
units::second_t nextTimeDiff =
units::math::abs(timestamp - m_pastObserverSnapshots[nextIdx].first);
indexOfClosestEntry = prevTimeDiff < nextTimeDiff ? prevIdx : nextIdx;
}
units::second_t lastTimestamp =
m_pastObserverSnapshots[indexOfClosestEntry].first - nominalDt;