Tune the localizer to work reasonably well
The biggest changes here are adding back in voltage/angular error (in
effect, reverting parts of I9744c4808edf3a43ae1c76d022460ee1d4c9ed3e)
and tweaking some of the constants so that we better trust encoders.
Change-Id: Ibd9488e0d92d86792bb7cc6437a387589252a463
diff --git a/y2019/control_loops/drivetrain/localizer.h b/y2019/control_loops/drivetrain/localizer.h
index b717837..58b6901 100644
--- a/y2019/control_loops/drivetrain/localizer.h
+++ b/y2019/control_loops/drivetrain/localizer.h
@@ -100,7 +100,7 @@
// The threshold to use for completely rejecting potentially bad target
// matches.
// TODO(james): Tune
- static constexpr Scalar kRejectionScore = 1.0;
+ static constexpr Scalar kRejectionScore = 1000000.0;
// Checks that the targets coming in make some sense--mostly to prevent NaNs
// or the such from propagating.
@@ -306,6 +306,11 @@
size_t view_idx = best_frames[ii];
if (view_idx < 0 || view_idx >= camera_views.size()) {
LOG(ERROR, "Somehow, the view scorer failed.\n");
+ h_functions->push_back(
+ [](const State &, const Input &) { return Output::Zero(); });
+ dhdx_functions->push_back([](const State &) {
+ return Eigen::Matrix<Scalar, kNOutputs, kNStates>::Zero();
+ });
continue;
}
const Eigen::Matrix<Scalar, kNOutputs, kNStates> best_H =