Don't assume orientation of target in y2020 localizer
The localizer had, for some reason, been assuming that the target was
oriented to have a yaw of zero. I suspect that using the idealized
target has led us to use a different orientation (maybe red instead of
blue) of target, which caused things not to converge properly.
Change-Id: I0ab645748ceefd7fc890e2d7293203c44454c56c
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index a1b6121..c0da838 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -84,6 +84,9 @@
H.block<3, 1>(0, 3) << 10.0, 0, 0;
locations.push_back(H);
H.block<3, 1>(0, 3) << -10.0, 0, 0;
+ H.block<3, 3>(0, 0) =
+ Eigen::AngleAxis<double>(1.1, Eigen::Vector3d::UnitZ()) *
+ H.block<3, 3>(0, 0);
locations.push_back(H);
return locations;
}