Split StatespaceLoop into a Plant, Controller, and Observer.
This doesn't yet move any of the logic out of the Loop.
Change-Id: I2cb0ea6d1a75c7011576ba752c50e512eeff5890
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index 5fc4b17..91c2fc9 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -163,7 +163,7 @@
::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
int min_FF_sum_index;
const double min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
- const double min_K_sum = loop_->K().col(min_FF_sum_index).sum();
+ const double min_K_sum = loop_->controller().K().col(min_FF_sum_index).sum();
const double high_min_FF_sum = FF_high.col(0).sum();
const double adjusted_ff_voltage =
@@ -244,11 +244,13 @@
// Construct a constraint on R by manipulating the constraint on U
::aos::controls::HVPolytope<2, 4, 4> R_poly_hv(
- U_Poly_.static_H() * (loop_->K() + FF),
- U_Poly_.static_k() + U_Poly_.static_H() * loop_->K() * loop_->X_hat(),
- (loop_->K() + FF).inverse() *
- ::aos::controls::ShiftPoints<2, 4>(U_Poly_.StaticVertices(),
- loop_->K() * loop_->X_hat()));
+ U_Poly_.static_H() * (loop_->controller().K() + FF),
+ U_Poly_.static_k() +
+ U_Poly_.static_H() * loop_->controller().K() * loop_->X_hat(),
+ (loop_->controller().K() + FF).inverse() *
+ ::aos::controls::ShiftPoints<2, 4>(
+ U_Poly_.StaticVertices(),
+ loop_->controller().K() * loop_->X_hat()));
// Limit R back inside the box.
loop_->mutable_R() =
@@ -257,7 +259,7 @@
const Eigen::Matrix<double, 2, 1> FF_volts = FF * loop_->R();
const Eigen::Matrix<double, 2, 1> U_ideal =
- loop_->K() * (loop_->R() - loop_->X_hat()) + FF_volts;
+ loop_->controller().K() * (loop_->R() - loop_->X_hat()) + FF_volts;
for (int i = 0; i < 2; i++) {
loop_->mutable_U()[i] = ::aos::Clip(U_ideal[i], -12, 12);
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index b7a68a9..4aeb74b 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -35,9 +35,11 @@
LOG_MATRIX(DEBUG, "U_uncapped", *U);
Eigen::Matrix<double, 2, 2> position_K;
- position_K << kf_->K(0, 0), kf_->K(0, 2), kf_->K(1, 0), kf_->K(1, 2);
+ position_K << kf_->controller().K(0, 0), kf_->controller().K(0, 2),
+ kf_->controller().K(1, 0), kf_->controller().K(1, 2);
Eigen::Matrix<double, 2, 2> velocity_K;
- velocity_K << kf_->K(0, 1), kf_->K(0, 3), kf_->K(1, 1), kf_->K(1, 3);
+ velocity_K << kf_->controller().K(0, 1), kf_->controller().K(0, 3),
+ kf_->controller().K(1, 1), kf_->controller().K(1, 3);
Eigen::Matrix<double, 2, 1> position_error;
position_error << error(0, 0), error(2, 0);
@@ -144,7 +146,7 @@
goal.right_velocity_goal, 0.0, 0.0, 0.0;
use_profile_ =
- !kf_->Kff().isZero(0) &&
+ !kf_->controller().Kff().isZero(0) &&
(goal.linear.max_velocity != 0.0 && goal.linear.max_acceleration != 0.0 &&
goal.angular.max_velocity != 0.0 &&
goal.angular.max_acceleration != 0.0);
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 91e7ae7..4d09bbc 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -259,8 +259,10 @@
status->estimator_state = this->EstimatorState(0);
Eigen::Matrix<double, 3, 1> error = this->controller().error();
- status->position_power = this->controller().K(0, 0) * error(0, 0);
- status->velocity_power = this->controller().K(0, 1) * error(1, 0);
+ status->position_power =
+ this->controller().controller().K(0, 0) * error(0, 0);
+ status->velocity_power =
+ this->controller().controller().K(0, 1) * error(1, 0);
}
template <class ZeroingEstimator>
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index e8900af..77fd905 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -71,8 +71,12 @@
typename, num_states, num_inputs, num_outputs)
def _ControllerType(self):
- """Returns a template name for StateFeedbackControllerConstants."""
- return self._GenericType('StateFeedbackControllerConstants')
+ """Returns a template name for StateFeedbackController."""
+ return self._GenericType('StateFeedbackController')
+
+ def _ObserverType(self):
+ """Returns a template name for StateFeedbackObserver."""
+ return self._GenericType('StateFeedbackObserver')
def _LoopType(self):
"""Returns a template name for StateFeedbackLoop."""
@@ -82,10 +86,18 @@
"""Returns a template name for StateFeedbackPlant."""
return self._GenericType('StateFeedbackPlant')
- def _CoeffType(self):
+ def _PlantCoeffType(self):
"""Returns a template name for StateFeedbackPlantCoefficients."""
return self._GenericType('StateFeedbackPlantCoefficients')
+ def _ControllerCoeffType(self):
+ """Returns a template name for StateFeedbackControllerCoefficients."""
+ return self._GenericType('StateFeedbackControllerCoefficients')
+
+ def _ObserverCoeffType(self):
+ """Returns a template name for StateFeedbackObserverCoefficients."""
+ return self._GenericType('StateFeedbackObserverCoefficients')
+
def WriteHeader(self, header_file, double_appendage=False, MoI_ratio=0.0):
"""Writes the header file to the file named header_file.
Set double_appendage to true in order to include a ratio of
@@ -108,10 +120,18 @@
fd.write('\n')
fd.write(loop.DumpControllerHeader())
fd.write('\n')
+ fd.write(loop.DumpObserverHeader())
+ fd.write('\n')
fd.write('%s Make%sPlant();\n\n' %
(self._PlantType(), self._gain_schedule_name))
+ fd.write('%s Make%sController();\n\n' %
+ (self._ControllerType(), self._gain_schedule_name))
+
+ fd.write('%s Make%sObserver();\n\n' %
+ (self._ObserverType(), self._gain_schedule_name))
+
fd.write('%s Make%sLoop();\n\n' %
(self._LoopType(), self._gain_schedule_name))
@@ -139,27 +159,48 @@
fd.write(loop.DumpController())
fd.write('\n')
+ for loop in self._loops:
+ fd.write(loop.DumpObserver())
+ fd.write('\n')
+
fd.write('%s Make%sPlant() {\n' %
(self._PlantType(), self._gain_schedule_name))
fd.write(' ::std::vector< ::std::unique_ptr<%s>> plants(%d);\n' % (
- self._CoeffType(), len(self._loops)))
+ self._PlantCoeffType(), len(self._loops)))
for index, loop in enumerate(self._loops):
fd.write(' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
- (index, self._CoeffType(), self._CoeffType(),
+ (index, self._PlantCoeffType(), self._PlantCoeffType(),
loop.PlantFunction()))
fd.write(' return %s(&plants);\n' % self._PlantType())
fd.write('}\n\n')
- fd.write('%s Make%sLoop() {\n' %
- (self._LoopType(), self._gain_schedule_name))
+ fd.write('%s Make%sController() {\n' %
+ (self._ControllerType(), self._gain_schedule_name))
fd.write(' ::std::vector< ::std::unique_ptr<%s>> controllers(%d);\n' % (
- self._ControllerType(), len(self._loops)))
+ self._ControllerCoeffType(), len(self._loops)))
for index, loop in enumerate(self._loops):
fd.write(' controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
- (index, self._ControllerType(), self._ControllerType(),
+ (index, self._ControllerCoeffType(), self._ControllerCoeffType(),
loop.ControllerFunction()))
- fd.write(' return %s(Make%sPlant(), &controllers);\n' %
- (self._LoopType(), self._gain_schedule_name))
+ fd.write(' return %s(&controllers);\n' % self._ControllerType())
+ fd.write('}\n\n')
+
+ fd.write('%s Make%sObserver() {\n' %
+ (self._ObserverType(), self._gain_schedule_name))
+ fd.write(' ::std::vector< ::std::unique_ptr<%s>> observers(%d);\n' % (
+ self._ObserverCoeffType(), len(self._loops)))
+ for index, loop in enumerate(self._loops):
+ fd.write(' 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('}\n\n')
+
+ fd.write('%s Make%sLoop() {\n' %
+ (self._LoopType(), self._gain_schedule_name))
+ fd.write(' return %s(Make%sPlant(), Make%sController(), Make%sObserver());\n' %
+ (self._LoopType(), self._gain_schedule_name,
+ self._gain_schedule_name, self._gain_schedule_name))
fd.write('}\n\n')
fd.write(self._namespace_end)
@@ -298,7 +339,11 @@
def ControllerFunction(self):
"""Returns the name of the controller function."""
- return 'Make%sController()' % self._name
+ return 'Make%sControllerCoefficients()' % self._name
+
+ def ObserverFunction(self):
+ """Returns the name of the controller function."""
+ return 'Make%sObserverCoefficients()' % self._name
def DumpControllerHeader(self):
"""Writes out a c++ header declaration which will create a Controller object.
@@ -309,7 +354,7 @@
num_states = self.A.shape[0]
num_inputs = self.B.shape[1]
num_outputs = self.C.shape[0]
- return 'StateFeedbackControllerConstants<%d, %d, %d> %s;\n' % (
+ return 'StateFeedbackControllerCoefficients<%d, %d, %d> %s;\n' % (
num_states, num_inputs, num_outputs, self.ControllerFunction())
def DumpController(self):
@@ -321,18 +366,48 @@
num_states = self.A.shape[0]
num_inputs = self.B.shape[1]
num_outputs = self.C.shape[0]
- ans = ['StateFeedbackControllerConstants<%d, %d, %d> %s {\n' % (
+ ans = ['StateFeedbackControllerCoefficients<%d, %d, %d> %s {\n' % (
num_states, num_inputs, num_outputs, self.ControllerFunction())]
- ans.append(self._DumpMatrix('L', self.L))
ans.append(self._DumpMatrix('K', self.K))
if not hasattr(self, 'Kff'):
self.Kff = numpy.matrix(numpy.zeros(self.K.shape))
ans.append(self._DumpMatrix('Kff', self.Kff))
- ans.append(' return StateFeedbackControllerConstants<%d, %d, %d>'
- '(L, K, Kff);\n' % (
+ ans.append(' return StateFeedbackControllerCoefficients<%d, %d, %d>'
+ '(K, Kff);\n' % (
num_states, num_inputs, num_outputs))
ans.append('}\n')
return ''.join(ans)
+
+ def DumpObserverHeader(self):
+ """Writes out a c++ header declaration which will create a Observer object.
+
+ Returns:
+ string, The header declaration for the function.
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ return 'StateFeedbackObserverCoefficients<%d, %d, %d> %s;\n' % (
+ num_states, num_inputs, num_outputs, self.ObserverFunction())
+
+ def DumpObserver(self):
+ """Returns a c++ function which will create a Observer object.
+
+ Returns:
+ string, The function which will create the object.
+ """
+ num_states = self.A.shape[0]
+ num_inputs = self.B.shape[1]
+ num_outputs = self.C.shape[0]
+ ans = ['StateFeedbackObserverCoefficients<%d, %d, %d> %s {\n' % (
+ num_states, num_inputs, num_outputs, self.ObserverFunction())]
+
+ ans.append(self._DumpMatrix('L', self.L))
+
+ ans.append(' return StateFeedbackObserverCoefficients<%d, %d, %d>'
+ '(L);\n' % (num_states, num_inputs, num_outputs))
+ ans.append('}\n')
+ return ''.join(ans)
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 3ed1a9e..402585a 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -191,22 +191,143 @@
DISALLOW_COPY_AND_ASSIGN(StateFeedbackPlant);
};
-// A Controller is a structure which holds a plant and the K and L matrices.
-// This is designed such that multiple controllers can share one set of state to
-// support gain scheduling easily.
+// A container for all the controller coefficients.
template <int number_of_states, int number_of_inputs, int number_of_outputs>
-struct StateFeedbackControllerConstants final {
+struct StateFeedbackControllerCoefficients final {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
const Eigen::Matrix<double, number_of_inputs, number_of_states> K;
const Eigen::Matrix<double, number_of_inputs, number_of_states> Kff;
- StateFeedbackControllerConstants(
- const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
+ StateFeedbackControllerCoefficients(
const Eigen::Matrix<double, number_of_inputs, number_of_states> &K,
const Eigen::Matrix<double, number_of_inputs, number_of_states> &Kff)
- : L(L), K(K), Kff(Kff) {}
+ : K(K), Kff(Kff) {}
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+class StateFeedbackController {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ explicit StateFeedbackController(
+ ::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>> *controllers)
+ : coefficients_(::std::move(*controllers)) {}
+
+ StateFeedbackController(StateFeedbackController &&other)
+ : index_(other.index_) {
+ ::std::swap(coefficients_, other.coefficients_);
+ }
+
+ const Eigen::Matrix<double, number_of_inputs, number_of_states> &K() const {
+ return coefficients().K;
+ }
+ double K(int i, int j) const { return K()(i, j); }
+ const Eigen::Matrix<double, number_of_inputs, number_of_states> &Kff() const {
+ return coefficients().Kff;
+ }
+ double Kff(int i, int j) const { return Kff()(i, j); }
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> &L() const {
+ return coefficients().L;
+ }
+ double L(int i, int j) const { return L()(i, j); }
+
+ // Sets the current controller to be index, clamped to be within range.
+ void set_index(int index) {
+ if (index < 0) {
+ index_ = 0;
+ } else if (index >= static_cast<int>(coefficients_.size())) {
+ index_ = static_cast<int>(coefficients_.size()) - 1;
+ } else {
+ index_ = index;
+ }
+ }
+
+ int index() const { return index_; }
+
+ const StateFeedbackControllerCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients(int index) const {
+ return *coefficients_[index];
+ }
+
+ const StateFeedbackControllerCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients() const {
+ return *coefficients_[index_];
+ }
+
+ private:
+ int index_ = 0;
+ ::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ coefficients_;
+};
+
+
+// A container for all the observer coefficients.
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+struct StateFeedbackObserverCoefficients final {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
+
+ StateFeedbackObserverCoefficients(
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> &L)
+ : L(L) {}
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+class StateFeedbackObserver {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ explicit StateFeedbackObserver(
+ ::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>> *observers)
+ : coefficients_(::std::move(*observers)) {}
+
+ StateFeedbackObserver(StateFeedbackObserver &&other)
+ : index_(other.index_) {
+ ::std::swap(coefficients_, other.coefficients_);
+ }
+
+ const Eigen::Matrix<double, number_of_states, number_of_outputs> &L() const {
+ return coefficients().L;
+ }
+ double L(int i, int j) const { return L()(i, j); }
+
+ // Sets the current controller to be index, clamped to be within range.
+ void set_index(int index) {
+ if (index < 0) {
+ index_ = 0;
+ } else if (index >= static_cast<int>(coefficients_.size())) {
+ index_ = static_cast<int>(coefficients_.size()) - 1;
+ } else {
+ index_ = index;
+ }
+ }
+
+ int index() const { return index_; }
+
+ const StateFeedbackObserverCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients(int index) const {
+ return *coefficients_[index];
+ }
+
+ const StateFeedbackObserverCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients() const {
+ return *coefficients_[index_];
+ }
+
+ private:
+ int index_ = 0;
+ ::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ coefficients_;
};
template <int number_of_states, int number_of_inputs, int number_of_outputs>
@@ -214,41 +335,35 @@
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- StateFeedbackLoop(
+ explicit StateFeedbackLoop(
StateFeedbackPlant<number_of_states, number_of_inputs, number_of_outputs>
&&plant,
- ::std::vector<::std::unique_ptr<StateFeedbackControllerConstants<
- number_of_states, number_of_inputs, number_of_outputs>>> *controllers)
+ StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs> &&controller,
+ StateFeedbackObserver<number_of_states, number_of_inputs,
+ number_of_outputs> &&observer)
: plant_(::std::move(plant)),
- controllers_(::std::move(*controllers)),
- index_(0) {
+ controller_(::std::move(controller)),
+ observer_(::std::move(observer)) {
Reset();
}
StateFeedbackLoop(StateFeedbackLoop &&other)
- : plant_(::std::move(other.plant_)) {
+ : plant_(::std::move(other.plant_)),
+ controller_(::std::move(other.controller_)),
+ observer_(::std::move(other.observer_)) {
X_hat_.swap(other.X_hat_);
R_.swap(other.R_);
next_R_.swap(other.next_R_);
U_.swap(other.U_);
U_uncapped_.swap(other.U_uncapped_);
ff_U_.swap(other.ff_U_);
- ::std::swap(controllers_, other.controllers_);
- index_ = other.index_;
}
virtual ~StateFeedbackLoop() {}
- const Eigen::Matrix<double, number_of_inputs, number_of_states> &K() const {
- return controller().K;
- }
- double K(int i, int j) const { return K()(i, j); }
- const Eigen::Matrix<double, number_of_inputs, number_of_states> &Kff() const {
- return controller().Kff;
- }
- double Kff(int i, int j) const { return Kff()(i, j); }
const Eigen::Matrix<double, number_of_states, number_of_outputs> &L() const {
- return controller().L;
+ return observer().L();
}
double L(int i, int j) const { return L()(i, j); }
@@ -296,16 +411,16 @@
return plant_;
}
- const StateFeedbackControllerConstants<number_of_states, number_of_inputs,
- number_of_outputs>
+ const StateFeedbackController<number_of_states, number_of_inputs,
+ number_of_outputs>
&controller() const {
- return *controllers_[index_];
+ return controller_;
}
- const StateFeedbackControllerConstants<number_of_states, number_of_inputs,
- number_of_outputs>
- &controller(int index) const {
- return *controllers_[index];
+ const StateFeedbackObserver<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &observer() const {
+ return observer_;
}
void Reset() {
@@ -341,13 +456,15 @@
// Returns the calculated controller power.
virtual const Eigen::Matrix<double, number_of_inputs, 1> ControllerOutput() {
+ // TODO(austin): Should this live in StateSpaceController?
ff_U_ = FeedForward();
- return K() * error() + ff_U_;
+ return controller().K() * error() + ff_U_;
}
// Calculates the feed forwards power.
virtual const Eigen::Matrix<double, number_of_inputs, 1> FeedForward() {
- return Kff() * (next_R() - plant().A() * R());
+ // TODO(austin): Should this live in StateSpaceController?
+ return controller().Kff() * (next_R() - plant().A() * R());
}
// stop_motors is whether or not to output all 0s.
@@ -369,36 +486,33 @@
// Updates R() after any CapU operations happen on U().
void UpdateFFReference() {
ff_U_ -= U_uncapped() - U();
- if (!Kff().isZero(0)) {
+ if (!controller().Kff().isZero(0)) {
R_ = plant().A() * R() + plant().B() * ff_U_;
}
}
void UpdateObserver(const Eigen::Matrix<double, number_of_inputs, 1> &new_u) {
+ // TODO(austin): Should this live in StateSpacePlant?
X_hat_ = plant().A() * X_hat() + plant().B() * new_u;
}
- // Sets the current controller to be index, clamped to be within range.
+ // Sets the current controller to be index.
void set_index(int index) {
- if (index < 0) {
- index_ = 0;
- } else if (index >= static_cast<int>(controllers_.size())) {
- index_ = static_cast<int>(controllers_.size()) - 1;
- } else {
- index_ = index;
- }
+ controller_.set_index(index);
plant_.set_index(index);
}
- int index() const { return index_; }
+ int index() const { return plant_.index(); }
protected:
StateFeedbackPlant<number_of_states, number_of_inputs, number_of_outputs>
plant_;
- ::std::vector<::std::unique_ptr<StateFeedbackControllerConstants<
- number_of_states, number_of_inputs, number_of_outputs>>>
- controllers_;
+ StateFeedbackController<number_of_states, number_of_inputs, number_of_outputs>
+ controller_;
+
+ StateFeedbackObserver<number_of_states, number_of_inputs, number_of_outputs>
+ observer_;
// These are accessible from non-templated subclasses.
static constexpr int kNumStates = number_of_states;
diff --git a/frc971/control_loops/state_feedback_loop_test.cc b/frc971/control_loops/state_feedback_loop_test.cc
index e81f6fd..ebe2871 100644
--- a/frc971/control_loops/state_feedback_loop_test.cc
+++ b/frc971/control_loops/state_feedback_loop_test.cc
@@ -34,13 +34,21 @@
plant.CheckU(Eigen::Matrix<double, 4, 1>::Zero());
// Now build a controller.
- ::std::vector<::std::unique_ptr<StateFeedbackControllerConstants<2, 4, 7>>>
- v_loop;
- v_loop.emplace_back(new StateFeedbackControllerConstants<2, 4, 7>(
- Eigen::Matrix<double, 2, 7>::Identity(),
+ ::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<2, 4, 7>>>
+ v_controller;
+ v_controller.emplace_back(new StateFeedbackControllerCoefficients<2, 4, 7>(
Eigen::Matrix<double, 4, 2>::Identity(),
Eigen::Matrix<double, 4, 2>::Identity()));
- StateFeedbackLoop<2, 4, 7> test_loop(::std::move(plant), &v_loop);
+ StateFeedbackController<2, 4, 7> controller(&v_controller);
+
+ ::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<2, 4, 7>>>
+ v_observer;
+ v_observer.emplace_back(new StateFeedbackObserverCoefficients<2, 4, 7>(
+ Eigen::Matrix<double, 2, 7>::Identity()));
+ StateFeedbackObserver<2, 4, 7> observer(&v_observer);
+
+ StateFeedbackLoop<2, 4, 7> test_loop(
+ ::std::move(plant), ::std::move(controller), ::std::move(observer));
test_loop.Correct(Eigen::Matrix<double, 7, 1>::Identity());
test_loop.Update(false);
test_loop.CapU();