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.h b/y2018/control_loops/superstructure/arm/ekf.h
new file mode 100644
index 0000000..9f45303
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/ekf.h
@@ -0,0 +1,44 @@
+#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_EKF_H_
+#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_EKF_H_
+
+#include "Eigen/Dense"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+// An extended kalman filter for the Arm.
+// Our states are:
+// [theta0, omega0, theta1, omega1, voltage error0, voltage error1]
+class EKF {
+ public:
+ EKF();
+
+ // Resets the internal state back to X. Resets the torque disturbance to 0.
+ void Reset(const ::Eigen::Matrix<double, 4, 1> &X);
+ // TODO(austin): Offset the internal state when we calibrate.
+
+ // Corrects the state estimate with the provided sensor reading.
+ void Correct(const ::Eigen::Matrix<double, 2, 1> &Y, double dt);
+
+ // Predicts the next state and covariance given the control input.
+ void Predict(const ::Eigen::Matrix<double, 2, 1> &U, double dt);
+
+ // Returns the current state and covariance estimates.
+ const ::Eigen::Matrix<double, 6, 1> &X_hat() const { return X_hat_; }
+ double X_hat(int i) const { return X_hat_(i); }
+ const ::Eigen::Matrix<double, 6, 6> &P() const { return P_; }
+
+ private:
+ ::Eigen::Matrix<double, 6, 1> X_hat_;
+ ::Eigen::Matrix<double, 6, 6> P_;
+ ::Eigen::Matrix<double, 6, 6> P_reset_;
+};
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
+
+#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_EKF_H_