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/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 5a450ec..1343770 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -36,6 +36,7 @@
: loop_(::std::move(loop)), estimators_(::std::move(estimators)) {
zeroed_.fill(false);
unprofiled_goal_.setZero();
+ X_hat_.setZero();
}
// Returns whether an error has occured
@@ -95,11 +96,14 @@
return unprofiled_goal_(row, col);
}
- // Returns the current state estimate.
+ // Returns the current state estimate after the most recent Correct. This
+ // does not change when Predict is run.
const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
- return loop_->X_hat();
+ return X_hat_;
}
- double X_hat(int row, int col) const { return loop_->X_hat(row, col); }
+ double X_hat(int row, int col) const { return X_hat()(row, col); }
+ // Returns a mutable reference to the current state of the actual kalman
+ // filter state. Note: changing this won't change X_hat() immediately.
double &mutable_X_hat(int row, int col) const {
return loop_->mutable_X_hat(row, col);
}
@@ -127,6 +131,8 @@
// The goal that the profile tries to reach.
Eigen::Matrix<double, number_of_states, 1> unprofiled_goal_;
+ Eigen::Matrix<double, number_of_states, 1> X_hat_;
+
bool initialized_ = false;
// If true, the subclass should reset in Update. It should then clear this
@@ -277,13 +283,13 @@
builder.add_zeroed(this->zeroed());
// We don't know, so default to the bad case.
- builder.add_position(this->X_hat(0, 0));
- builder.add_velocity(this->X_hat(1, 0));
+ builder.add_position(this->X_hat_(0, 0));
+ builder.add_velocity(this->X_hat_(1, 0));
builder.add_goal_position(this->goal(0, 0));
builder.add_goal_velocity(this->goal(1, 0));
builder.add_unprofiled_goal_position(this->unprofiled_goal(0, 0));
builder.add_unprofiled_goal_velocity(this->unprofiled_goal(1, 0));
- builder.add_voltage_error(this->X_hat(2, 0));
+ builder.add_voltage_error(this->X_hat_(2, 0));
builder.add_calculated_velocity(
(position() - last_position_) /
::aos::time::DurationInSeconds(this->loop_->plant().coefficients().dt));
@@ -305,6 +311,7 @@
if (this->estimators_[0].error()) {
AOS_LOG(ERROR, "zeroing error\n");
+ this->X_hat_ = this->loop_->X_hat();
return;
}
@@ -324,6 +331,7 @@
this->Y_ << new_position.encoder();
this->Y_ += this->offset_;
this->loop_->Correct(Y_);
+ this->X_hat_ = this->loop_->X_hat();
}
template <class ZeroingEstimator>
@@ -375,6 +383,7 @@
this->loop_->mutable_X_hat(0, 0) = Y_(0, 0);
this->loop_->mutable_X_hat(1, 0) = 0.0;
this->loop_->mutable_X_hat(2, 0) = 0.0;
+ this->X_hat_.setZero();
this->should_reset_ = false;
}
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index d6e8f15..65135d2 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -86,11 +86,12 @@
// Returns the current position
double position() const { return profiled_subsystem_.position(); }
+ // Returns the most recently corrected state.
Eigen::Vector3d estimated_state() const {
return profiled_subsystem_.X_hat();
}
- double estimated_position() const { return profiled_subsystem_.X_hat(0, 0); }
- double estimated_velocity() const { return profiled_subsystem_.X_hat(1, 0); }
+ double estimated_position() const { return estimated_state()(0, 0); }
+ double estimated_velocity() const { return estimated_state()(1, 0); }
// Corrects the internal state, adjusts limits, and sets nominal goals.
// Returns true if the controller should run.