commit 286b428295651413cceb2d14585eb310eed4b245
author James Kuszmaul <jabukuszmaul@gmail.com> Wed Feb 26 20:29:32 2020 -0800
committer James Kuszmaul <jabukuszmaul@gmail.com> Wed Feb 26 20:43:26 2020 -0800
tree c0d9a3ea02b244ac153b1689cf74be35a832a207
parent 818b974a95c1b4684933b277394f7a5d2fd3b91c

Initialize y2020 Localizer correctly

This prevents a crash loop.

Change-Id: I5683faa7752a4b0fcea81023ae784a9990c24288

y2020/control_loops/drivetrain/localizer.cc

diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 4b11f6f..0178790 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -40,6 +40,11 @@
                              HandleSuperstructureStatus(status);
                            });
 
+  event_loop->OnRun([this, event_loop]() {
+    ekf_.ResetInitialState(event_loop->monotonic_now(), Ekf::State::Zero(),
+                           ekf_.P());
+  });
+
   image_fetchers_.emplace_back(
       event_loop_->MakeFetcher<frc971::vision::sift::ImageMatchResult>(
           "/pi1/camera"));