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/ekf.cc b/y2018/control_loops/superstructure/arm/ekf.cc
new file mode 100644
index 0000000..93c88d4
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/ekf.cc
@@ -0,0 +1,96 @@
+#include "y2018/control_loops/superstructure/arm/ekf.h"
+
+#include "Eigen/Dense"
+#include <iostream>
+
+#include "frc971/control_loops/jacobian.h"
+#include "y2018/control_loops/superstructure/arm/dynamics.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+namespace {
+// TODO(austin): When tuning this, make sure to verify that you are waiting
+// enough cycles to make sure it converges at startup. Otherwise you will have a
+// bad day.
+const ::Eigen::Matrix<double, 6, 6> Q_covariance(
+ (::Eigen::DiagonalMatrix<double, 6>().diagonal() << ::std::pow(0.1, 2),
+ ::std::pow(2.0, 2), ::std::pow(0.1, 2), ::std::pow(2.0, 2),
+ ::std::pow(0.80, 2), ::std::pow(0.70, 2))
+ .finished()
+ .asDiagonal());
+} // namespace
+
+EKF::EKF() {
+ X_hat_.setZero();
+ P_ = Q_covariance;
+ //::std::cout << "Reset P: " << P_ << ::std::endl;
+ // TODO(austin): Running the EKF 2000 cycles works, but isn't super clever.
+ // We could just solve the DARE.
+
+ for (int i = 0; i < 1000; ++i) {
+ Predict(::Eigen::Matrix<double, 2, 1>::Zero(), 0.00505);
+ Correct(::Eigen::Matrix<double, 2, 1>::Zero(), 0.00505);
+ }
+ //::std::cout << "Stabilized P: " << P_ << ::std::endl;
+ for (int i = 0; i < 1000; ++i) {
+ Predict(::Eigen::Matrix<double, 2, 1>::Zero(), 0.00505);
+ Correct(::Eigen::Matrix<double, 2, 1>::Zero(), 0.00505);
+ }
+ //::std::cout << "Really stabilized P: " << P_ << ::std::endl;
+ P_reset_ = P_;
+
+ Reset(::Eigen::Matrix<double, 4, 1>::Zero());
+}
+
+void EKF::Reset(const ::Eigen::Matrix<double, 4, 1> &X) {
+ X_hat_.setZero();
+ P_ = P_reset_;
+ X_hat_.block<4, 1>(0, 0) = X;
+}
+
+void EKF::Predict(const ::Eigen::Matrix<double, 2, 1> &U, double dt) {
+ const ::Eigen::Matrix<double, 6, 6> A =
+ ::frc971::control_loops::NumericalJacobianX<6, 2>(
+ Dynamics::UnboundedEKFDiscreteDynamics, X_hat_, U, dt);
+
+ X_hat_ = Dynamics::UnboundedEKFDiscreteDynamics(X_hat_, U, dt);
+ P_ = A * P_ * A.transpose() + Q_covariance;
+}
+
+void EKF::Correct(const ::Eigen::Matrix<double, 2, 1> &Y, double /*dt*/) {
+ const ::Eigen::Matrix<double, 2, 2> R_covariance(
+ (::Eigen::DiagonalMatrix<double, 2>().diagonal() << ::std::pow(0.01, 2),
+ ::std::pow(0.01, 2))
+ .finished()
+ .asDiagonal());
+ // H is the jacobian of the h(x) measurement prediction function
+ const ::Eigen::Matrix<double, 2, 6> H_jacobian =
+ (::Eigen::Matrix<double, 2, 6>() << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 1.0, 0.0, 0.0, 0.0)
+ .finished();
+
+ // Update step Measurement residual error of proximal and distal joint
+ // angles.
+ const ::Eigen::Matrix<double, 2, 1> Y_hat =
+ Y - (::Eigen::Matrix<double, 2, 1>() << X_hat_(0), X_hat_(2)).finished();
+ // Residual covariance
+ const ::Eigen::Matrix<double, 2, 2> S =
+ H_jacobian * P_ * H_jacobian.transpose() + R_covariance;
+
+ // K is the Near-optimal Kalman gain
+ const ::Eigen::Matrix<double, 6, 2> kalman_gain =
+ P_ * H_jacobian.transpose() * S.inverse();
+ // Updated state estimate
+ X_hat_ = X_hat_ + kalman_gain * Y_hat;
+ // Updated covariance estimate
+ P_ = (::Eigen::Matrix<double, 6, 6>::Identity() - kalman_gain * H_jacobian) *
+ P_;
+}
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018