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