Added the feed forwards constant to StateFeedbackLoop.
Change-Id: Ie222de1f302b0edcf5c8b1fdd7f2958e0de608c3
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 586a375..951a114 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -38,7 +38,7 @@
self._namespace_end = '\n'.join(
['} // namespace %s' % name for name in reversed(self._namespaces)])
-
+
self._constant_list = []
def AddConstant(self, constant):
@@ -330,10 +330,14 @@
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(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
ans.append(' return StateFeedbackController<%d, %d, %d>'
- '(L, K, A_inv, Make%sPlantCoefficients());\n' % (
+ '(L, K, Kff, A_inv, Make%sPlantCoefficients());\n' % (
num_states, num_inputs, num_outputs, self._name))
ans.append('}\n')
return ''.join(ans)
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index e2c6c93..ed6a809 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -149,3 +149,25 @@
P = (I - K * C) * P_prior
return K, P
+
+def TwoStateFeedForwards(B, Q):
+ """Computes the feed forwards constant for a 2 state controller.
+
+ This will take the form U = Kff * (R(n + 1) - A * R(n)), where Kff is the
+ feed-forwards constant. It is important that Kff is *only* computed off
+ the goal and not the feed back terms.
+
+ Args:
+ B: numpy.Matrix[num_states, num_inputs] The B matrix.
+ Q: numpy.Matrix[num_states, num_states] The Q (cost) matrix.
+
+ Returns:
+ numpy.Matrix[num_inputs, num_states]
+ """
+
+ # We want to find the optimal U such that we minimize the tracking cost.
+ # This means that we want to minimize
+ # (B * U - (R(n+1) - A R(n)))^T * Q * (B * U - (R(n+1) - A R(n)))
+ # TODO(austin): This doesn't take into account the cost of U
+
+ return numpy.linalg.inv(B.T * Q * B) * B.T * Q.T
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index d1b24a5..7a04cb9 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -215,6 +215,7 @@
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;
const Eigen::Matrix<double, number_of_states, number_of_states> A_inv;
StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
number_of_outputs> plant;
@@ -222,11 +223,13 @@
StateFeedbackController(
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,
const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv,
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
number_of_outputs> &plant)
: L(L),
K(K),
+ Kff(Kff),
A_inv(A_inv),
plant(plant) {
}
@@ -309,6 +312,10 @@
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;
}