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/BUILD b/y2019/control_loops/drivetrain/BUILD
index 38c73f0..310b853 100644
--- a/y2019/control_loops/drivetrain/BUILD
+++ b/y2019/control_loops/drivetrain/BUILD
@@ -1,4 +1,5 @@
load("//aos/build:queues.bzl", "queue_library")
+load("//tools/build_rules:select.bzl", "cpu_select", "compiler_select")
genrule(
name = "genrule_drivetrain",
@@ -99,3 +100,42 @@
"//aos/testing:googletest",
],
)
+
+cc_library(
+ name = "localizer",
+ hdrs = ["localizer.h"],
+ deps = [
+ ":camera",
+ "//frc971/control_loops:pose",
+ "//frc971/control_loops/drivetrain:hybrid_ekf",
+ ],
+)
+
+cc_test(
+ name = "localizer_test",
+ srcs = ["localizer_test.cc"],
+ defines =
+ cpu_select({
+ "amd64": [
+ "SUPPORT_PLOT=1",
+ ],
+ "arm": [],
+ }),
+ linkstatic = True,
+ deps = [
+ ":localizer",
+ ":drivetrain_base",
+ "//aos/testing:googletest",
+ "//aos/testing:random_seed",
+ "//aos/testing:test_shm",
+ "//frc971/control_loops/drivetrain:trajectory",
+ "//y2019:constants",
+ "//frc971/control_loops/drivetrain:splinedrivetrain",
+ "@com_github_gflags_gflags//:gflags",
+ ] + cpu_select({
+ "amd64": [
+ "//third_party/matplotlib-cpp",
+ ],
+ "arm": [],
+ }),
+)