Reject target matches if they are too unreasonable
Note that this does mean that the robot needs to have its position
initialized with a sane heading.
Change-Id: Ib00115baa8065594af4ad2b4d0c4b989081edbdf
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 4b164b5..0add232 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -193,6 +193,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