Arm works
Added gravity and calibrated it. Terifying...
Change-Id: I70babb1cd3b83ddd7a81f06fb2a75cefd55bcdb8
diff --git a/y2018/control_loops/superstructure/arm/ekf.h b/y2018/control_loops/superstructure/arm/ekf.h
index 9f45303..9ae8b47 100644
--- a/y2018/control_loops/superstructure/arm/ekf.h
+++ b/y2018/control_loops/superstructure/arm/ekf.h
@@ -29,10 +29,19 @@
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_; }
+ const ::Eigen::Matrix<double, 6, 6> &P_reset() const { return P_reset_; }
+ const ::Eigen::Matrix<double, 6, 6> &P_half_converged() const {
+ return P_half_converged_;
+ }
+ const ::Eigen::Matrix<double, 6, 6> &P_converged() const {
+ return P_converged_;
+ }
private:
::Eigen::Matrix<double, 6, 1> X_hat_;
::Eigen::Matrix<double, 6, 6> P_;
+ ::Eigen::Matrix<double, 6, 6> P_half_converged_;
+ ::Eigen::Matrix<double, 6, 6> P_converged_;
::Eigen::Matrix<double, 6, 6> P_reset_;
};