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/frc971/control_loops/pose.h b/frc971/control_loops/pose.h
index ab8afd0..7945e5e 100644
--- a/frc971/control_loops/pose.h
+++ b/frc971/control_loops/pose.h
@@ -74,8 +74,8 @@
// pitch/roll components of the rotation. Ignores the bottom row.
TypedPose(const Eigen::Matrix<Scalar, 4, 4> &H) {
pos_ = H.template block<3, 1>(0, 3);
- const Eigen::Vector3d rotated_x =
- H.template block<3, 3>(0, 0) * Eigen::Vector3d::UnitX();
+ const Eigen::Matrix<Scalar, 3, 1> rotated_x =
+ H.template block<3, 3>(0, 0) * Eigen::Matrix<Scalar, 3, 1>::UnitX();
theta_ = std::atan2(rotated_x.y(), rotated_x.x());
}