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;
diff --git a/y2019/actors/autonomous_actor.cc b/y2019/actors/autonomous_actor.cc
index 85d516c..1647f04 100644
--- a/y2019/actors/autonomous_actor.cc
+++ b/y2019/actors/autonomous_actor.cc
@@ -60,6 +60,7 @@
localizer_resetter->x = 1.0;
localizer_resetter->y = 1.5 * turn_scalar;
localizer_resetter->theta = M_PI;
+ localizer_resetter->theta_uncertainty = 0.0000001;
if (!localizer_resetter.Send()) {
LOG(ERROR, "Failed to reset localizer.\n");
}
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index f33f4e5..282ca9e 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -34,14 +34,19 @@
target_selector_(event_loop) {
localizer_.ResetInitialState(::aos::monotonic_clock::now(),
Localizer::State::Zero(), localizer_.P());
- ResetPosition(::aos::monotonic_clock::now(), 0.5, 3.4, 0.0);
+ ResetPosition(::aos::monotonic_clock::now(), 0.5, 3.4, 0.0, 0.0);
frame_fetcher_ = event_loop_->MakeFetcher<CameraFrame>(
".y2019.control_loops.drivetrain.camera_frames");
}
void EventLoopLocalizer::Reset(::aos::monotonic_clock::time_point now,
- const Localizer::State &state) {
- localizer_.ResetInitialState(now, state, localizer_.P());
+ const Localizer::State &state,
+ double theta_uncertainty) {
+ Localizer::StateSquare newP = localizer_.P();
+ if (theta_uncertainty > 0.0) {
+ newP(StateIdx::kTheta, StateIdx::kTheta) = theta_uncertainty;
+ }
+ localizer_.ResetInitialState(now, state, newP);
}
void EventLoopLocalizer::Update(
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.h b/y2019/control_loops/drivetrain/event_loop_localizer.h
index 6d5ca29..9255109 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.h
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.h
@@ -34,9 +34,9 @@
::aos::EventLoop *event_loop);
void Reset(::aos::monotonic_clock::time_point t,
- const Localizer::State &state);
+ const Localizer::State &state, double theta_uncertainty);
void ResetPosition(::aos::monotonic_clock::time_point t, double x, double y,
- double theta) override {
+ double theta, double theta_uncertainty) override {
// When we reset the state, we want to keep the encoder positions intact, so
// we copy from the original state and reset everything else.
Localizer::State new_state = localizer_.X_hat();
@@ -50,7 +50,8 @@
new_state(7, 0) = 0.0;
new_state(8, 0) = 0.0;
new_state(9, 0) = 0.0;
- Reset(t, new_state);
+
+ Reset(t, new_state, theta_uncertainty);
}
void Update(const ::Eigen::Matrix<double, 2, 1> &U,
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index b405588..c6928e0 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -71,7 +71,7 @@
localizer_state;
localizer_state.setZero();
localizer_state.block<3, 1>(0, 0) = xytheta;
- localizer_.Reset(monotonic_clock::now(), localizer_state);
+ localizer_.Reset(monotonic_clock::now(), localizer_state, 0.0);
}
void RunIteration() {