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/dynamics.h b/y2018/control_loops/superstructure/arm/dynamics.h
index 832796a..d0194b0 100644
--- a/y2018/control_loops/superstructure/arm/dynamics.h
+++ b/y2018/control_loops/superstructure/arm/dynamics.h
@@ -104,6 +104,33 @@
.finished();
}
+ // Calculates the acceleration given the current augmented kalman filter state
+ // and control input.
+ static const ::Eigen::Matrix<double, 6, 1> EKFAcceleration(
+ const ::Eigen::Matrix<double, 6, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) {
+ ::Eigen::Matrix<double, 2, 2> K1;
+ ::Eigen::Matrix<double, 2, 2> K2;
+
+ MatriciesForState(X.block<4, 1>(0, 0), &K1, &K2);
+
+ const ::Eigen::Matrix<double, 2, 1> velocity =
+ (::Eigen::Matrix<double, 2, 1>() << X(1, 0), X(3, 0)).finished();
+
+ const ::Eigen::Matrix<double, 2, 1> torque =
+ K3 *
+ (U +
+ (::Eigen::Matrix<double, 2, 1>() << X(4, 0), X(5, 0)).finished()) -
+ K4 * velocity;
+
+ const ::Eigen::Matrix<double, 2, 1> accel =
+ K1.inverse() * (torque - K2 * velocity);
+
+ return (::Eigen::Matrix<double, 6, 1>() << X(1, 0), accel(0, 0), X(3, 0),
+ accel(1, 0), 0.0, 0.0)
+ .finished();
+ }
+
// Calculates the voltage required to follow the trajectory. This requires
// knowing the current state, desired angular velocity and acceleration.
static const ::Eigen::Matrix<double, 2, 1> FF_U(
@@ -124,6 +151,13 @@
return ::frc971::control_loops::RungeKutta(Dynamics::Acceleration, X, U,
dt);
}
+
+ static const ::Eigen::Matrix<double, 6, 1> UnboundedEKFDiscreteDynamics(
+ const ::Eigen::Matrix<double, 6, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U, double dt) {
+ return ::frc971::control_loops::RungeKutta(Dynamics::EKFAcceleration, X, U,
+ dt);
+ }
};
} // namespace arm