Store Q/R in StateFeedbackObserverCoefficients
I want R available to get noise values for use in the EKF. I pull in Q
because I may want it and it doesn't really cost us anything.
Change-Id: I7eae2669333cf8eac99a05d10b1d2cefa305cc2f
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 9aa00df..e3e00b7 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -453,10 +453,17 @@
if observer_coefficient_type.startswith('StateFeedbackObserver'):
if hasattr(self, 'KalmanGain'):
KalmanGain = self.KalmanGain
+ Q = self.Q
+ R = self.R
else:
KalmanGain = numpy.linalg.inv(self.A) * self.L
+ Q = numpy.zeros(self.A.shape)
+ R = numpy.zeros((self.C.shape[0], self.C.shape[0]))
ans.append(self._DumpMatrix('KalmanGain', KalmanGain, scalar_type))
- ans.append(' return %s(KalmanGain);\n' % (observer_coefficient_type,))
+ ans.append(self._DumpMatrix('Q', Q, scalar_type))
+ ans.append(self._DumpMatrix('R', R, scalar_type))
+ ans.append(' return %s(KalmanGain, Q, R);\n'
+ % (observer_coefficient_type,))
elif observer_coefficient_type.startswith('HybridKalman'):
ans.append(self._DumpMatrix('Q_continuous', self.Q_continuous, scalar_type))