Merge "Reject target matches if they are too unreasonable"
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 8330289..d26552a 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -199,6 +199,12 @@
H(0, StateIdx::kX) = 1;
H(1, StateIdx::kY) = 1;
H(2, StateIdx::kTheta) = 1;
+ // If the heading is off by too much, assume that we got a false-positive
+ // and don't use the correction.
+ if (std::abs(aos::math::DiffAngle(theta(), Z(2))) > M_PI_2) {
+ AOS_LOG(WARNING, "Dropped image match due to heading mismatch.\n");
+ return;
+ }
ekf_.Correct(Z, nullptr, {}, [H, Z](const State &X, const Input &) {
Eigen::Vector3d Zhat = H * X;
// In order to deal with wrapping of the