Swap the localizer reset order
This is prevents a situation where we end up saying that two measurements
occur at the exact same time and so end up performing a correction with
an extraordinarily high gain, which, with single-precision floats,
causes issues.
Change-Id: I5567963fcb2953087bf455705a91a6c17d939fca
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 7e7d44a..de4f721 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -672,6 +672,10 @@
// We use X_hat_ and P_ to store the intermediate states, and then
// once we reach the end they will all be up-to-date.
ProcessObservation(&*cur_it, cur_it->t - cur_it->prev_t, &X_hat_, &P_);
+ // TOOD(james): Note that this can be triggered when there are extremely
+ // small values in P_. This is particularly likely if Scalar is just float
+ // and we are performing zero-time updates where the predict step never
+ // runs.
CHECK(X_hat_.allFinite());
if (next_it != observations_.end()) {
next_it->X_hat = X_hat_;