Tune flywheels
Switch to hybrid kalman filter, plot voltage error
Change-Id: I9ade9a741aae58bdb3818c23bfe91b18786235f3
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 27a659a..9c36c90 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -284,9 +284,9 @@
def InitializeState(self):
"""Sets X, Y, and X_hat to zero defaults."""
- self.X = numpy.zeros((self.A.shape[0], 1))
+ self.X = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
self.Y = self.C * self.X
- self.X_hat = numpy.zeros((self.A.shape[0], 1))
+ self.X_hat = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
def PlaceControllerPoles(self, poles):
"""Places the controller poles.