Tune the localizer to work reasonably well
The biggest changes here are adding back in voltage/angular error (in
effect, reverting parts of I9744c4808edf3a43ae1c76d022460ee1d4c9ed3e)
and tweaking some of the constants so that we better trust encoders.
Change-Id: Ibd9488e0d92d86792bb7cc6437a387589252a463
diff --git a/y2019/constants.cc b/y2019/constants.cc
index aac1f95..ba737dd 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -143,15 +143,15 @@
stilts_params->zeroing_constants.allowable_encoder_error = 0.9;
r->camera_noise_parameters = {.max_viewable_distance = 10.0,
- .heading_noise = 0.25,
- .nominal_distance_noise = 0.3,
+ .heading_noise = 0.1,
+ .nominal_distance_noise = 0.15,
.nominal_skew_noise = 0.45,
.nominal_height_noise = 0.01};
// Deliberately make FOV a bit large so that we are overly conservative in
// our EKF.
for (auto &camera : r->cameras) {
- camera.fov = M_PI_2 * 1.1;
+ camera.fov = M_PI_2 * 1.5;
}
switch (team) {
@@ -349,7 +349,7 @@
{{kRocketFarX, kRocketHatchY, kNormalZ}, kRocketFarTheta}, kDiscRadius,
kDiscGoal);
- const Target hp_slot({{0.0, kHpSlotY, kNormalZ}, kHpSlotTheta}, 0.05,
+ const Target hp_slot({{0.0, kHpSlotY, kNormalZ}, kHpSlotTheta}, 0.00,
kBothGoal);
const ::std::array<Target, 8> quarter_field_targets{
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index dad667f..9403b2e 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -33,14 +33,14 @@
localizer_(dt_config, &robot_pose_) {
localizer_.ResetInitialState(::aos::monotonic_clock::now(),
Localizer::State::Zero(), localizer_.P());
- ResetPosition(0.5, 3.4, 0.0);
+ ResetPosition(::aos::monotonic_clock::now(), 0.5, 3.4, 0.0);
frame_fetcher_ = event_loop_->MakeFetcher<CameraFrame>(
".y2019.control_loops.drivetrain.camera_frames");
}
-void EventLoopLocalizer::Reset(const Localizer::State &state) {
- localizer_.ResetInitialState(::aos::monotonic_clock::now(), state,
- localizer_.P());
+void EventLoopLocalizer::Reset(::aos::monotonic_clock::time_point now,
+ const Localizer::State &state) {
+ localizer_.ResetInitialState(now, state, localizer_.P());
}
void EventLoopLocalizer::Update(
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.h b/y2019/control_loops/drivetrain/event_loop_localizer.h
index 9207ec1..6d5ca29 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.h
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.h
@@ -33,8 +33,10 @@
&dt_config,
::aos::EventLoop *event_loop);
- void Reset(const Localizer::State &state);
- void ResetPosition(double x, double y, double theta) override {
+ void Reset(::aos::monotonic_clock::time_point t,
+ const Localizer::State &state);
+ void ResetPosition(::aos::monotonic_clock::time_point t, double x, double y,
+ double theta) 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();
@@ -44,7 +46,11 @@
// Velocity terms.
new_state(4, 0) = 0.0;
new_state(6, 0) = 0.0;
- Reset(new_state);
+ // Voltage/angular error terms.
+ new_state(7, 0) = 0.0;
+ new_state(8, 0) = 0.0;
+ new_state(9, 0) = 0.0;
+ Reset(t, new_state);
}
void Update(const ::Eigen::Matrix<double, 2, 1> &U,
@@ -58,14 +64,24 @@
return localizer_.X_hat(StateIdx::kY); }
double theta() const override {
return localizer_.X_hat(StateIdx::kTheta); }
+ double left_encoder() const override {
+ return localizer_.X_hat(StateIdx::kLeftEncoder);
+ }
+ double right_encoder() const override {
+ return localizer_.X_hat(StateIdx::kRightEncoder);
+ }
double left_velocity() const override {
return localizer_.X_hat(StateIdx::kLeftVelocity);
}
double right_velocity() const override {
return localizer_.X_hat(StateIdx::kRightVelocity);
}
- double left_voltage_error() const override { return 0.0; }
- double right_voltage_error() const override { return 0.0; }
+ double left_voltage_error() const override {
+ return localizer_.X_hat(StateIdx::kLeftVoltageError);
+ }
+ double right_voltage_error() const override {
+ return localizer_.X_hat(StateIdx::kRightVoltageError);
+ }
TargetSelector *target_selector() override {
return &target_selector_;
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index f0732a3..b405588 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -67,10 +67,11 @@
void SetStartingPosition(const Eigen::Matrix<double, 3, 1> &xytheta) {
*drivetrain_motor_plant_.mutable_state() << xytheta.x(), xytheta.y(),
xytheta(2, 0), 0.0, 0.0;
- Eigen::Matrix<double, 7, 1> localizer_state;
+ Eigen::Matrix<double, EventLoopLocalizer::Localizer::kNStates, 1>
+ localizer_state;
localizer_state.setZero();
localizer_state.block<3, 1>(0, 0) = xytheta;
- localizer_.Reset(localizer_state);
+ localizer_.Reset(monotonic_clock::now(), localizer_state);
}
void RunIteration() {
@@ -310,7 +311,7 @@
SetStartingPosition({4, HPSlotLeft().abs_pos().y(), M_PI});
my_drivetrain_queue_.goal.MakeWithBuilder()
.controller_type(3)
- .throttle(0.9)
+ .throttle(0.5)
.Send();
RunForTime(chrono::seconds(6));
diff --git a/y2019/control_loops/drivetrain/localizer.h b/y2019/control_loops/drivetrain/localizer.h
index b717837..58b6901 100644
--- a/y2019/control_loops/drivetrain/localizer.h
+++ b/y2019/control_loops/drivetrain/localizer.h
@@ -100,7 +100,7 @@
// The threshold to use for completely rejecting potentially bad target
// matches.
// TODO(james): Tune
- static constexpr Scalar kRejectionScore = 1.0;
+ static constexpr Scalar kRejectionScore = 1000000.0;
// Checks that the targets coming in make some sense--mostly to prevent NaNs
// or the such from propagating.
@@ -306,6 +306,11 @@
size_t view_idx = best_frames[ii];
if (view_idx < 0 || view_idx >= camera_views.size()) {
LOG(ERROR, "Somehow, the view scorer failed.\n");
+ h_functions->push_back(
+ [](const State &, const Input &) { return Output::Zero(); });
+ dhdx_functions->push_back([](const State &) {
+ return Eigen::Matrix<Scalar, kNOutputs, kNStates>::Zero();
+ });
continue;
}
const Eigen::Matrix<Scalar, kNOutputs, kNStates> best_H =
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index f062234..563c159 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -399,7 +399,7 @@
U(1, 0) = ::std::max(::std::min(U(1, 0), 12.0), -12.0);
state = ::frc971::control_loops::RungeKuttaU(
- [this](const ::Eigen::Matrix<double, 7, 1> &X,
+ [this](const ::Eigen::Matrix<double, 10, 1> &X,
const ::Eigen::Matrix<double, 2, 1> &U) { return DiffEq(X, U); },
state, U,
::std::chrono::duration_cast<::std::chrono::duration<double>>(
@@ -418,7 +418,7 @@
::std::pow(state(StateIdx::kRightVelocity, 0), 2)) /
3.0);
TestLocalizer::State disturbance;
- disturbance << 0.02, 0.02, 0.001, 0.03, 0.02, 0.0, 0.0;
+ disturbance << 0.02, 0.02, 0.001, 0.03, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0;
disturbance *= disturbance_scale;
state += disturbance;
}
@@ -498,9 +498,11 @@
LocalizerTestParams({
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
- (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
.finished(),
- (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
.finished(),
/*noisify=*/false,
/*disturb=*/false,
@@ -512,9 +514,11 @@
LocalizerTestParams({
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
- (TestLocalizer::State() << 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
.finished(),
- (TestLocalizer::State() << 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
.finished(),
/*noisify=*/false,
/*disturb=*/false,
@@ -525,22 +529,41 @@
LocalizerTestParams({
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
- (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
.finished(),
- (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
.finished(),
/*noisify=*/true,
/*disturb=*/false,
/*estimate_tolerance=*/0.2,
- /*goal_tolerance=*/0.2,
+ /*goal_tolerance=*/0.3,
}),
// Repeats perfect scenario, but add initial estimator error.
LocalizerTestParams({
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
- (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
.finished(),
- (TestLocalizer::State() << 0.1, -5.1, -0.01, 0.0, 0.0, 0.0, 0.0)
+ (TestLocalizer::State() << 0.1, -5.1, -0.01, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0)
+ .finished(),
+ /*noisify=*/false,
+ /*disturb=*/false,
+ /*estimate_tolerance=*/1e-4,
+ /*goal_tolerance=*/2e-2,
+ }),
+ // Repeats perfect scenario, but add voltage + angular errors:
+ LocalizerTestParams({
+ /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
+ /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
+ 0.5, 0.02)
+ .finished(),
+ (TestLocalizer::State() << 0.1, -5.1, -0.01, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0)
.finished(),
/*noisify=*/false,
/*disturb=*/false,
@@ -551,9 +574,11 @@
LocalizerTestParams({
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
- (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
.finished(),
- (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
.finished(),
/*noisify=*/false,
/*disturb=*/true,
@@ -564,14 +589,16 @@
LocalizerTestParams({
/*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
/*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
- (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
.finished(),
- (TestLocalizer::State() << 0.1, -5.1, 0.03, 0.0, 0.0, 0.0, 0.0)
+ (TestLocalizer::State() << 0.1, -5.1, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
.finished(),
/*noisify=*/true,
/*disturb=*/true,
/*estimate_tolerance=*/0.15,
- /*goal_tolerance=*/0.8,
+ /*goal_tolerance=*/0.5,
}),
// Try another spline, just in case the one I was using is special for
// some reason; this path will also go straight up to a target, to
@@ -579,14 +606,16 @@
LocalizerTestParams({
/*control_pts_x=*/{{0.5, 3.5, 4.0, 8.0, 11.0, 10.2}},
/*control_pts_y=*/{{1.0, 1.0, -3.0, -2.0, -3.5, -3.65}},
- (TestLocalizer::State() << 0.6, 1.01, 0.01, 0.0, 0.0, 0.0, 0.0)
+ (TestLocalizer::State() << 0.6, 1.01, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
.finished(),
- (TestLocalizer::State() << 0.5, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+ (TestLocalizer::State() << 0.5, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
.finished(),
/*noisify=*/true,
/*disturb=*/false,
- /*estimate_tolerance=*/0.15,
- /*goal_tolerance=*/0.5,
+ /*estimate_tolerance=*/0.25,
+ /*goal_tolerance=*/0.7,
})));
} // namespace testing