Tune localizer, remove error states
Tuning constants are from whatever I was running yesterday.
More substantially, I removed the voltage error and angular error states
(during the day I had just zeroed them out on every iteration, but
actually removing them reduces the dimensionality of the EKF, which is
nice). When I looked at the log streamer when we were running the robot,
it just looked like the voltage error terms were oscillating a bit
around zero, suggesting that they were just being counterproductive.
Change-Id: I9744c4808edf3a43ae1c76d022460ee1d4c9ed3e
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index af07089..f0d81cb 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -95,9 +95,12 @@
}
void ResetPosition(double x, double y, double theta) override {
+ const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
+ const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
ekf_.ResetInitialState(
::aos::monotonic_clock::now(),
- (Ekf::State() << x, y, theta, 0, 0, 0, 0, 0, 0, 0).finished(),
+ (Ekf::State() << x, y, theta, left_encoder, 0, right_encoder, 0)
+ .finished(),
ekf_.P());
};
@@ -110,12 +113,8 @@
double right_velocity() const override {
return ekf_.X_hat(StateIdx::kRightVelocity);
}
- double left_voltage_error() const override {
- return ekf_.X_hat(StateIdx::kLeftVoltageError);
- }
- double right_voltage_error() const override {
- return ekf_.X_hat(StateIdx::kRightVoltageError);
- }
+ double left_voltage_error() const override { return 0.0; }
+ double right_voltage_error() const override { return 0.0; }
TrivialTargetSelector *target_selector() override {
return &target_selector_;