Added a hybrid KF for the shooter.
Change-Id: If3ba2e6978773aef2e63c9bfeb0cc6e2dec483d5
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 77fd905..7301390 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -16,7 +16,9 @@
class ControlLoopWriter(object):
- def __init__(self, gain_schedule_name, loops, namespaces=None, write_constants=False):
+ def __init__(self, gain_schedule_name, loops, namespaces=None,
+ write_constants=False, plant_type='StateFeedbackPlant',
+ observer_type='StateFeedbackObserver'):
"""Constructs a control loop writer.
Args:
@@ -25,6 +27,8 @@
in order.
namespaces: array[string], a list of names of namespaces to nest in
order. If None, the default will be used.
+ plant_type: string, The C++ type of the plant.
+ observer_type: string, The C++ type of the observer.
"""
self._gain_schedule_name = gain_schedule_name
self._loops = loops
@@ -40,6 +44,8 @@
['} // namespace %s' % name for name in reversed(self._namespaces)])
self._constant_list = []
+ self._plant_type = plant_type
+ self._observer_type = observer_type
def AddConstant(self, constant):
"""Adds a constant to write.
@@ -62,13 +68,17 @@
self.WriteHeader(header_file)
self.WriteCC(os.path.basename(header_file), cc_file)
- def _GenericType(self, typename):
+ def _GenericType(self, typename, extra_args=None):
"""Returns a loop template using typename for the type."""
num_states = self._loops[0].A.shape[0]
num_inputs = self._loops[0].B.shape[1]
num_outputs = self._loops[0].C.shape[0]
- return '%s<%d, %d, %d>' % (
- typename, num_states, num_inputs, num_outputs)
+ if extra_args is not None:
+ extra_args = ', ' + extra_args
+ else:
+ extra_args = ''
+ return '%s<%d, %d, %d%s>' % (
+ typename, num_states, num_inputs, num_outputs, extra_args)
def _ControllerType(self):
"""Returns a template name for StateFeedbackController."""
@@ -76,19 +86,20 @@
def _ObserverType(self):
"""Returns a template name for StateFeedbackObserver."""
- return self._GenericType('StateFeedbackObserver')
+ return self._GenericType(self._observer_type)
def _LoopType(self):
"""Returns a template name for StateFeedbackLoop."""
- return self._GenericType('StateFeedbackLoop')
+ extra_args = '%s, %s' % (self._PlantType(), self._ObserverType())
+ return self._GenericType('StateFeedbackLoop', extra_args)
def _PlantType(self):
"""Returns a template name for StateFeedbackPlant."""
- return self._GenericType('StateFeedbackPlant')
+ return self._GenericType(self._plant_type)
def _PlantCoeffType(self):
"""Returns a template name for StateFeedbackPlantCoefficients."""
- return self._GenericType('StateFeedbackPlantCoefficients')
+ return self._GenericType(self._plant_type + 'Coefficients')
def _ControllerCoeffType(self):
"""Returns a template name for StateFeedbackControllerCoefficients."""
@@ -96,7 +107,7 @@
def _ObserverCoeffType(self):
"""Returns a template name for StateFeedbackObserverCoefficients."""
- return self._GenericType('StateFeedbackObserverCoefficients')
+ return self._GenericType(self._observer_type + 'Coefficients')
def WriteHeader(self, header_file, double_appendage=False, MoI_ratio=0.0):
"""Writes the header file to the file named header_file.
@@ -116,11 +127,11 @@
fd.write('\n\n')
for loop in self._loops:
- fd.write(loop.DumpPlantHeader())
+ fd.write(loop.DumpPlantHeader(self._PlantCoeffType()))
fd.write('\n')
fd.write(loop.DumpControllerHeader())
fd.write('\n')
- fd.write(loop.DumpObserverHeader())
+ fd.write(loop.DumpObserverHeader(self._ObserverCoeffType()))
fd.write('\n')
fd.write('%s Make%sPlant();\n\n' %
@@ -152,7 +163,7 @@
fd.write(self._namespace_start)
fd.write('\n\n')
for loop in self._loops:
- fd.write(loop.DumpPlant())
+ fd.write(loop.DumpPlant(self._PlantCoeffType()))
fd.write('\n')
for loop in self._loops:
@@ -160,7 +171,7 @@
fd.write('\n')
for loop in self._loops:
- fd.write(loop.DumpObserver())
+ fd.write(loop.DumpObserver(self._ObserverCoeffType()))
fd.write('\n')
fd.write('%s Make%sPlant() {\n' %
@@ -292,44 +303,45 @@
return ''.join(ans)
- def DumpPlantHeader(self):
+ def DumpPlantHeader(self, plant_coefficient_type):
"""Writes out a c++ header declaration which will create a Plant object.
Returns:
string, The header declaration for the function.
"""
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- return 'StateFeedbackPlantCoefficients<%d, %d, %d> Make%sPlantCoefficients();\n' % (
- num_states, num_inputs, num_outputs, self._name)
+ return '%s Make%sPlantCoefficients();\n' % (
+ plant_coefficient_type, self._name)
- def DumpPlant(self):
+ def DumpPlant(self, plant_coefficient_type):
"""Writes out a c++ function which will create a PlantCoefficients object.
Returns:
string, The function which will create the object.
"""
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- ans = ['StateFeedbackPlantCoefficients<%d, %d, %d>'
- ' Make%sPlantCoefficients() {\n' % (
- num_states, num_inputs, num_outputs, self._name)]
+ ans = ['%s Make%sPlantCoefficients() {\n' % (
+ plant_coefficient_type, self._name)]
- ans.append(self._DumpMatrix('A', self.A))
- ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
- ans.append(self._DumpMatrix('A_continuous', self.A_continuous))
- ans.append(self._DumpMatrix('B', self.B))
- ans.append(self._DumpMatrix('B_continuous', self.B_continuous))
ans.append(self._DumpMatrix('C', self.C))
ans.append(self._DumpMatrix('D', self.D))
ans.append(self._DumpMatrix('U_max', self.U_max))
ans.append(self._DumpMatrix('U_min', self.U_min))
- ans.append(' return StateFeedbackPlantCoefficients<%d, %d, %d>'
- '(A, A_inv, A_continuous, B, B_continuous, C, D, U_max, U_min);\n' % (
- num_states, num_inputs, num_outputs))
+ if plant_coefficient_type.startswith('StateFeedbackPlant'):
+ ans.append(self._DumpMatrix('A', self.A))
+ ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
+ ans.append(self._DumpMatrix('B', self.B))
+ ans.append(' return %s'
+ '(A, A_inv, B, C, D, U_max, U_min);\n' % (
+ plant_coefficient_type))
+ elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'):
+ ans.append(self._DumpMatrix('A_continuous', self.A_continuous))
+ ans.append(self._DumpMatrix('B_continuous', self.B_continuous))
+ ans.append(' return %s'
+ '(A_continuous, B_continuous, C, D, U_max, U_min);\n' % (
+ plant_coefficient_type))
+ else:
+ glog.fatal('Unsupported plant type %s', plant_coefficient_type)
+
ans.append('}\n')
return ''.join(ans)
@@ -381,33 +393,62 @@
ans.append('}\n')
return ''.join(ans)
- def DumpObserverHeader(self):
+ def DumpObserverHeader(self, observer_coefficient_type):
"""Writes out a c++ header declaration which will create a Observer object.
Returns:
string, The header declaration for the function.
"""
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- return 'StateFeedbackObserverCoefficients<%d, %d, %d> %s;\n' % (
- num_states, num_inputs, num_outputs, self.ObserverFunction())
+ return '%s %s;\n' % (
+ observer_coefficient_type, self.ObserverFunction())
- def DumpObserver(self):
+ def DumpObserver(self, observer_coefficient_type):
"""Returns a c++ function which will create a Observer object.
Returns:
string, The function which will create the object.
"""
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- ans = ['StateFeedbackObserverCoefficients<%d, %d, %d> %s {\n' % (
- num_states, num_inputs, num_outputs, self.ObserverFunction())]
+ ans = ['%s %s {\n' % (
+ observer_coefficient_type, self.ObserverFunction())]
- ans.append(self._DumpMatrix('L', self.L))
+ if observer_coefficient_type.startswith('StateFeedbackObserver'):
+ ans.append(self._DumpMatrix('L', self.L))
+ ans.append(' return %s(L);\n' % (observer_coefficient_type,))
+ elif observer_coefficient_type.startswith('HybridKalman'):
+ ans.append(self._DumpMatrix('Q_continuous', self.Q_continuous))
+ ans.append(self._DumpMatrix('R_continuous', self.R_continuous))
+ ans.append(self._DumpMatrix('P_steady_state', self.P_steady_state))
+ ans.append(' return %s(Q_continuous, R_continuous, P_steady_state);\n' % (
+ observer_coefficient_type,))
- ans.append(' return StateFeedbackObserverCoefficients<%d, %d, %d>'
- '(L);\n' % (num_states, num_inputs, num_outputs))
ans.append('}\n')
return ''.join(ans)
+
+class HybridControlLoop(ControlLoop):
+ def __init__(self, name):
+ super(HybridControlLoop, self).__init__(name=name)
+
+ def Discritize(self, dt):
+ [self.A, self.B, self.Q, self.R] = \
+ controls.kalmd(self.A_continuous, self.B_continuous,
+ self.Q_continuous, self.R_continuous, dt)
+
+ def PredictHybridObserver(self, U, dt):
+ self.Discritize(dt)
+ self.X_hat = self.A * self.X_hat + self.B * U
+ self.P = (self.A * self.P * self.A.T + self.Q)
+
+ def CorrectHybridObserver(self, U):
+ Y_bar = self.Y - self.C * self.X_hat
+ C_t = self.C.T
+ S = self.C * self.P * C_t + self.R
+ self.KalmanGain = self.P * C_t * numpy.linalg.inv(S)
+ self.X_hat = self.X_hat + self.KalmanGain * Y_bar
+ self.P = (numpy.eye(len(self.A)) - self.KalmanGain * self.C) * self.P
+
+ def InitializeState(self):
+ super(HybridControlLoop, self).InitializeState()
+ if hasattr(self, 'Q_steady_state'):
+ self.P = self.Q_steady_state
+ else:
+ self.P = numpy.matrix(numpy.zeros((self.A.shape[0], self.A.shape[0])))
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index 211b478..7b0317d 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -151,6 +151,49 @@
return K, P
+
+def kalmd(A_continuous, B_continuous, Q_continuous, R_continuous, dt):
+ """Converts a continuous time kalman filter to discrete time.
+
+ Args:
+ A_continuous: The A continuous matrix
+ B_continuous: the B continuous matrix
+ Q_continuous: The continuous cost matrix
+ R_continuous: The R continuous matrix
+ dt: Timestep
+
+ The math for this is from:
+ https://www.mathworks.com/help/control/ref/kalmd.html
+
+ Returns:
+ The discrete matrices of A, B, Q, and R.
+ """
+ # TODO(austin): Verify that the dimensions make sense.
+ number_of_states = A_continuous.shape[0]
+ number_of_inputs = B_continuous.shape[1]
+ M = numpy.zeros((len(A_continuous) + number_of_inputs,
+ len(A_continuous) + number_of_inputs))
+ M[0:number_of_states, 0:number_of_states] = A_continuous
+ M[0:number_of_states, number_of_states:] = B_continuous
+ M_exp = scipy.linalg.expm(M * dt)
+ A_discrete = M_exp[0:number_of_states, 0:number_of_states]
+ B_discrete = numpy.matrix(M_exp[0:number_of_states, number_of_states:])
+ Q_continuous = (Q_continuous + Q_continuous.T) / 2.0
+ R_continuous = (R_continuous + R_continuous.T) / 2.0
+ M = numpy.concatenate((-A_continuous, Q_continuous), axis=1)
+ M = numpy.concatenate(
+ (M, numpy.concatenate((numpy.matrix(
+ numpy.zeros((number_of_states, number_of_states))),
+ numpy.transpose(A_continuous)), axis = 1)), axis = 0)
+ phi = numpy.matrix(scipy.linalg.expm(M*dt))
+ phi12 = phi[0:number_of_states, number_of_states:(2*number_of_states)]
+ phi22 = phi[number_of_states:2*number_of_states, number_of_states:2*number_of_states]
+ Q_discrete = phi22.T * phi12
+ Q_discrete = (Q_discrete + Q_discrete.T) / 2.0
+ R_discrete = R_continuous / dt
+ return (A_discrete, B_discrete, Q_discrete, R_discrete)
+
+
def TwoStateFeedForwards(B, Q):
"""Computes the feed forwards constant for a 2 state controller.