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,
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,
diff --git a/frc971/control_loops/drivetrain/localizer.q b/frc971/control_loops/drivetrain/localizer.q
index 1ae0adc..8fef686 100644
--- a/frc971/control_loops/drivetrain/localizer.q
+++ b/frc971/control_loops/drivetrain/localizer.q
@@ -6,6 +6,7 @@
float x; // X position, meters
float y; // Y position, meters
float theta; // heading, radians
+ double theta_uncertainty; // Uncertainty in theta.
};
queue LocalizerControl localizer_control;