Tune localizer for actual camera data
It turns out just using the implied pose directly caused issues due to
us not handling highly cross-correlated noise well. As such, just
extract the best signals and then reconstruct them into a position
measurement. This does introduce some imperfections because I haven't
started doing time compensation again.
Change-Id: I9eab075c734b34d0588e1fd3131dd55842158aad
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index c47faa7..c0995b0 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -443,7 +443,10 @@
RunFor(chrono::seconds(3));
VerifyNearGoal();
- EXPECT_TRUE(VerifyEstimatorAccurate(5e-4));
+ // Note: because the current localizer code doesn't do time-compensation
+ // correctly (see comments in localizer.cc), the "perfect" camera updates
+ // aren't actually handled perfectly.
+ EXPECT_TRUE(VerifyEstimatorAccurate(2e-2));
}
// Tests that camera updates with a constant initial error in the position
@@ -461,7 +464,7 @@
// Give the filters enough time to converge.
RunFor(chrono::seconds(10));
VerifyNearGoal(5e-3);
- EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
+ EXPECT_TRUE(VerifyEstimatorAccurate(4e-2));
}
// Tests that camera updates using a non-turreted camera work.
@@ -477,7 +480,7 @@
// Give the filters enough time to converge.
RunFor(chrono::seconds(10));
VerifyNearGoal(5e-3);
- EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
+ EXPECT_TRUE(VerifyEstimatorAccurate(4e-2));
}
// Tests that we are able to handle a constant, non-zero turret angle.
@@ -494,7 +497,7 @@
// Give the filters enough time to converge.
RunFor(chrono::seconds(10));
VerifyNearGoal(5e-3);
- EXPECT_TRUE(VerifyEstimatorAccurate(1e-3));
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
}
// Tests that we are able to handle a constant velocity turret.
@@ -511,7 +514,7 @@
// Give the filters enough time to converge.
RunFor(chrono::seconds(10));
VerifyNearGoal(5e-3);
- EXPECT_TRUE(VerifyEstimatorAccurate(1e-3));
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
}
// Tests that we reject camera measurements when the turret is spinning too
@@ -548,7 +551,7 @@
RunFor(chrono::seconds(10));
VerifyNearGoal(5e-3);
- EXPECT_TRUE(VerifyEstimatorAccurate(1e-3));
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-2));
}
} // namespace testing