Undelay our status X_hat
There are 2 steps in the Kalman Filter. Predict and Correct. We
typically run Correct, compute our controller value, and then run
Predict.
Previously, we were grabbing the Predicted value on the cycle before it
was correct. This means we were publishing our best guess at where the
state would be next cycle instead of the state we used. This isn't
particularly helpful...
Change-Id: I03c1088d0b919e1fbf17ceabd1448d7f1894149a
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2016/control_loops/superstructure/superstructure_controls.cc b/y2016/control_loops/superstructure/superstructure_controls.cc
index d16c73c..ee2020e 100644
--- a/y2016/control_loops/superstructure/superstructure_controls.cc
+++ b/y2016/control_loops/superstructure/superstructure_controls.cc
@@ -101,10 +101,12 @@
// Handle zeroing errors
if (estimators_[kShoulderIndex].error()) {
AOS_LOG(ERROR, "zeroing error with shoulder_estimator\n");
+ X_hat_ = loop_->X_hat();
return;
}
if (estimators_[kWristIndex].error()) {
AOS_LOG(ERROR, "zeroing error with wrist_estimator\n");
+ X_hat_ = loop_->X_hat();
return;
}
@@ -130,6 +132,7 @@
Y_ << position_shoulder->encoder(), position_wrist->encoder();
Y_ += offset_;
loop_->Correct(Y_);
+ X_hat_ = loop_->X_hat();
}
}