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/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 88e7a02..13fee1a 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -253,24 +253,23 @@
Y << position->left_encoder(), position->right_encoder(), last_gyro_rate_,
last_accel_;
kf_.Correct(Y);
- // If we get a new message setting the absolute position, then reset the
- // localizer.
- // TODO(james): Use a watcher (instead of a fetcher) once we support it in
- // simulation.
- if (localizer_control_fetcher_.Fetch()) {
- VLOG(1) << "localizer_control "
- << aos::FlatbufferToJson(localizer_control_fetcher_.get());
- localizer_->ResetPosition(
- monotonic_now, localizer_control_fetcher_->x(),
- localizer_control_fetcher_->y(), localizer_control_fetcher_->theta(),
- localizer_control_fetcher_->theta_uncertainty(),
- !localizer_control_fetcher_->keep_current_theta());
- }
localizer_->Update({last_last_left_voltage_, last_last_right_voltage_},
monotonic_now, position->left_encoder(),
position->right_encoder(),
down_estimator_.avg_recent_yaw_rates(),
down_estimator_.avg_recent_accel());
+ // If we get a new message setting the absolute position, then reset the
+ // localizer.
+ if (localizer_control_fetcher_.Fetch()) {
+ VLOG(1) << "localizer_control "
+ << aos::FlatbufferToJson(localizer_control_fetcher_.get());
+ localizer_->ResetPosition(
+ monotonic_now,
+ localizer_control_fetcher_->x(), localizer_control_fetcher_->y(),
+ localizer_control_fetcher_->theta(),
+ localizer_control_fetcher_->theta_uncertainty(),
+ !localizer_control_fetcher_->keep_current_theta());
+ }
}
dt_openloop_.SetPosition(position, left_gear_, right_gear_);
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_;