Sundry fixes to statespace controllers
1) Instead of taking a pointer to arguments, take a r value reference
and move them. Much more C++ like.
2) Some controllers don't want a fixed C, D, Q, and R. There is no harm
in allowing those to be passed in. Expose that intermediate form.
3) We had uninitialized state variables. Initialize them to something
sane.
Change-Id: I00f90af7bb53bfd3385e3ebfda1c3c31600b7001
Signed-off-by: Austin Schuh <austin.schuh@bluerivertech.com>
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 7680688..3fa138c 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -41,7 +41,7 @@
coefs.emplace_back(new StateFeedbackPlantCoefficients<4, 2, 2, double>(
dt_config.make_drivetrain_loop().plant().coefficients(ii)));
}
- return StateFeedbackPlant<4, 2, 2, double>(&coefs);
+ return StateFeedbackPlant<4, 2, 2, double>(std::move(coefs));
}
} // namespace
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index 6285024..23a3d21 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -63,8 +63,8 @@
StateFeedbackHybridPlant(
::std::vector<::std::unique_ptr<StateFeedbackHybridPlantCoefficients<
number_of_states, number_of_inputs, number_of_outputs>>>
- *coefficients)
- : coefficients_(::std::move(*coefficients)), index_(0) {
+ &&coefficients)
+ : coefficients_(::std::move(coefficients)), index_(0) {
Reset();
}
@@ -106,14 +106,14 @@
Scalar U_max(int i, int j) const { return U_max()(i, j); }
const Eigen::Matrix<Scalar, number_of_states, 1> &X() const { return X_; }
- Scalar X(int i, int j) const { return X()(i, j); }
+ Scalar X(int i) const { return X()(i); }
const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y() const { return Y_; }
- Scalar Y(int i, int j) const { return Y()(i, j); }
+ Scalar Y(int i) const { return Y()(i); }
Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X() { return X_; }
- Scalar &mutable_X(int i, int j) { return mutable_X()(i, j); }
+ Scalar &mutable_X(int i) { return mutable_X()(i); }
Eigen::Matrix<Scalar, number_of_outputs, 1> &mutable_Y() { return Y_; }
- Scalar &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
+ Scalar &mutable_Y(int i) { return mutable_Y()(i); }
const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
number_of_outputs, Scalar>
@@ -247,8 +247,12 @@
explicit HybridKalman(
::std::vector<::std::unique_ptr<HybridKalmanCoefficients<
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
- *observers)
- : coefficients_(::std::move(*observers)) {}
+ &&observers)
+ : coefficients_(::std::move(observers)) {
+ R_.setZero();
+ X_hat_.setZero();
+ P_ = coefficients().P_steady_state;
+ }
HybridKalman(HybridKalman &&other) : index_(other.index_) {
::std::swap(coefficients_, other.coefficients_);
@@ -281,22 +285,37 @@
return X_hat_;
}
Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+ Scalar &mutable_X_hat(int i) { return mutable_X_hat()(i); }
+ Scalar X_hat(int i) const { return X_hat_(i); }
void Reset(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
number_of_outputs> *plant) {
X_hat_.setZero();
P_ = coefficients().P_steady_state;
- UpdateQR(plant, ::std::chrono::milliseconds(5));
+ UpdateQR(plant, coefficients().Q_continuous, coefficients().R_continuous,
+ ::std::chrono::milliseconds(5));
}
void Predict(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
number_of_outputs, Scalar> *plant,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
::std::chrono::nanoseconds dt) {
+ Predict(plant, new_u, dt, coefficients().Q_continuous,
+ coefficients().R_continuous);
+ }
+
+ void Predict(
+ StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs, Scalar> *plant,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
+ ::std::chrono::nanoseconds dt,
+ Eigen::Matrix<Scalar, number_of_states, number_of_states> Q_continuous,
+ Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs>
+ R_continuous) {
// Trigger the predict step. This will update A() and B() in the plant.
mutable_X_hat() = plant->Update(X_hat(), new_u, dt);
- UpdateQR(plant, dt);
+ UpdateQR(plant, Q_continuous, R_continuous, dt);
P_ = plant->A() * P_ * plant->A().transpose() + Q_;
}
@@ -305,18 +324,29 @@
number_of_outputs, Scalar> &plant,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y) {
+ DynamicCorrect(plant.C(), plant.D(), U, Y, R_);
+ }
+
+ // Corrects based on the sensor information available.
+ template <int number_of_measurements>
+ void DynamicCorrect(
+ const Eigen::Matrix<Scalar, number_of_measurements, number_of_states> &C,
+ const Eigen::Matrix<Scalar, number_of_measurements, number_of_inputs> &D,
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
+ const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y,
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> &R) {
Eigen::Matrix<Scalar, number_of_outputs, 1> Y_bar =
- Y - (plant.C() * X_hat_ + plant.D() * U);
+ Y - (C * X_hat_ + D * U);
Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> S =
- plant.C() * P_ * plant.C().transpose() + R_;
+ C * P_ * C.transpose() + R;
Eigen::Matrix<Scalar, number_of_states, number_of_outputs> KalmanGain;
- KalmanGain =
- (S.transpose().ldlt().solve((P() * plant.C().transpose()).transpose()))
- .transpose();
+ KalmanGain = (S.transpose().ldlt().solve((P() * C.transpose()).transpose()))
+ .transpose();
X_hat_ = X_hat_ + KalmanGain * Y_bar;
- P_ = (plant.coefficients().A_continuous.Identity() -
- KalmanGain * plant.C()) *
- P();
+ P_ =
+ (Eigen::Matrix<Scalar, number_of_states, number_of_states>::Identity() -
+ KalmanGain * C) *
+ P();
}
// Sets the current controller to be index, clamped to be within range.
@@ -345,17 +375,17 @@
}
private:
- void UpdateQR(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
- number_of_outputs> *plant,
- ::std::chrono::nanoseconds dt) {
- ::frc971::controls::DiscretizeQ(coefficients().Q_continuous,
- plant->coefficients().A_continuous, dt,
- &Q_);
+ void UpdateQR(
+ StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs> *plant,
+ Eigen::Matrix<Scalar, number_of_states, number_of_states> Q_continuous,
+ Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> R_continuous,
+ ::std::chrono::nanoseconds dt) {
+ frc971::controls::DiscretizeQ(Q_continuous,
+ plant->coefficients().A_continuous, dt, &Q_);
Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> Rtemp =
- (coefficients().R_continuous +
- coefficients().R_continuous.transpose()) /
- static_cast<Scalar>(2.0);
+ (R_continuous + R_continuous.transpose()) / static_cast<Scalar>(2.0);
R_ = Rtemp / ::aos::time::TypedDurationInSeconds<Scalar>(dt);
}
diff --git a/frc971/control_loops/hybrid_state_feedback_loop_test.cc b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
index 13f74fa..c5b0771 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop_test.cc
+++ b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
@@ -84,7 +84,7 @@
plants[0] = ::std::unique_ptr<StateFeedbackHybridPlantCoefficients<3, 1, 1>>(
new StateFeedbackHybridPlantCoefficients<3, 1, 1>(
MakeIntegralShooterPlantCoefficients()));
- return StateFeedbackHybridPlant<3, 1, 1>(&plants);
+ return StateFeedbackHybridPlant<3, 1, 1>(std::move(plants));
}
StateFeedbackController<3, 1, 1> MakeIntegralShooterController() {
@@ -94,7 +94,7 @@
::std::unique_ptr<StateFeedbackControllerCoefficients<3, 1, 1>>(
new StateFeedbackControllerCoefficients<3, 1, 1>(
MakeIntegralShooterControllerCoefficients()));
- return StateFeedbackController<3, 1, 1>(&controllers);
+ return StateFeedbackController<3, 1, 1>(std::move(controllers));
}
HybridKalman<3, 1, 1> MakeIntegralShooterObserver() {
@@ -103,7 +103,7 @@
observers[0] = ::std::unique_ptr<HybridKalmanCoefficients<3, 1, 1>>(
new HybridKalmanCoefficients<3, 1, 1>(
MakeIntegralShooterObserverCoefficients()));
- return HybridKalman<3, 1, 1>(&observers);
+ return HybridKalman<3, 1, 1>(std::move(observers));
}
StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
@@ -134,7 +134,7 @@
v_plant;
v_plant.emplace_back(
new StateFeedbackPlantCoefficients<2, 4, 7>(coefficients));
- StateFeedbackPlant<2, 4, 7> plant(&v_plant);
+ StateFeedbackPlant<2, 4, 7> plant(std::move(v_plant));
plant.Update(Eigen::Matrix<double, 4, 1>::Zero());
plant.Reset();
plant.CheckU(Eigen::Matrix<double, 4, 1>::Zero());
@@ -145,7 +145,7 @@
v_controller.emplace_back(new StateFeedbackControllerCoefficients<2, 4, 7>(
Eigen::Matrix<double, 4, 2>::Identity(),
Eigen::Matrix<double, 4, 2>::Identity()));
- StateFeedbackController<2, 4, 7> controller(&v_controller);
+ StateFeedbackController<2, 4, 7> controller(std::move(v_controller));
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<2, 4, 7>>>
v_observer;
@@ -153,7 +153,7 @@
Eigen::Matrix<double, 2, 7>::Identity(),
Eigen::Matrix<double, 2, 2>::Identity(),
Eigen::Matrix<double, 7, 7>::Identity()));
- StateFeedbackObserver<2, 4, 7> observer(&v_observer);
+ StateFeedbackObserver<2, 4, 7> observer(std::move(v_observer));
StateFeedbackLoop<2, 4, 7> test_loop(
::std::move(plant), ::std::move(controller), ::std::move(observer));
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 74141c1..cea1fdd 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -216,7 +216,7 @@
fd.write(' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
% (index, self._PlantCoeffType(),
self._PlantCoeffType(), loop.PlantFunction()))
- fd.write(' return %s(&plants);\n' % self._PlantType())
+ fd.write(' return %s(std::move(plants));\n' % self._PlantType())
fd.write('}\n\n')
fd.write('%s Make%sController() {\n' % (self._ControllerType(),
@@ -229,7 +229,8 @@
' controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
% (index, self._ControllerCoeffType(),
self._ControllerCoeffType(), loop.ControllerFunction()))
- fd.write(' return %s(&controllers);\n' % self._ControllerType())
+ fd.write(' return %s(std::move(controllers));\n' %
+ self._ControllerType())
fd.write('}\n\n')
fd.write('%s Make%sObserver() {\n' % (self._ObserverType(),
@@ -241,7 +242,8 @@
' observers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
% (index, self._ObserverCoeffType(),
self._ObserverCoeffType(), loop.ObserverFunction()))
- fd.write(' return %s(&observers);\n' % self._ObserverType())
+ fd.write(
+ ' return %s(std::move(observers));\n' % self._ObserverType())
fd.write('}\n\n')
fd.write('%s Make%sLoop() {\n' % (self._LoopType(),
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 4250df2..f591847 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -69,8 +69,8 @@
StateFeedbackPlant(
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
- *coefficients)
- : coefficients_(::std::move(*coefficients)), index_(0) {
+ &&coefficients)
+ : coefficients_(::std::move(coefficients)), index_(0) {
Reset();
}
@@ -222,8 +222,8 @@
explicit StateFeedbackController(
::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
- *controllers)
- : coefficients_(::std::move(*controllers)) {}
+ &&controllers)
+ : coefficients_(::std::move(controllers)) {}
StateFeedbackController(StateFeedbackController &&other)
: index_(other.index_) {
@@ -300,8 +300,8 @@
explicit StateFeedbackObserver(
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
- *observers)
- : coefficients_(::std::move(*observers)) {}
+ &&observers)
+ : coefficients_(::std::move(observers)) {}
StateFeedbackObserver(StateFeedbackObserver &&other)
: X_hat_(other.X_hat_), index_(other.index_) {