Tune localizer for actual camera data
It turns out just using the implied pose directly caused issues due to
us not handling highly cross-correlated noise well. As such, just
extract the best signals and then reconstruct them into a position
measurement. This does introduce some imperfections because I haven't
started doing time compensation again.
Change-Id: I9eab075c734b34d0588e1fd3131dd55842158aad
diff --git a/y2020/control_loops/drivetrain/localizer.h b/y2020/control_loops/drivetrain/localizer.h
index 1d6f816..32a78da 100644
--- a/y2020/control_loops/drivetrain/localizer.h
+++ b/y2020/control_loops/drivetrain/localizer.h
@@ -73,7 +73,8 @@
// Processes new image data from the given pi and updates the EKF.
void HandleImageMatch(std::string_view pi,
- const frc971::vision::sift::ImageMatchResult &result);
+ const frc971::vision::sift::ImageMatchResult &result,
+ aos::monotonic_clock::time_point now);
// Processes the most recent turret position and stores it in the turret_data_
// buffer.