Add acceptably tuned EKF for the arm.
I had to switch to a voltage error to get it to converge. I don't know
if that's a residual problem from uninitialized stack or what, but it's
working much better now.
The disturbance estimator has a time constant of like 0.5 seconds right
now. Faster would be nice, but I'll wait until we see it on a bot
before cranking it up much further.
Change-Id: I72d46aa308ce806a35cfed191ee3b15864e6905a
diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index 6224df1..6961dd3 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -19,10 +19,10 @@
srcs = [
"trajectory_test.cc",
],
- restricted_to = ["//tools:k8"],
deps = [
":demo_path",
":dynamics",
+ ":ekf",
":trajectory",
"//aos/testing:googletest",
"//third_party/eigen",
@@ -71,8 +71,24 @@
restricted_to = ["//tools:k8"],
deps = [
":demo_path",
+ ":ekf",
":trajectory",
"//third_party/eigen",
"//third_party/matplotlib-cpp",
],
)
+
+cc_library(
+ name = "ekf",
+ srcs = [
+ "ekf.cc",
+ ],
+ hdrs = [
+ "ekf.h",
+ ],
+ deps = [
+ ":dynamics",
+ "//frc971/control_loops:jacobian",
+ "//third_party/eigen",
+ ],
+)