Implement 2019 EKF
The main remaining task with this is to integrate it with the actual
drivetrain code, and then to tune it on the actual robot.
For performance, on my computer I'm seeing ~0.6ms per simulation
iteration, which will vary depending on exact camera frame rates and
latencies. It might need a bit of optimization for CPU load, but we
should be reasonably close.
Change-Id: I286599e2c2f88dfef200afeb9ebbe9e7108714bf
diff --git a/y2019/control_loops/drivetrain/camera.h b/y2019/control_loops/drivetrain/camera.h
index ff53b7b..6439d0d 100644
--- a/y2019/control_loops/drivetrain/camera.h
+++ b/y2019/control_loops/drivetrain/camera.h
@@ -193,7 +193,7 @@
::std::vector<::std::vector<Pose>> PlotPoints() const {
::std::vector<::std::vector<Pose>> list_of_lists;
for (const auto &view : target_views()) {
- list_of_lists.push_back({pose_, view.target->pose()});
+ list_of_lists.push_back({pose_, view.target->pose().Rebase(&pose_)});
}
return list_of_lists;
}
@@ -254,9 +254,9 @@
// This number is unitless and if greater than 1, implies that the target is
// visible to the camera and if less than 1 implies it is too small to be
// registered on the camera.
- const Scalar apparent_width =
- ::std::max(0.0, ::std::cos(skew) *
- noise_parameters_.max_viewable_distance / distance);
+ const Scalar apparent_width = ::std::max<Scalar>(
+ 0.0,
+ ::std::cos(skew) * noise_parameters_.max_viewable_distance / distance);
if (apparent_width < 1.0) {
return;