fix confusion between "input" and "output"
The code relied on number_of_inputs and number_of_outputs being
the same, which has always been the case so we never noticed. There are
also tests now to try to catch that happening in the future.
Also, there was an accessor function bug which ended up accessing a
matrix of the wrong size, which the new testing caught.
Change-Id: I31791444349fb42ca9105b21336a308277d3c74e
diff --git a/frc971/control_loops/control_loops.gyp b/frc971/control_loops/control_loops.gyp
index 848a210..1976850 100644
--- a/frc971/control_loops/control_loops.gyp
+++ b/frc971/control_loops/control_loops.gyp
@@ -1,6 +1,17 @@
{
'targets': [
{
+ 'target_name': 'state_feedback_loop_test',
+ 'type': 'executable',
+ 'sources': [
+ 'state_feedback_loop_test.cc',
+ ],
+ 'dependencies': [
+ 'state_feedback_loop',
+ '<(EXTERNALS):gtest',
+ ],
+ },
+ {
'target_name': 'hall_effect_tracker',
'type': 'static_library',
'sources': [
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index eefaa1a..8dcce5a 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -11,6 +11,13 @@
#include "aos/common/logging/logging.h"
#include "aos/common/macros.h"
+// For everything in this file, "inputs" and "outputs" are defined from the
+// perspective of the plant. This means U is an input and Y is an output
+// (because you give the plant U (powers) and it gives you back a Y (sensor
+// values). This is the opposite of what they mean from the perspective of the
+// controller (U is an output because that's what goes to the motors and Y is an
+// input because that's what comes back from the sensors).
+
template <int number_of_states, int number_of_inputs, int number_of_outputs>
class StateFeedbackPlantCoefficients final {
public:
@@ -30,8 +37,8 @@
const Eigen::Matrix<double, number_of_states, number_of_inputs> &B,
const Eigen::Matrix<double, number_of_outputs, number_of_states> &C,
const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D,
- const Eigen::Matrix<double, number_of_outputs, 1> &U_max,
- const Eigen::Matrix<double, number_of_outputs, 1> &U_min)
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_max,
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_min)
: A_(A),
B_(B),
C_(C),
@@ -135,7 +142,7 @@
const Eigen::Matrix<double, number_of_outputs, 1> &Y() const { return Y_; }
double Y(int i, int j) const { return Y()(i, j); }
const Eigen::Matrix<double, number_of_inputs, 1> &U() const { return U_; }
- double U(int i, int j) const { return X()(i, j); }
+ double U(int i, int j) const { return U()(i, j); }
Eigen::Matrix<double, number_of_states, 1> &mutable_X() { return X_; }
double &mutable_X(int i, int j) { return mutable_X()(i, j); }
@@ -169,7 +176,7 @@
// Assert that U is within the hardware range.
virtual void CheckU() {
- for (int i = 0; i < kNumOutputs; ++i) {
+ for (int i = 0; i < kNumInputs; ++i) {
assert(U(i, 0) <= U_max(i, 0) + 0.00001);
assert(U(i, 0) >= U_min(i, 0) - 0.00001);
}
@@ -211,13 +218,13 @@
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
const Eigen::Matrix<double, number_of_states, number_of_outputs> L;
- const Eigen::Matrix<double, number_of_outputs, number_of_states> K;
+ const Eigen::Matrix<double, number_of_inputs, number_of_states> K;
StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
number_of_outputs> plant;
StateFeedbackController(
const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
- const Eigen::Matrix<double, number_of_outputs, number_of_states> &K,
+ const Eigen::Matrix<double, number_of_inputs, number_of_states> &K,
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
number_of_outputs> &plant)
: L(L),
@@ -241,7 +248,7 @@
StateFeedbackLoop(
const Eigen::Matrix<double, number_of_states, number_of_outputs> &L,
- const Eigen::Matrix<double, number_of_outputs, number_of_states> &K,
+ const Eigen::Matrix<double, number_of_inputs, number_of_states> &K,
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
number_of_outputs> &plant)
: controller_index_(0) {
@@ -300,7 +307,7 @@
}
double U_max(int i, int j) const { return U_max()(i, j); }
- const Eigen::Matrix<double, number_of_outputs, number_of_states> &K() const {
+ 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); }
@@ -360,7 +367,7 @@
// If U is outside the hardware range, limit it before the plant tries to use
// it.
virtual void CapU() {
- for (int i = 0; i < kNumOutputs; ++i) {
+ for (int i = 0; i < kNumInputs; ++i) {
if (U(i, 0) > U_max(i, 0)) {
U_(i, 0) = U_max(i, 0);
} else if (U(i, 0) < U_min(i, 0)) {
diff --git a/frc971/control_loops/state_feedback_loop_test.cc b/frc971/control_loops/state_feedback_loop_test.cc
new file mode 100644
index 0000000..33f17b2
--- /dev/null
+++ b/frc971/control_loops/state_feedback_loop_test.cc
@@ -0,0 +1,46 @@
+#include "frc971/control_loops/state_feedback_loop.h"
+
+#include "gtest/gtest.h"
+
+namespace testing {
+
+// Tests that everything compiles and nothing crashes even if
+// number_of_inputs!=number_of_outputs.
+// There used to be lots of bugs in this area.
+TEST(StateFeedbackLoopTest, UnequalSizes) {
+ // In general, most (all?) errors will make these statements either not
+ // compile or have assertion failures at runtime.
+ const StateFeedbackPlantCoefficients<2, 4, 7> coefficients(
+ Eigen::Matrix<double, 2, 2>::Identity(),
+ Eigen::Matrix<double, 2, 4>::Identity(),
+ Eigen::Matrix<double, 7, 2>::Identity(),
+ Eigen::Matrix<double, 7, 4>::Identity(),
+ Eigen::Matrix<double, 4, 1>::Constant(1),
+ Eigen::Matrix<double, 4, 1>::Constant(-1));
+
+ {
+ StateFeedbackPlant<2, 4, 7> plant(
+ {new StateFeedbackPlantCoefficients<2, 4, 7>(coefficients)});
+ plant.Update();
+ plant.Reset();
+ plant.CheckU();
+ }
+ {
+ StateFeedbackLoop<2, 4, 7> test_loop(StateFeedbackController<2, 4, 7>(
+ Eigen::Matrix<double, 2, 7>::Identity(),
+ Eigen::Matrix<double, 4, 2>::Identity(), coefficients));
+ test_loop.Correct(Eigen::Matrix<double, 7, 1>::Identity());
+ test_loop.Update(false);
+ test_loop.CapU();
+ }
+ {
+ StateFeedbackLoop<2, 4, 7> test_loop(
+ Eigen::Matrix<double, 2, 7>::Identity(),
+ Eigen::Matrix<double, 4, 2>::Identity(), coefficients);
+ test_loop.Correct(Eigen::Matrix<double, 7, 1>::Identity());
+ test_loop.Update(false);
+ test_loop.CapU();
+ }
+}
+
+} // namespace testing