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/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index 9866803..bd48315 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -50,7 +50,8 @@
double gyro_rate, double longitudinal_accelerometer) = 0;
// Reset the absolute position of the estimator.
virtual void ResetPosition(::aos::monotonic_clock::time_point t, double x,
- double y, double theta) = 0;
+ double y, double theta,
+ double theta_uncertainty) = 0;
// There are several subtly different norms floating around for state
// matrices. In order to avoid that mess, we jus tprovide direct accessors for
// the values that most people care about.
@@ -109,7 +110,7 @@
}
void ResetPosition(::aos::monotonic_clock::time_point t, double x, double y,
- double theta) override {
+ double theta, double /*theta_override*/) override {
const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
ekf_.ResetInitialState(t, (Ekf::State() << x, y, theta, left_encoder, 0,