Remove dependency on Loop from the observer and plant
We can now create an observer and plant without a loop for the pistol
grip controller.
Change-Id: I144af2938230fb6b5a526268ae733dd3df19fa47
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index 7b81e04..27ad4fb 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -283,48 +283,40 @@
}
Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
- void Reset(StateFeedbackLoop<
- number_of_states, number_of_inputs, number_of_outputs,
- StateFeedbackHybridPlant<number_of_states, number_of_inputs,
- number_of_outputs>,
- HybridKalman> *loop) {
+ void Reset(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs> *plant) {
X_hat_.setZero();
P_ = coefficients().P_steady_state;
- UpdateQR(loop, ::std::chrono::milliseconds(5));
+ UpdateQR(plant, ::std::chrono::milliseconds(5));
}
- void Predict(StateFeedbackLoop<
- number_of_states, number_of_inputs, number_of_outputs,
- StateFeedbackHybridPlant<number_of_states, number_of_inputs,
- number_of_outputs>,
- HybridKalman> *loop,
+ void Predict(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs> *plant,
const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
::std::chrono::nanoseconds dt) {
// Trigger the predict step. This will update A() and B() in the plant.
- mutable_X_hat() = loop->mutable_plant()->Update(X_hat(), new_u, dt);
+ mutable_X_hat() = plant->Update(X_hat(), new_u, dt);
- UpdateQR(loop, dt);
- P_ = loop->plant().A() * P_ * loop->plant().A().transpose() + Q_;
+ UpdateQR(plant, dt);
+ P_ = plant->A() * P_ * plant->A().transpose() + Q_;
}
- void Correct(const StateFeedbackLoop<
- number_of_states, number_of_inputs, number_of_outputs,
- StateFeedbackHybridPlant<number_of_states, number_of_inputs,
- number_of_outputs>,
- HybridKalman> &loop,
- const Eigen::Matrix<double, number_of_inputs, 1> &U,
- const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
+ void Correct(
+ const StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs> &plant,
+ const Eigen::Matrix<double, number_of_inputs, 1> &U,
+ const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
Eigen::Matrix<double, number_of_outputs, 1> Y_bar =
- Y - (loop.plant().C() * X_hat_ + loop.plant().D() * U);
+ Y - (plant.C() * X_hat_ + plant.D() * U);
Eigen::Matrix<double, number_of_outputs, number_of_outputs> S =
- loop.plant().C() * P_ * loop.plant().C().transpose() + R_;
+ plant.C() * P_ * plant.C().transpose() + R_;
Eigen::Matrix<double, number_of_states, number_of_outputs> KalmanGain;
- KalmanGain = (S.transpose().ldlt().solve(
- (P() * loop.plant().C().transpose()).transpose()))
- .transpose();
+ KalmanGain =
+ (S.transpose().ldlt().solve((P() * plant.C().transpose()).transpose()))
+ .transpose();
X_hat_ = X_hat_ + KalmanGain * Y_bar;
- P_ = (loop.plant().coefficients().A_continuous.Identity() -
- KalmanGain * loop.plant().C()) *
+ P_ = (plant.coefficients().A_continuous.Identity() -
+ KalmanGain * plant.C()) *
P();
}
@@ -354,11 +346,8 @@
}
private:
- void UpdateQR(StateFeedbackLoop<
- number_of_states, number_of_inputs, number_of_outputs,
- StateFeedbackHybridPlant<number_of_states, number_of_inputs,
- number_of_outputs>,
- HybridKalman> *loop,
+ void UpdateQR(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs> *plant,
::std::chrono::nanoseconds dt) {
// Now, compute the discrete time Q and R coefficients.
Eigen::Matrix<double, number_of_states, number_of_states> Qtemp =
@@ -374,12 +363,12 @@
M_gain.setZero();
// Set up the matrix M = [[-A, Q], [0, A.T]]
M_gain.template block<number_of_states, number_of_states>(0, 0) =
- -loop->plant().coefficients().A_continuous;
+ -plant->coefficients().A_continuous;
M_gain.template block<number_of_states, number_of_states>(
0, number_of_states) = Qtemp;
M_gain.template block<number_of_states, number_of_states>(
number_of_states, number_of_states) =
- loop->plant().coefficients().A_continuous.transpose();
+ plant->coefficients().A_continuous.transpose();
Eigen::Matrix<double, 2 * number_of_states, 2 *number_of_states> phi =
(M_gain *
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index c9b5528..34a0fcf 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -301,33 +301,24 @@
}
Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
- void Reset(
- StateFeedbackLoop<number_of_states, number_of_inputs, number_of_outputs,
- StateFeedbackPlant<number_of_states, number_of_inputs,
- number_of_outputs>,
- StateFeedbackObserver> * /*loop*/) {
+ void Reset(StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs> * /*loop*/) {
X_hat_.setZero();
}
- void Predict(
- StateFeedbackLoop<number_of_states, number_of_inputs, number_of_outputs,
- StateFeedbackPlant<number_of_states, number_of_inputs,
- number_of_outputs>,
- StateFeedbackObserver> *loop,
- const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
- ::std::chrono::nanoseconds /*dt*/) {
- mutable_X_hat() = loop->plant().Update(X_hat(), new_u);
+ void Predict(StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs> *plant,
+ const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
+ ::std::chrono::nanoseconds /*dt*/) {
+ mutable_X_hat() = plant->Update(X_hat(), new_u);
}
- void Correct(const StateFeedbackLoop<
- number_of_states, number_of_inputs, number_of_outputs,
- StateFeedbackPlant<number_of_states, number_of_inputs,
- number_of_outputs>,
- StateFeedbackObserver> &loop,
+ void Correct(const StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs> &plant,
const Eigen::Matrix<double, number_of_inputs, 1> &U,
const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
- mutable_X_hat() += loop.plant().A_inv() * L() *
- (Y - loop.plant().C() * X_hat() - loop.plant().D() * U);
+ mutable_X_hat() +=
+ plant.A_inv() * L() * (Y - plant.C() * X_hat() - plant.D() * U);
}
// Sets the current controller to be index, clamped to be within range.
@@ -458,7 +449,7 @@
plant_.Reset();
controller_.Reset();
- observer_.Reset(this);
+ observer_.Reset(this->mutable_plant());
}
// If U is outside the hardware range, limit it before the plant tries to use
@@ -475,7 +466,7 @@
// Corrects X_hat given the observation in Y.
void Correct(const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
- observer_.Correct(*this, U(), Y);
+ observer_.Correct(this->plant(), U(), Y);
}
const Eigen::Matrix<double, number_of_states, 1> error() const {
@@ -522,7 +513,7 @@
void UpdateObserver(const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
::std::chrono::nanoseconds dt) {
- observer_.Predict(this, new_u, dt);
+ observer_.Predict(this->mutable_plant(), new_u, dt);
}
// Sets the current controller to be index.