Trust base mounted cameras less
We care less about them, and the calibration is worse.
Change-Id: I8c5298be06372addb8f03b0555070e0c2dd5a453
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 2d31144..937d8c4 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -333,6 +333,13 @@
noises *= 1.0 + std::abs((right_velocity() - left_velocity()) /
(2.0 * dt_config_.robot_radius) +
(is_turret ? turret_data.velocity : 0.0));
+
+ // Pay less attention to cameras that aren't actually on the turret, since they
+ // are less useful when it comes to actually making shots.
+ if (!is_turret) {
+ noises *= 3.0;
+ }
+
Eigen::Matrix3f R = Eigen::Matrix3f::Zero();
R.diagonal() = noises.cwiseAbs2();
Eigen::Matrix<float, HybridEkf::kNOutputs, HybridEkf::kNStates> H;
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index aac8f75..8ccb55c 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -521,7 +521,7 @@
// Give the filters enough time to converge.
RunFor(chrono::seconds(10));
VerifyNearGoal(5e-2);
- EXPECT_TRUE(VerifyEstimatorAccurate(4e-2));
+ EXPECT_TRUE(VerifyEstimatorAccurate(0.1));
}
// Tests that we are able to handle a constant, non-zero turret angle.
@@ -592,7 +592,7 @@
RunFor(chrono::seconds(10));
VerifyNearGoal(5e-3);
- EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
+ EXPECT_TRUE(VerifyEstimatorAccurate(5e-2));
}
// Tests that we don't blow up if we stop getting updates for an extended period