Assume angular uncertainty in auto is good
We align the robot really well in auto. So we can assume that the
covariance of the heading is small.
Change-Id: Ifae45c36612476e21ce50987ccab8aa8846df86f
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 6a2ea76..2af6765 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -245,7 +245,8 @@
LOG_STRUCT(DEBUG, "localizer_control", *localizer_control_fetcher_);
localizer_->ResetPosition(monotonic_now, localizer_control_fetcher_->x,
localizer_control_fetcher_->y,
- localizer_control_fetcher_->theta);
+ localizer_control_fetcher_->theta,
+ localizer_control_fetcher_->theta_uncertainty);
}
localizer_->Update({last_last_left_voltage_, last_last_right_voltage_},
monotonic_now, position->left_encoder,