Move y2020 localizer to use floats
This change seems to save ~20-30% of the current drivetrain CPU usage.
I experimented with changing the down estimator to use floats, but the
effects were negligible.
Change-Id: I19edb0431ba03414a890342122db781dc6a7ed51
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index e864330..34d772c 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -208,9 +208,9 @@
void SetStartingPosition(const Eigen::Matrix<double, 3, 1> &xytheta) {
*drivetrain_plant_.mutable_state() << xytheta.x(), xytheta.y(),
xytheta(2, 0), 0.0, 0.0;
- Eigen::Matrix<double, Localizer::HybridEkf::kNStates, 1> localizer_state;
+ Eigen::Matrix<float, Localizer::HybridEkf::kNStates, 1> localizer_state;
localizer_state.setZero();
- localizer_state.block<3, 1>(0, 0) = xytheta;
+ localizer_state.block<3, 1>(0, 0) = xytheta.cast<float>();
localizer_.Reset(monotonic_now(), localizer_state);
}