diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index a31390a..2b5a37e 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -9,6 +9,7 @@
 
 
 class AngularSystemParams(object):
+
     def __init__(self,
                  name,
                  motor,
@@ -35,6 +36,7 @@
 
 
 class AngularSystem(control_loop.ControlLoop):
+
     def __init__(self, params, name="AngularSystem"):
         super(AngularSystem, self).__init__(name)
         self.params = params
@@ -45,15 +47,15 @@
         self.G = params.G
 
         # Moment of inertia in kg m^2
-        self.J = params.J + self.motor.motor_inertia / (self.G ** 2.0)
+        self.J = params.J + self.motor.motor_inertia / (self.G**2.0)
 
         # Control loop time step
         self.dt = params.dt
 
         # State is [position, velocity]
         # Input is [Voltage]
-        C1 = self.motor.Kt / (self.G * self.G * self.motor.resistance *
-                              self.J * self.motor.Kv)
+        C1 = self.motor.Kt / (
+            self.G * self.G * self.motor.resistance * self.J * self.motor.Kv)
         C2 = self.motor.Kt / (self.G * self.J * self.motor.resistance)
 
         self.A_continuous = numpy.matrix([[0, 1], [0, -C1]])
@@ -116,6 +118,7 @@
 
 
 class IntegralAngularSystem(AngularSystem):
+
     def __init__(self, params, name="IntegralAngularSystem"):
         super(IntegralAngularSystem, self).__init__(params, name=name)
 
@@ -136,10 +139,9 @@
         self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
                                                    self.B_continuous, self.dt)
 
-        self.Q = numpy.matrix(
-            [[(self.params.kalman_q_pos**2.0), 0.0, 0.0],
-             [0.0, (self.params.kalman_q_vel**2.0), 0.0],
-             [0.0, 0.0, (self.params.kalman_q_voltage**2.0)]])
+        self.Q = numpy.matrix([[(self.params.kalman_q_pos**2.0), 0.0, 0.0],
+                               [0.0, (self.params.kalman_q_vel**2.0), 0.0],
+                               [0.0, 0.0, (self.params.kalman_q_voltage**2.0)]])
 
         self.R = numpy.matrix([[(self.params.kalman_r_position**2.0)]])
 
@@ -197,8 +199,8 @@
 
     vbat = 12.0
 
-    goal = numpy.concatenate(
-        (plant.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
+    goal = numpy.concatenate((plant.X, numpy.matrix(numpy.zeros((1, 1)))),
+                             axis=0)
 
     profile = TrapezoidProfile(plant.dt)
     profile.set_maximum_acceleration(max_acceleration)
@@ -257,8 +259,7 @@
         goal = controller.A * goal + controller.B * ff_U
 
         if U[0, 0] != U_uncapped[0, 0]:
-            profile.MoveCurrentState(
-                numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
+            profile.MoveCurrentState(numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
 
     glog.debug('Time: %f', t_plot[-1])
     glog.debug('goal_error %s', repr(end_goal - goal))
@@ -374,7 +375,8 @@
     loop_writer.AddConstant(
         control_loop.Constant('kOutputRatio', '%f', angular_system.G))
     loop_writer.AddConstant(
-        control_loop.Constant('kFreeSpeed', '%f', angular_system.motor.free_speed))
+        control_loop.Constant('kFreeSpeed', '%f',
+                              angular_system.motor.free_speed))
     loop_writer.Write(plant_files[0], plant_files[1])
 
     integral_angular_system = IntegralAngularSystem(params,
diff --git a/frc971/control_loops/python/cim.py b/frc971/control_loops/python/cim.py
index fc39eb9..8c13eb0 100644
--- a/frc971/control_loops/python/cim.py
+++ b/frc971/control_loops/python/cim.py
@@ -3,44 +3,46 @@
 from frc971.control_loops.python import control_loop
 import numpy
 
+
 class CIM(control_loop.ControlLoop):
-  def __init__(self):
-    super(CIM, self).__init__("CIM")
-    # Stall Torque in N m
-    self.stall_torque = 2.42
-    # Stall Current in Amps
-    self.stall_current = 133
-    # Free Speed in RPM
-    self.free_speed = 4650.0
-    # Free Current in Amps
-    self.free_current = 2.7
-    # Moment of inertia of the CIM in kg m^2
-    self.J = 0.0001
-    # Resistance of the motor, divided by 2 to account for the 2 motors
-    self.resistance = 12.0 / self.stall_current
-    # Motor velocity constant
-    self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
-              (12.0 - self.resistance * self.free_current))
-    # Torque constant
-    self.Kt = self.stall_torque / self.stall_current
-    # Control loop time step
-    self.dt = 0.005
 
-    # State feedback matrices
-    self.A_continuous = numpy.matrix(
-        [[-self.Kt / self.Kv / (self.J * self.resistance)]])
-    self.B_continuous = numpy.matrix(
-        [[self.Kt / (self.J * self.resistance)]])
-    self.C = numpy.matrix([[1]])
-    self.D = numpy.matrix([[0]])
+    def __init__(self):
+        super(CIM, self).__init__("CIM")
+        # Stall Torque in N m
+        self.stall_torque = 2.42
+        # Stall Current in Amps
+        self.stall_current = 133
+        # Free Speed in RPM
+        self.free_speed = 4650.0
+        # Free Current in Amps
+        self.free_current = 2.7
+        # Moment of inertia of the CIM in kg m^2
+        self.J = 0.0001
+        # Resistance of the motor, divided by 2 to account for the 2 motors
+        self.resistance = 12.0 / self.stall_current
+        # Motor velocity constant
+        self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+                   (12.0 - self.resistance * self.free_current))
+        # Torque constant
+        self.Kt = self.stall_torque / self.stall_current
+        # Control loop time step
+        self.dt = 0.005
 
-    self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
-                                               self.B_continuous, self.dt)
+        # State feedback matrices
+        self.A_continuous = numpy.matrix(
+            [[-self.Kt / self.Kv / (self.J * self.resistance)]])
+        self.B_continuous = numpy.matrix(
+            [[self.Kt / (self.J * self.resistance)]])
+        self.C = numpy.matrix([[1]])
+        self.D = numpy.matrix([[0]])
 
-    self.PlaceControllerPoles([0.01])
-    self.PlaceObserverPoles([0.01])
+        self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+                                                   self.B_continuous, self.dt)
 
-    self.U_max = numpy.matrix([[12.0]])
-    self.U_min = numpy.matrix([[-12.0]])
+        self.PlaceControllerPoles([0.01])
+        self.PlaceObserverPoles([0.01])
 
-    self.InitializeState()
+        self.U_max = numpy.matrix([[12.0]])
+        self.U_min = numpy.matrix([[-12.0]])
+
+        self.InitializeState()
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index e3e00b7..ac22821 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -2,548 +2,591 @@
 import numpy
 import os
 
-class Constant(object):
-  def __init__ (self, name, formatt, value):
-    self.name = name
-    self.formatt = formatt
-    self.value = value
-    self.formatToType = {}
-    self.formatToType['%f'] = "double"
-    self.formatToType['%d'] = "int"
 
-  def Render(self, loop_type):
-    typestring = self.formatToType[self.formatt]
-    if loop_type == 'float' and typestring == 'double':
-      typestring = loop_type
-    return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
-        (typestring, self.name, self.value)
+class Constant(object):
+
+    def __init__(self, name, formatt, value):
+        self.name = name
+        self.formatt = formatt
+        self.value = value
+        self.formatToType = {}
+        self.formatToType['%f'] = "double"
+        self.formatToType['%d'] = "int"
+
+    def Render(self, loop_type):
+        typestring = self.formatToType[self.formatt]
+        if loop_type == 'float' and typestring == 'double':
+            typestring = loop_type
+        return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
+            (typestring, self.name, self.value)
 
 
 class ControlLoopWriter(object):
-  def __init__(self, gain_schedule_name, loops, namespaces=None,
-               write_constants=False, plant_type='StateFeedbackPlant',
-               observer_type='StateFeedbackObserver',
-               scalar_type='double'):
-    """Constructs a control loop writer.
 
-    Args:
-      gain_schedule_name: string, Name of the overall controller.
-      loops: array[ControlLoop], a list of control loops to gain schedule
-        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.
-      scalar_type: string, The C++ type of the base scalar.
-    """
-    self._gain_schedule_name = gain_schedule_name
-    self._loops = loops
-    if namespaces:
-      self._namespaces = namespaces
-    else:
-      self._namespaces = ['frc971', 'control_loops']
+    def __init__(self,
+                 gain_schedule_name,
+                 loops,
+                 namespaces=None,
+                 write_constants=False,
+                 plant_type='StateFeedbackPlant',
+                 observer_type='StateFeedbackObserver',
+                 scalar_type='double'):
+        """Constructs a control loop writer.
 
-    self._namespace_start = '\n'.join(
-        ['namespace %s {' % name for name in self._namespaces])
+        Args:
+            gain_schedule_name: string, Name of the overall controller.
+            loops: array[ControlLoop], a list of control loops to gain schedule
+                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.
+            scalar_type: string, The C++ type of the base scalar.
+        """
+        self._gain_schedule_name = gain_schedule_name
+        self._loops = loops
+        if namespaces:
+            self._namespaces = namespaces
+        else:
+            self._namespaces = ['frc971', 'control_loops']
 
-    self._namespace_end = '\n'.join(
-        ['}  // namespace %s' % name for name in reversed(self._namespaces)])
+        self._namespace_start = '\n'.join(
+            ['namespace %s {' % name for name in self._namespaces])
 
-    self._constant_list = []
-    self._plant_type = plant_type
-    self._observer_type = observer_type
-    self._scalar_type = scalar_type
+        self._namespace_end = '\n'.join([
+            '}  // namespace %s' % name for name in reversed(self._namespaces)
+        ])
 
-  def AddConstant(self, constant):
-    """Adds a constant to write.
+        self._constant_list = []
+        self._plant_type = plant_type
+        self._observer_type = observer_type
+        self._scalar_type = scalar_type
 
-    Args:
-      constant: Constant, the constant to add to the header.
-    """
-    self._constant_list.append(constant)
+    def AddConstant(self, constant):
+        """Adds a constant to write.
 
-  def _TopDirectory(self):
-    return self._namespaces[0]
+        Args:
+            constant: Constant, the constant to add to the header.
+        """
+        self._constant_list.append(constant)
 
-  def _HeaderGuard(self, header_file):
-    return ('_'.join([namespace.upper() for namespace in self._namespaces]) + '_' +
-            os.path.basename(header_file).upper()
-                .replace('.', '_').replace('/', '_') + '_')
+    def _TopDirectory(self):
+        return self._namespaces[0]
 
-  def Write(self, header_file, cc_file):
-    """Writes the loops to the specified files."""
-    self.WriteHeader(header_file)
-    self.WriteCC(os.path.basename(header_file), cc_file)
+    def _HeaderGuard(self, header_file):
+        return ('_'.join([namespace.upper() for namespace in self._namespaces])
+                + '_' + os.path.basename(header_file).upper().replace(
+                    '.', '_').replace('/', '_') + '_')
 
-  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]
-    if extra_args is not None:
-      extra_args = ', ' + extra_args
-    else:
-      extra_args = ''
-    if self._scalar_type != 'double':
-      extra_args += ', ' + self._scalar_type
-    return '%s<%d, %d, %d%s>' % (
-        typename, num_states, num_inputs, num_outputs, extra_args)
+    def Write(self, header_file, cc_file):
+        """Writes the loops to the specified files."""
+        self.WriteHeader(header_file)
+        self.WriteCC(os.path.basename(header_file), cc_file)
 
-  def _ControllerType(self):
-    """Returns a template name for StateFeedbackController."""
-    return self._GenericType('StateFeedbackController')
+    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]
+        if extra_args is not None:
+            extra_args = ', ' + extra_args
+        else:
+            extra_args = ''
+        if self._scalar_type != 'double':
+            extra_args += ', ' + self._scalar_type
+        return '%s<%d, %d, %d%s>' % (typename, num_states, num_inputs,
+                                     num_outputs, extra_args)
 
-  def _ObserverType(self):
-    """Returns a template name for StateFeedbackObserver."""
-    return self._GenericType(self._observer_type)
+    def _ControllerType(self):
+        """Returns a template name for StateFeedbackController."""
+        return self._GenericType('StateFeedbackController')
 
-  def _LoopType(self):
-    """Returns a template name for StateFeedbackLoop."""
-    num_states = self._loops[0].A.shape[0]
-    num_inputs = self._loops[0].B.shape[1]
-    num_outputs = self._loops[0].C.shape[0]
+    def _ObserverType(self):
+        """Returns a template name for StateFeedbackObserver."""
+        return self._GenericType(self._observer_type)
 
-    return 'StateFeedbackLoop<%d, %d, %d, %s, %s, %s>' % (
-        num_states,
-        num_inputs,
-        num_outputs, self._scalar_type,
-        self._PlantType(), self._ObserverType())
+    def _LoopType(self):
+        """Returns a template name for StateFeedbackLoop."""
+        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 'StateFeedbackLoop<%d, %d, %d, %s, %s, %s>' % (
+            num_states, num_inputs, num_outputs, self._scalar_type,
+            self._PlantType(), self._ObserverType())
 
-  def _PlantType(self):
-    """Returns a template name for StateFeedbackPlant."""
-    return self._GenericType(self._plant_type)
+    def _PlantType(self):
+        """Returns a template name for StateFeedbackPlant."""
+        return self._GenericType(self._plant_type)
 
-  def _PlantCoeffType(self):
-    """Returns a template name for StateFeedbackPlantCoefficients."""
-    return self._GenericType(self._plant_type + 'Coefficients')
+    def _PlantCoeffType(self):
+        """Returns a template name for StateFeedbackPlantCoefficients."""
+        return self._GenericType(self._plant_type + 'Coefficients')
 
-  def _ControllerCoeffType(self):
-    """Returns a template name for StateFeedbackControllerCoefficients."""
-    return self._GenericType('StateFeedbackControllerCoefficients')
+    def _ControllerCoeffType(self):
+        """Returns a template name for StateFeedbackControllerCoefficients."""
+        return self._GenericType('StateFeedbackControllerCoefficients')
 
-  def _ObserverCoeffType(self):
-    """Returns a template name for StateFeedbackObserverCoefficients."""
-    return self._GenericType(self._observer_type + 'Coefficients')
+    def _ObserverCoeffType(self):
+        """Returns a template name for StateFeedbackObserverCoefficients."""
+        return self._GenericType(self._observer_type + 'Coefficients')
 
-  def WriteHeader(self, header_file):
-    """Writes the header file to the file named header_file."""
-    with open(header_file, 'w') as fd:
-      header_guard = self._HeaderGuard(header_file)
-      fd.write('#ifndef %s\n'
-               '#define %s\n\n' % (header_guard, header_guard))
-      fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
-      if (self._plant_type == 'StateFeedbackHybridPlant' or
-          self._observer_type == 'HybridKalman'):
-        fd.write('#include \"frc971/control_loops/hybrid_state_feedback_loop.h\"\n')
+    def WriteHeader(self, header_file):
+        """Writes the header file to the file named header_file."""
+        with open(header_file, 'w') as fd:
+            header_guard = self._HeaderGuard(header_file)
+            fd.write('#ifndef %s\n'
+                     '#define %s\n\n' % (header_guard, header_guard))
+            fd.write(
+                '#include \"frc971/control_loops/state_feedback_loop.h\"\n')
+            if (self._plant_type == 'StateFeedbackHybridPlant' or
+                    self._observer_type == 'HybridKalman'):
+                fd.write(
+                    '#include \"frc971/control_loops/hybrid_state_feedback_loop.h\"\n'
+                )
 
-      fd.write('\n')
+            fd.write('\n')
 
-      fd.write(self._namespace_start)
+            fd.write(self._namespace_start)
 
-      for const in self._constant_list:
-          fd.write(const.Render(self._scalar_type))
+            for const in self._constant_list:
+                fd.write(const.Render(self._scalar_type))
 
-      fd.write('\n\n')
-      for loop in self._loops:
-        fd.write(loop.DumpPlantHeader(self._PlantCoeffType()))
-        fd.write('\n')
-        fd.write(loop.DumpControllerHeader(self._scalar_type))
-        fd.write('\n')
-        fd.write(loop.DumpObserverHeader(self._ObserverCoeffType()))
-        fd.write('\n')
+            fd.write('\n\n')
+            for loop in self._loops:
+                fd.write(loop.DumpPlantHeader(self._PlantCoeffType()))
+                fd.write('\n')
+                fd.write(loop.DumpControllerHeader(self._scalar_type))
+                fd.write('\n')
+                fd.write(loop.DumpObserverHeader(self._ObserverCoeffType()))
+                fd.write('\n')
 
-      fd.write('%s Make%sPlant();\n\n' %
-               (self._PlantType(), self._gain_schedule_name))
+            fd.write('%s Make%sPlant();\n\n' % (self._PlantType(),
+                                                self._gain_schedule_name))
 
-      fd.write('%s Make%sController();\n\n' %
-               (self._ControllerType(), self._gain_schedule_name))
+            fd.write('%s Make%sController();\n\n' % (self._ControllerType(),
+                                                     self._gain_schedule_name))
 
-      fd.write('%s Make%sObserver();\n\n' %
-               (self._ObserverType(), self._gain_schedule_name))
+            fd.write('%s Make%sObserver();\n\n' % (self._ObserverType(),
+                                                   self._gain_schedule_name))
 
-      fd.write('%s Make%sLoop();\n\n' %
-               (self._LoopType(), self._gain_schedule_name))
+            fd.write('%s Make%sLoop();\n\n' % (self._LoopType(),
+                                               self._gain_schedule_name))
 
-      fd.write(self._namespace_end)
-      fd.write('\n\n')
-      fd.write("#endif  // %s\n" % header_guard)
+            fd.write(self._namespace_end)
+            fd.write('\n\n')
+            fd.write("#endif  // %s\n" % header_guard)
 
-  def WriteCC(self, header_file_name, cc_file):
-    """Writes the cc file to the file named cc_file."""
-    with open(cc_file, 'w') as fd:
-      fd.write('#include \"%s/%s\"\n' %
-               (os.path.join(*self._namespaces), header_file_name))
-      fd.write('\n')
-      fd.write('#include <vector>\n')
-      fd.write('\n')
-      fd.write('#include \"frc971/control_loops/state_feedback_loop.h\"\n')
-      fd.write('\n')
-      fd.write(self._namespace_start)
-      fd.write('\n\n')
-      for loop in self._loops:
-        fd.write(loop.DumpPlant(self._PlantCoeffType(), self._scalar_type))
-        fd.write('\n')
+    def WriteCC(self, header_file_name, cc_file):
+        """Writes the cc file to the file named cc_file."""
+        with open(cc_file, 'w') as fd:
+            fd.write('#include \"%s/%s\"\n' % (os.path.join(*self._namespaces),
+                                               header_file_name))
+            fd.write('\n')
+            fd.write('#include <vector>\n')
+            fd.write('\n')
+            fd.write(
+                '#include \"frc971/control_loops/state_feedback_loop.h\"\n')
+            fd.write('\n')
+            fd.write(self._namespace_start)
+            fd.write('\n\n')
+            for loop in self._loops:
+                fd.write(
+                    loop.DumpPlant(self._PlantCoeffType(), self._scalar_type))
+                fd.write('\n')
 
-      for loop in self._loops:
-        fd.write(loop.DumpController(self._scalar_type))
-        fd.write('\n')
+            for loop in self._loops:
+                fd.write(loop.DumpController(self._scalar_type))
+                fd.write('\n')
 
-      for loop in self._loops:
-        fd.write(loop.DumpObserver(self._ObserverCoeffType(), self._scalar_type))
-        fd.write('\n')
+            for loop in self._loops:
+                fd.write(
+                    loop.DumpObserver(self._ObserverCoeffType(),
+                                      self._scalar_type))
+                fd.write('\n')
 
-      fd.write('%s Make%sPlant() {\n' %
-               (self._PlantType(), self._gain_schedule_name))
-      fd.write('  ::std::vector< ::std::unique_ptr<%s>> plants(%d);\n' % (
-          self._PlantCoeffType(), len(self._loops)))
-      for index, loop in enumerate(self._loops):
-        fd.write('  plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
-                 (index, self._PlantCoeffType(), self._PlantCoeffType(),
-                  loop.PlantFunction()))
-      fd.write('  return %s(&plants);\n' % self._PlantType())
-      fd.write('}\n\n')
+            fd.write('%s Make%sPlant() {\n' % (self._PlantType(),
+                                               self._gain_schedule_name))
+            fd.write('  ::std::vector< ::std::unique_ptr<%s>> plants(%d);\n' %
+                     (self._PlantCoeffType(), len(self._loops)))
+            for index, loop in enumerate(self._loops):
+                fd.write('  plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
+                         (index, self._PlantCoeffType(), self._PlantCoeffType(),
+                          loop.PlantFunction()))
+            fd.write('  return %s(&plants);\n' % self._PlantType())
+            fd.write('}\n\n')
 
-      fd.write('%s Make%sController() {\n' %
-               (self._ControllerType(), self._gain_schedule_name))
-      fd.write('  ::std::vector< ::std::unique_ptr<%s>> controllers(%d);\n' % (
-          self._ControllerCoeffType(), len(self._loops)))
-      for index, loop in enumerate(self._loops):
-        fd.write('  controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
-                 (index, self._ControllerCoeffType(), self._ControllerCoeffType(),
-                  loop.ControllerFunction()))
-      fd.write('  return %s(&controllers);\n' % self._ControllerType())
-      fd.write('}\n\n')
+            fd.write('%s Make%sController() {\n' % (self._ControllerType(),
+                                                    self._gain_schedule_name))
+            fd.write(
+                '  ::std::vector< ::std::unique_ptr<%s>> controllers(%d);\n' %
+                (self._ControllerCoeffType(), len(self._loops)))
+            for index, loop in enumerate(self._loops):
+                fd.write(
+                    '  controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
+                    (index, self._ControllerCoeffType(),
+                     self._ControllerCoeffType(), loop.ControllerFunction()))
+            fd.write('  return %s(&controllers);\n' % self._ControllerType())
+            fd.write('}\n\n')
 
-      fd.write('%s Make%sObserver() {\n' %
-               (self._ObserverType(), self._gain_schedule_name))
-      fd.write('  ::std::vector< ::std::unique_ptr<%s>> observers(%d);\n' % (
-          self._ObserverCoeffType(), len(self._loops)))
-      for index, loop in enumerate(self._loops):
-        fd.write('  observers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' %
-                 (index, self._ObserverCoeffType(), self._ObserverCoeffType(),
-                  loop.ObserverFunction()))
-      fd.write('  return %s(&observers);\n' % self._ObserverType())
-      fd.write('}\n\n')
+            fd.write('%s Make%sObserver() {\n' % (self._ObserverType(),
+                                                  self._gain_schedule_name))
+            fd.write('  ::std::vector< ::std::unique_ptr<%s>> observers(%d);\n'
+                     % (self._ObserverCoeffType(), len(self._loops)))
+            for index, loop in enumerate(self._loops):
+                fd.write(
+                    '  observers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
+                    % (index, self._ObserverCoeffType(),
+                       self._ObserverCoeffType(), loop.ObserverFunction()))
+            fd.write('  return %s(&observers);\n' % self._ObserverType())
+            fd.write('}\n\n')
 
-      fd.write('%s Make%sLoop() {\n' %
-               (self._LoopType(), self._gain_schedule_name))
-      fd.write('  return %s(Make%sPlant(), Make%sController(), Make%sObserver());\n' %
-          (self._LoopType(), self._gain_schedule_name,
-           self._gain_schedule_name, self._gain_schedule_name))
-      fd.write('}\n\n')
+            fd.write('%s Make%sLoop() {\n' % (self._LoopType(),
+                                              self._gain_schedule_name))
+            fd.write(
+                '  return %s(Make%sPlant(), Make%sController(), Make%sObserver());\n'
+                % (self._LoopType(), self._gain_schedule_name,
+                   self._gain_schedule_name, self._gain_schedule_name))
+            fd.write('}\n\n')
 
-      fd.write(self._namespace_end)
-      fd.write('\n')
+            fd.write(self._namespace_end)
+            fd.write('\n')
 
 
 class ControlLoop(object):
-  def __init__(self, name):
-    """Constructs a control loop object.
 
-    Args:
-      name: string, The name of the loop to use when writing the C++ files.
-    """
-    self._name = name
+    def __init__(self, name):
+        """Constructs a control loop object.
 
-  @property
-  def name(self):
-    """Returns the name"""
-    return self._name
+        Args:
+            name: string, The name of the loop to use when writing the C++ files.
+        """
+        self._name = name
 
-  def ContinuousToDiscrete(self, A_continuous, B_continuous, dt):
-    """Calculates the discrete time values for A and B.
+    @property
+    def name(self):
+        """Returns the name"""
+        return self._name
 
-      Args:
-        A_continuous: numpy.matrix, The continuous time A matrix
-        B_continuous: numpy.matrix, The continuous time B matrix
-        dt: float, The time step of the control loop
+    def ContinuousToDiscrete(self, A_continuous, B_continuous, dt):
+        """Calculates the discrete time values for A and B.
 
-      Returns:
-        (A, B), numpy.matrix, the control matricies.
-    """
-    return controls.c2d(A_continuous, B_continuous, dt)
+        Args:
+            A_continuous: numpy.matrix, The continuous time A matrix
+            B_continuous: numpy.matrix, The continuous time B matrix
+            dt: float, The time step of the control loop
 
-  def InitializeState(self):
-    """Sets X, Y, and X_hat to zero defaults."""
-    self.X = numpy.zeros((self.A.shape[0], 1))
-    self.Y = self.C * self.X
-    self.X_hat = numpy.zeros((self.A.shape[0], 1))
+        Returns:
+            (A, B), numpy.matrix, the control matricies.
+        """
+        return controls.c2d(A_continuous, B_continuous, dt)
 
-  def PlaceControllerPoles(self, poles):
-    """Places the controller poles.
+    def InitializeState(self):
+        """Sets X, Y, and X_hat to zero defaults."""
+        self.X = numpy.zeros((self.A.shape[0], 1))
+        self.Y = self.C * self.X
+        self.X_hat = numpy.zeros((self.A.shape[0], 1))
 
-    Args:
-      poles: array, An array of poles.  Must be complex conjegates if they have
-        any imaginary portions.
-    """
-    self.K = controls.dplace(self.A, self.B, poles)
+    def PlaceControllerPoles(self, poles):
+        """Places the controller poles.
 
-  def PlaceObserverPoles(self, poles):
-    """Places the observer poles.
+        Args:
+            poles: array, An array of poles.  Must be complex conjegates if they have
+                any imaginary portions.
+        """
+        self.K = controls.dplace(self.A, self.B, poles)
 
-    Args:
-      poles: array, An array of poles.  Must be complex conjegates if they have
-        any imaginary portions.
-    """
-    self.L = controls.dplace(self.A.T, self.C.T, poles).T
+    def PlaceObserverPoles(self, poles):
+        """Places the observer poles.
 
+        Args:
+            poles: array, An array of poles.  Must be complex conjegates if they have
+                any imaginary portions.
+        """
+        self.L = controls.dplace(self.A.T, self.C.T, poles).T
 
-  def Update(self, U):
-    """Simulates one time step with the provided U."""
-    #U = numpy.clip(U, self.U_min, self.U_max)
-    self.X = self.A * self.X + self.B * U
-    self.Y = self.C * self.X + self.D * U
+    def Update(self, U):
+        """Simulates one time step with the provided U."""
+        #U = numpy.clip(U, self.U_min, self.U_max)
+        self.X = self.A * self.X + self.B * U
+        self.Y = self.C * self.X + self.D * U
 
-  def PredictObserver(self, U):
-    """Runs the predict step of the observer update."""
-    self.X_hat = (self.A * self.X_hat + self.B * U)
+    def PredictObserver(self, U):
+        """Runs the predict step of the observer update."""
+        self.X_hat = (self.A * self.X_hat + self.B * U)
 
-  def CorrectObserver(self, U):
-    """Runs the correct step of the observer update."""
-    if hasattr(self, 'KalmanGain'):
-      KalmanGain = self.KalmanGain
-    else:
-      KalmanGain = numpy.linalg.inv(self.A) * self.L
-    self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat - self.D * U)
+    def CorrectObserver(self, U):
+        """Runs the correct step of the observer update."""
+        if hasattr(self, 'KalmanGain'):
+            KalmanGain = self.KalmanGain
+        else:
+            KalmanGain = numpy.linalg.inv(self.A) * self.L
+        self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat - self.D * U)
 
-  def UpdateObserver(self, U):
-    """Updates the observer given the provided U."""
-    if hasattr(self, 'KalmanGain'):
-      KalmanGain = self.KalmanGain
-    else:
-      KalmanGain =  numpy.linalg.inv(self.A) * self.L
-    self.X_hat = (self.A * self.X_hat + self.B * U +
-                  self.A * KalmanGain * (self.Y - self.C * self.X_hat - self.D * U))
+    def UpdateObserver(self, U):
+        """Updates the observer given the provided U."""
+        if hasattr(self, 'KalmanGain'):
+            KalmanGain = self.KalmanGain
+        else:
+            KalmanGain = numpy.linalg.inv(self.A) * self.L
+        self.X_hat = (self.A * self.X_hat + self.B * U + self.A * KalmanGain *
+                      (self.Y - self.C * self.X_hat - self.D * U))
 
-  def _DumpMatrix(self, matrix_name, matrix, scalar_type):
-    """Dumps the provided matrix into a variable called matrix_name.
+    def _DumpMatrix(self, matrix_name, matrix, scalar_type):
+        """Dumps the provided matrix into a variable called matrix_name.
 
-    Args:
-      matrix_name: string, The variable name to save the matrix to.
-      matrix: The matrix to dump.
-      scalar_type: The C++ type to use for the scalar in the matrix.
+        Args:
+            matrix_name: string, The variable name to save the matrix to.
+            matrix: The matrix to dump.
+            scalar_type: The C++ type to use for the scalar in the matrix.
 
-    Returns:
-      string, The C++ commands required to populate a variable named matrix_name
-        with the contents of matrix.
-    """
-    ans = ['  Eigen::Matrix<%s, %d, %d> %s;\n' % (
-        scalar_type, matrix.shape[0], matrix.shape[1], matrix_name)]
-    for x in xrange(matrix.shape[0]):
-      for y in xrange(matrix.shape[1]):
-        write_type =  repr(matrix[x, y])
-        if scalar_type == 'float':
-          if '.' not in write_type:
-            write_type += '.0'
-          write_type += 'f'
-        ans.append('  %s(%d, %d) = %s;\n' % (matrix_name, x, y, write_type))
+        Returns:
+            string, The C++ commands required to populate a variable named matrix_name
+                with the contents of matrix.
+        """
+        ans = [
+            '  Eigen::Matrix<%s, %d, %d> %s;\n' % (scalar_type, matrix.shape[0],
+                                                   matrix.shape[1], matrix_name)
+        ]
+        for x in xrange(matrix.shape[0]):
+            for y in xrange(matrix.shape[1]):
+                write_type = repr(matrix[x, y])
+                if scalar_type == 'float':
+                    if '.' not in write_type:
+                        write_type += '.0'
+                    write_type += 'f'
+                ans.append(
+                    '  %s(%d, %d) = %s;\n' % (matrix_name, x, y, write_type))
 
-    return ''.join(ans)
+        return ''.join(ans)
 
-  def DumpPlantHeader(self, plant_coefficient_type):
-    """Writes out a c++ header declaration which will create a Plant object.
+    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.
-    """
-    return '%s Make%sPlantCoefficients();\n' % (
-        plant_coefficient_type, self._name)
+        Returns:
+            string, The header declaration for the function.
+        """
+        return '%s Make%sPlantCoefficients();\n' % (plant_coefficient_type,
+                                                    self._name)
 
-  def DumpPlant(self, plant_coefficient_type, scalar_type):
-    """Writes out a c++ function which will create a PlantCoefficients object.
+    def DumpPlant(self, plant_coefficient_type, scalar_type):
+        """Writes out a c++ function which will create a PlantCoefficients object.
 
-    Returns:
-      string, The function which will create the object.
-    """
-    ans = ['%s Make%sPlantCoefficients() {\n' % (
-        plant_coefficient_type, self._name)]
+        Returns:
+            string, The function which will create the object.
+        """
+        ans = [
+            '%s Make%sPlantCoefficients() {\n' % (plant_coefficient_type,
+                                                  self._name)
+        ]
 
-    ans.append(self._DumpMatrix('C', self.C, scalar_type))
-    ans.append(self._DumpMatrix('D', self.D, scalar_type))
-    ans.append(self._DumpMatrix('U_max', self.U_max, scalar_type))
-    ans.append(self._DumpMatrix('U_min', self.U_min, scalar_type))
+        ans.append(self._DumpMatrix('C', self.C, scalar_type))
+        ans.append(self._DumpMatrix('D', self.D, scalar_type))
+        ans.append(self._DumpMatrix('U_max', self.U_max, scalar_type))
+        ans.append(self._DumpMatrix('U_min', self.U_min, scalar_type))
 
-    if plant_coefficient_type.startswith('StateFeedbackPlant'):
-      ans.append(self._DumpMatrix('A', self.A, scalar_type))
-      ans.append(self._DumpMatrix('B', self.B, scalar_type))
-      ans.append('  return %s'
-                 '(A, 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, scalar_type))
-      ans.append(self._DumpMatrix('B_continuous', self.B_continuous, scalar_type))
-      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)
+        if plant_coefficient_type.startswith('StateFeedbackPlant'):
+            ans.append(self._DumpMatrix('A', self.A, scalar_type))
+            ans.append(self._DumpMatrix('B', self.B, scalar_type))
+            ans.append(
+                '  return %s'
+                '(A, 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,
+                                 scalar_type))
+            ans.append(
+                self._DumpMatrix('B_continuous', self.B_continuous,
+                                 scalar_type))
+            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)
+        ans.append('}\n')
+        return ''.join(ans)
 
-  def PlantFunction(self):
-    """Returns the name of the plant coefficient function."""
-    return 'Make%sPlantCoefficients()' % self._name
+    def PlantFunction(self):
+        """Returns the name of the plant coefficient function."""
+        return 'Make%sPlantCoefficients()' % self._name
 
-  def ControllerFunction(self):
-    """Returns the name of the controller function."""
-    return 'Make%sControllerCoefficients()' % self._name
+    def ControllerFunction(self):
+        """Returns the name of the controller function."""
+        return 'Make%sControllerCoefficients()' % self._name
 
-  def ObserverFunction(self):
-    """Returns the name of the controller function."""
-    return 'Make%sObserverCoefficients()' % self._name
+    def ObserverFunction(self):
+        """Returns the name of the controller function."""
+        return 'Make%sObserverCoefficients()' % self._name
 
-  def DumpControllerHeader(self, scalar_type):
-    """Writes out a c++ header declaration which will create a Controller object.
+    def DumpControllerHeader(self, scalar_type):
+        """Writes out a c++ header declaration which will create a Controller 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 'StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s;\n' % (
-        num_states, num_inputs, num_outputs, scalar_type,
-        self.ControllerFunction())
+        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 'StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s;\n' % (
+            num_states, num_inputs, num_outputs, scalar_type,
+            self.ControllerFunction())
 
-  def DumpController(self, scalar_type):
-    """Returns a c++ function which will create a Controller object.
+    def DumpController(self, scalar_type):
+        """Returns a c++ function which will create a Controller 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 = ['StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s {\n' % (
-        num_states, num_inputs, num_outputs, scalar_type,
-        self.ControllerFunction())]
+        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 = [
+            'StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s {\n' %
+            (num_states, num_inputs, num_outputs, scalar_type,
+             self.ControllerFunction())
+        ]
 
-    ans.append(self._DumpMatrix('K', self.K, scalar_type))
-    if not hasattr(self, 'Kff'):
-      self.Kff = numpy.matrix(numpy.zeros(self.K.shape))
+        ans.append(self._DumpMatrix('K', self.K, scalar_type))
+        if not hasattr(self, 'Kff'):
+            self.Kff = numpy.matrix(numpy.zeros(self.K.shape))
 
-    ans.append(self._DumpMatrix('Kff', self.Kff, scalar_type))
+        ans.append(self._DumpMatrix('Kff', self.Kff, scalar_type))
 
-    ans.append('  return StateFeedbackControllerCoefficients<%d, %d, %d, %s>'
-               '(K, Kff);\n' % (
-                   num_states, num_inputs, num_outputs, scalar_type))
-    ans.append('}\n')
-    return ''.join(ans)
+        ans.append(
+            '  return StateFeedbackControllerCoefficients<%d, %d, %d, %s>'
+            '(K, Kff);\n' % (num_states, num_inputs, num_outputs, scalar_type))
+        ans.append('}\n')
+        return ''.join(ans)
 
-  def DumpObserverHeader(self, observer_coefficient_type):
-    """Writes out a c++ header declaration which will create a Observer object.
+    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.
-    """
-    return '%s %s;\n' % (
-        observer_coefficient_type, self.ObserverFunction())
+        Returns:
+            string, The header declaration for the function.
+        """
+        return '%s %s;\n' % (observer_coefficient_type, self.ObserverFunction())
 
-  def DumpObserver(self, observer_coefficient_type, scalar_type):
-    """Returns a c++ function which will create a Observer object.
+    def DumpObserver(self, observer_coefficient_type, scalar_type):
+        """Returns a c++ function which will create a Observer object.
 
-    Returns:
-      string, The function which will create the object.
-    """
-    ans = ['%s %s {\n' % (
-           observer_coefficient_type, self.ObserverFunction())]
+        Returns:
+            string, The function which will create the object.
+        """
+        ans = [
+            '%s %s {\n' % (observer_coefficient_type, self.ObserverFunction())
+        ]
 
-    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(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,))
+        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(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))
-      ans.append(self._DumpMatrix('R_continuous', self.R_continuous, scalar_type))
-      ans.append(self._DumpMatrix('P_steady_state', self.P_steady_state, scalar_type))
-      ans.append('  return %s(Q_continuous, R_continuous, P_steady_state);\n' % (
-          observer_coefficient_type,))
-    else:
-      glog.fatal('Unsupported observer type %s', observer_coefficient_type)
+        elif observer_coefficient_type.startswith('HybridKalman'):
+            ans.append(
+                self._DumpMatrix('Q_continuous', self.Q_continuous,
+                                 scalar_type))
+            ans.append(
+                self._DumpMatrix('R_continuous', self.R_continuous,
+                                 scalar_type))
+            ans.append(
+                self._DumpMatrix('P_steady_state', self.P_steady_state,
+                                 scalar_type))
+            ans.append(
+                '  return %s(Q_continuous, R_continuous, P_steady_state);\n' %
+                (observer_coefficient_type,))
+        else:
+            glog.fatal('Unsupported observer type %s',
+                       observer_coefficient_type)
 
-    ans.append('}\n')
-    return ''.join(ans)
+        ans.append('}\n')
+        return ''.join(ans)
+
 
 class HybridControlLoop(ControlLoop):
-  def __init__(self, name):
-    super(HybridControlLoop, self).__init__(name=name)
 
-  def Discretize(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 __init__(self, name):
+        super(HybridControlLoop, self).__init__(name=name)
 
-  def PredictHybridObserver(self, U, dt):
-    self.Discretize(dt)
-    self.X_hat = self.A * self.X_hat + self.B * U
-    self.P = (self.A * self.P * self.A.T + self.Q)
+    def Discretize(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 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 PredictHybridObserver(self, U, dt):
+        self.Discretize(dt)
+        self.X_hat = self.A * self.X_hat + self.B * U
+        self.P = (self.A * self.P * self.A.T + self.Q)
 
-  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])))
+    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])))
 
 
 class CIM(object):
-  def __init__(self):
-    # Stall Torque in N m
-    self.stall_torque = 2.42
-    # Stall Current in Amps
-    self.stall_current = 133.0
-    # Free Speed in rad/s
-    self.free_speed = 5500.0 / 60.0 * 2.0 * numpy.pi
-    # Free Current in Amps
-    self.free_current = 4.7
-    # Resistance of the motor
-    self.resistance = 12.0 / self.stall_current
-    # Motor velocity constant
-    self.Kv = (self.free_speed / (12.0 - self.resistance * self.free_current))
-    # Torque constant
-    self.Kt = self.stall_torque / self.stall_current
+
+    def __init__(self):
+        # Stall Torque in N m
+        self.stall_torque = 2.42
+        # Stall Current in Amps
+        self.stall_current = 133.0
+        # Free Speed in rad/s
+        self.free_speed = 5500.0 / 60.0 * 2.0 * numpy.pi
+        # Free Current in Amps
+        self.free_current = 4.7
+        # Resistance of the motor
+        self.resistance = 12.0 / self.stall_current
+        # Motor velocity constant
+        self.Kv = (
+            self.free_speed / (12.0 - self.resistance * self.free_current))
+        # Torque constant
+        self.Kt = self.stall_torque / self.stall_current
 
 
 class MiniCIM(object):
-  def __init__(self):
-    # Stall Torque in N m
-    self.stall_torque = 1.41
-    # Stall Current in Amps
-    self.stall_current = 89.0
-    # Free Speed in rad/s
-    self.free_speed = 5840.0 / 60.0 * 2.0 * numpy.pi
-    # Free Current in Amps
-    self.free_current = 3.0
-    # Resistance of the motor
-    self.resistance = 12.0 / self.stall_current
-    # Motor velocity constant
-    self.Kv = (self.free_speed / (12.0 - self.resistance * self.free_current))
-    # Torque constant
-    self.Kt = self.stall_torque / self.stall_current
+
+    def __init__(self):
+        # Stall Torque in N m
+        self.stall_torque = 1.41
+        # Stall Current in Amps
+        self.stall_current = 89.0
+        # Free Speed in rad/s
+        self.free_speed = 5840.0 / 60.0 * 2.0 * numpy.pi
+        # Free Current in Amps
+        self.free_current = 3.0
+        # Resistance of the motor
+        self.resistance = 12.0 / self.stall_current
+        # Motor velocity constant
+        self.Kv = (
+            self.free_speed / (12.0 - self.resistance * self.free_current))
+        # Torque constant
+        self.Kt = self.stall_torque / self.stall_current
 
 
 class NMotor(object):
+
     def __init__(self, motor, n):
         """Gangs together n motors."""
         self.motor = motor
@@ -558,6 +601,7 @@
 
 
 class Vex775Pro(object):
+
     def __init__(self):
         # Stall Torque in N m
         self.stall_torque = 0.71
@@ -570,7 +614,8 @@
         # Resistance of the motor
         self.resistance = 12.0 / self.stall_current
         # Motor velocity constant
-        self.Kv = (self.free_speed / (12.0 - self.resistance * self.free_current))
+        self.Kv = (
+            self.free_speed / (12.0 - self.resistance * self.free_current))
         # Torque constant
         self.Kt = self.stall_torque / self.stall_current
         # Motor inertia in kg m^2
@@ -578,37 +623,40 @@
 
 
 class BAG(object):
-  # BAG motor specs available at http://motors.vex.com/vexpro-motors/bag-motor
-  def __init__(self):
-    # Stall Torque in (N m)
-    self.stall_torque = 0.43
-    # Stall Current in (Amps)
-    self.stall_current = 53.0
-    # Free Speed in (rad/s)
-    self.free_speed = 13180.0 / 60.0 * 2.0 * numpy.pi
-    # Free Current in (Amps)
-    self.free_current = 1.8
-    # Resistance of the motor (Ohms)
-    self.resistance = 12.0 / self.stall_current
-    # Motor velocity constant (radians / (sec * volt))
-    self.Kv = (self.free_speed / (12.0 - self.resistance * self.free_current))
-    # Torque constant (N * m / A)
-    self.Kt = self.stall_torque / self.stall_current
-    # Motor inertia in kg m^2
-    self.motor_inertia = 0.000006
+    # BAG motor specs available at http://motors.vex.com/vexpro-motors/bag-motor
+    def __init__(self):
+        # Stall Torque in (N m)
+        self.stall_torque = 0.43
+        # Stall Current in (Amps)
+        self.stall_current = 53.0
+        # Free Speed in (rad/s)
+        self.free_speed = 13180.0 / 60.0 * 2.0 * numpy.pi
+        # Free Current in (Amps)
+        self.free_current = 1.8
+        # Resistance of the motor (Ohms)
+        self.resistance = 12.0 / self.stall_current
+        # Motor velocity constant (radians / (sec * volt))
+        self.Kv = (
+            self.free_speed / (12.0 - self.resistance * self.free_current))
+        # Torque constant (N * m / A)
+        self.Kt = self.stall_torque / self.stall_current
+        # Motor inertia in kg m^2
+        self.motor_inertia = 0.000006
+
 
 class MN3510(object):
-  def __init__(self):
-    # http://www.robotshop.com/en/t-motor-navigator-mn3510-360kv-brushless-motor.html#Specifications
-    # Free Current in Amps
-    self.free_current = 0.0
-    # Resistance of the motor
-    self.resistance = 0.188
-    # Stall Current in Amps
-    self.stall_current = 14.0 / self.resistance
-    # Motor velocity constant
-    self.Kv = 360.0 / 60.0 * (2.0 * numpy.pi)
-    # Torque constant Nm / A
-    self.Kt = 1.0 / self.Kv
-    # Stall Torque in N m
-    self.stall_torque = self.Kt * self.stall_current
+
+    def __init__(self):
+        # http://www.robotshop.com/en/t-motor-navigator-mn3510-360kv-brushless-motor.html#Specifications
+        # Free Current in Amps
+        self.free_current = 0.0
+        # Resistance of the motor
+        self.resistance = 0.188
+        # Stall Current in Amps
+        self.stall_current = 14.0 / self.resistance
+        # Motor velocity constant
+        self.Kv = 360.0 / 60.0 * (2.0 * numpy.pi)
+        # Torque constant Nm / A
+        self.Kt = 1.0 / self.Kv
+        # Stall Torque in N m
+        self.stall_torque = self.Kt * self.stall_current
diff --git a/frc971/control_loops/python/down_estimator.py b/frc971/control_loops/python/down_estimator.py
index 6db8f34..8efcff6 100644
--- a/frc971/control_loops/python/down_estimator.py
+++ b/frc971/control_loops/python/down_estimator.py
@@ -16,106 +16,108 @@
 
 gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
 
+
 class DownEstimator(control_loop.ControlLoop):
-  def __init__(self, name='DownEstimator'):
-    super(DownEstimator, self).__init__(name)
-    self.dt = 0.005
 
-    # State is [gyro_angle, bias].
-    # U is [gyro_x_velocity].
+    def __init__(self, name='DownEstimator'):
+        super(DownEstimator, self).__init__(name)
+        self.dt = 0.005
 
-    self.A_continuous = numpy.matrix([[0, 1],
-                                      [0, 0]])
+        # State is [gyro_angle, bias].
+        # U is [gyro_x_velocity].
 
-    self.B_continuous = numpy.matrix([[1],
-                                      [0]])
+        self.A_continuous = numpy.matrix([[0, 1], [0, 0]])
 
-    self.A, self.B = self.ContinuousToDiscrete(
-        self.A_continuous, self.B_continuous, self.dt)
+        self.B_continuous = numpy.matrix([[1], [0]])
 
-    q_gyro_noise = 1e-6
-    q_gyro_bias_noise = 1e-3
-    self.Q = numpy.matrix([[1.0 / (q_gyro_noise ** 2.0), 0.0],
-                           [0.0, 1.0 / (q_gyro_bias_noise ** 2.0)]])
+        self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+                                                   self.B_continuous, self.dt)
 
-    r_accelerometer_angle_noise = 5e+7
-    self.R = numpy.matrix([[(r_accelerometer_angle_noise ** 2.0)]])
+        q_gyro_noise = 1e-6
+        q_gyro_bias_noise = 1e-3
+        self.Q = numpy.matrix([[1.0 / (q_gyro_noise**2.0), 0.0],
+                               [0.0, 1.0 / (q_gyro_bias_noise**2.0)]])
 
-    self.C = numpy.matrix([[1.0, 0.0]])
-    self.D = numpy.matrix([[0]])
+        r_accelerometer_angle_noise = 5e+7
+        self.R = numpy.matrix([[(r_accelerometer_angle_noise**2.0)]])
 
-    self.U_max = numpy.matrix([[numpy.pi]])
-    self.U_min = numpy.matrix([[-numpy.pi]])
-    self.K = numpy.matrix(numpy.zeros((1, 2)))
+        self.C = numpy.matrix([[1.0, 0.0]])
+        self.D = numpy.matrix([[0]])
 
-    self.KalmanGain, self.Q_steady = controls.kalman(
-        A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+        self.U_max = numpy.matrix([[numpy.pi]])
+        self.U_min = numpy.matrix([[-numpy.pi]])
+        self.K = numpy.matrix(numpy.zeros((1, 2)))
 
-    self.L = self.A * self.KalmanGain
+        self.KalmanGain, self.Q_steady = controls.kalman(
+            A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
 
-    self.InitializeState()
+        self.L = self.A * self.KalmanGain
 
-  def Update(self, accelerometer_x, accelerometer_y, accelerometer_z, gyro_x):
-    acceleration = math.sqrt(
-        accelerometer_x**2 + accelerometer_y**2 + accelerometer_z**2)
-    if acceleration < 0.9 or acceleration > 1.1:
-      glog.error('bad total acceleration %f' % acceleration)
-      # TODO(Brian): Tune this?
-      return
-    accelerometer_angle = math.atan2(accelerometer_x, accelerometer_z)
-    Z = numpy.matrix([[accelerometer_angle], [gyro_x]])
+        self.InitializeState()
 
-    Y = Z - self.H * self.X_hat
-    S = self.H * self.P * self.H.transpose() + self.R
-    K = self.P * self.H.transpose() * numpy.linalg.inv(S)
-    self.X_hat += K * Y
-    self.P = (numpy.identity(K.shape[0]) - K * self.H) * self.P
+    def Update(self, accelerometer_x, accelerometer_y, accelerometer_z, gyro_x):
+        acceleration = math.sqrt(accelerometer_x**2 + accelerometer_y**2 +
+                                 accelerometer_z**2)
+        if acceleration < 0.9 or acceleration > 1.1:
+            glog.error('bad total acceleration %f' % acceleration)
+            # TODO(Brian): Tune this?
+            return
+        accelerometer_angle = math.atan2(accelerometer_x, accelerometer_z)
+        Z = numpy.matrix([[accelerometer_angle], [gyro_x]])
+
+        Y = Z - self.H * self.X_hat
+        S = self.H * self.P * self.H.transpose() + self.R
+        K = self.P * self.H.transpose() * numpy.linalg.inv(S)
+        self.X_hat += K * Y
+        self.P = (numpy.identity(K.shape[0]) - K * self.H) * self.P
+
 
 def main(argv):
-  argv = FLAGS(argv)
-  glog.init()
+    argv = FLAGS(argv)
+    glog.init()
 
-  estimator = DownEstimator()
+    estimator = DownEstimator()
 
-  if FLAGS.plot:
-    real_angles = [0]
-    real_velocities = [0]
-    estimated_angles = [0]
-    estimated_velocities = [0]
-    for _ in xrange(100):
-      estimator.Predict(0)
-      estimator.Update(numpy.sqrt(2) / 2.0, numpy.sqrt(2) / 2.0, 0, 0)
-      real_angles.append(math.pi / 2)
-      real_velocities.append(0)
-      estimated_angles.append(estimator.X_hat[0, 0])
-      estimated_velocities.append(estimator.X_hat[1, 0])
-    angle = math.pi / 2
-    velocity = 1
-    for i in xrange(100):
-      measured_velocity = velocity + (random.random() - 0.5) * 0.01 + 0.05
-      estimator.Predict(measured_velocity)
-      estimator.Update(math.sin(angle) + (random.random() - 0.5) * 0.02, 0,
-                       math.cos(angle) + (random.random() - 0.5) * 0.02,
-                       measured_velocity)
-      real_angles.append(angle)
-      real_velocities.append(velocity)
-      estimated_angles.append(estimator.X_hat[0, 0])
-      estimated_velocities.append(estimator.X_hat[1, 0])
-      angle += velocity * estimator.dt
-    pylab.plot(range(len(real_angles)), real_angles)
-    pylab.plot(range(len(real_velocities)), real_velocities)
-    pylab.plot(range(len(estimated_angles)), estimated_angles)
-    pylab.plot(range(len(estimated_velocities)), estimated_velocities)
-    pylab.show()
+    if FLAGS.plot:
+        real_angles = [0]
+        real_velocities = [0]
+        estimated_angles = [0]
+        estimated_velocities = [0]
+        for _ in xrange(100):
+            estimator.Predict(0)
+            estimator.Update(numpy.sqrt(2) / 2.0, numpy.sqrt(2) / 2.0, 0, 0)
+            real_angles.append(math.pi / 2)
+            real_velocities.append(0)
+            estimated_angles.append(estimator.X_hat[0, 0])
+            estimated_velocities.append(estimator.X_hat[1, 0])
+        angle = math.pi / 2
+        velocity = 1
+        for i in xrange(100):
+            measured_velocity = velocity + (random.random() - 0.5) * 0.01 + 0.05
+            estimator.Predict(measured_velocity)
+            estimator.Update(
+                math.sin(angle) + (random.random() - 0.5) * 0.02, 0,
+                math.cos(angle) + (random.random() - 0.5) * 0.02,
+                measured_velocity)
+            real_angles.append(angle)
+            real_velocities.append(velocity)
+            estimated_angles.append(estimator.X_hat[0, 0])
+            estimated_velocities.append(estimator.X_hat[1, 0])
+            angle += velocity * estimator.dt
+        pylab.plot(range(len(real_angles)), real_angles)
+        pylab.plot(range(len(real_velocities)), real_velocities)
+        pylab.plot(range(len(estimated_angles)), estimated_angles)
+        pylab.plot(range(len(estimated_velocities)), estimated_velocities)
+        pylab.show()
 
-  if len(argv) != 3:
-    print "Expected .h file name and .cc file name"
-  else:
-    namespaces = ['frc971', 'control_loops', 'drivetrain']
-    kf_loop_writer = control_loop.ControlLoopWriter(
-        "DownEstimator", [estimator],
-        namespaces = namespaces)
-    kf_loop_writer.Write(argv[1], argv[2])
+    if len(argv) != 3:
+        print "Expected .h file name and .cc file name"
+    else:
+        namespaces = ['frc971', 'control_loops', 'drivetrain']
+        kf_loop_writer = control_loop.ControlLoopWriter(
+            "DownEstimator", [estimator], namespaces=namespaces)
+        kf_loop_writer.Write(argv[1], argv[2])
+
 
 if __name__ == '__main__':
-  sys.exit(main(sys.argv))
+    sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index c59cbab..1da85bc 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -7,7 +7,9 @@
 from matplotlib import pylab
 import glog
 
+
 class DrivetrainParams(object):
+
     def __init__(self,
                  J,
                  mass,
@@ -107,6 +109,7 @@
 
 
 class Drivetrain(control_loop.ControlLoop):
+
     def __init__(self,
                  drivetrain_params,
                  name="Drivetrain",
@@ -190,8 +193,8 @@
 
         # Resistance of the motor, divided by the number of motors.
         # Motor velocity constant
-        self.Kv = (self.free_speed /
-                   (12.0 - self.resistance * self.free_current))
+        self.Kv = (
+            self.free_speed / (12.0 - self.resistance * self.free_current))
         # Torque constant
         self.Kt = self.stall_torque / self.stall_current
 
@@ -215,14 +218,11 @@
         # X will be of the format
         # [[positionl], [velocityl], [positionr], velocityr]]
         self.A_continuous = numpy.matrix(
-            [[0, 1, 0, 0],
-             [0, -self.mspl * self.tcl, 0, -self.msnr * self.tcr],
-             [0, 0, 0, 1],
-             [0, -self.msnl * self.tcl, 0, -self.mspr * self.tcr]])
+            [[0, 1, 0, 0], [0, -self.mspl * self.tcl, 0, -self.msnr * self.tcr],
+             [0, 0, 0, 1], [0, -self.msnl * self.tcl, 0,
+                            -self.mspr * self.tcr]])
         self.B_continuous = numpy.matrix(
-            [[0, 0],
-             [self.mspl * self.mpl, self.msnr * self.mpr],
-             [0, 0],
+            [[0, 0], [self.mspl * self.mpl, self.msnr * self.mpr], [0, 0],
              [self.msnl * self.mpl, self.mspr * self.mpr]])
         self.C = numpy.matrix([[1, 0, 0, 0], [0, 0, 1, 0]])
         self.D = numpy.matrix([[0, 0], [0, 0]])
@@ -232,10 +232,10 @@
 
     def BuildDrivetrainController(self, q_pos, q_vel):
         # Tune the LQR controller
-        self.Q = numpy.matrix([[(1.0 / (q_pos**2.0)), 0.0, 0.0,
-                                0.0], [0.0, (1.0 / (q_vel**2.0)), 0.0, 0.0],
-                               [0.0, 0.0, (1.0 / (q_pos**2.0)),
-                                0.0], [0.0, 0.0, 0.0, (1.0 / (q_vel**2.0))]])
+        self.Q = numpy.matrix([[(1.0 / (q_pos**2.0)), 0.0, 0.0, 0.0],
+                               [0.0, (1.0 / (q_vel**2.0)), 0.0, 0.0],
+                               [0.0, 0.0, (1.0 / (q_pos**2.0)), 0.0],
+                               [0.0, 0.0, 0.0, (1.0 / (q_vel**2.0))]])
 
         self.R = numpy.matrix([[(1.0 / (12.0**2.0)), 0.0],
                                [0.0, (1.0 / (12.0**2.0))]])
@@ -254,6 +254,7 @@
 
 
 class KFDrivetrain(Drivetrain):
+
     def __init__(self,
                  drivetrain_params,
                  name="KFDrivetrain",
@@ -291,9 +292,10 @@
         self.A_continuous[0:4, 0:4] = self.unaugmented_A_continuous
 
         if self.force:
-            self.A_continuous[0:4, 4:6] = numpy.matrix(
-                [[0.0, 0.0], [self.mspl, self.msnl], [0.0, 0.0],
-                 [self.msnr, self.mspr]])
+            self.A_continuous[0:4, 4:6] = numpy.matrix([[0.0, 0.0],
+                                                        [self.mspl, self.msnl],
+                                                        [0.0, 0.0],
+                                                        [self.msnr, self.mspr]])
             q_voltage = drivetrain_params.kf_q_voltage * self.mpl
         else:
             self.A_continuous[0:4, 4:6] = self.unaugmented_B_continuous
@@ -307,28 +309,31 @@
                                                    self.B_continuous, self.dt)
 
         if self.has_imu:
-            self.C = numpy.matrix(
-                [[1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0], [
-                    0, -0.5 / drivetrain_params.robot_radius, 0,
-                    0.5 / drivetrain_params.robot_radius, 0, 0, 0
-                ], [0, 0, 0, 0, 0, 0, 0]])
+            self.C = numpy.matrix([[1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0],
+                                   [
+                                       0, -0.5 / drivetrain_params.robot_radius,
+                                       0, 0.5 / drivetrain_params.robot_radius,
+                                       0, 0, 0
+                                   ], [0, 0, 0, 0, 0, 0, 0]])
             gravity = 9.8
             self.C[3, 0:6] = 0.5 * (
-                self.A_continuous[1, 0:6] + self.A_continuous[3, 0:6]
-            ) / gravity
+                self.A_continuous[1, 0:6] + self.A_continuous[3, 0:6]) / gravity
 
-            self.D = numpy.matrix([[0, 0], [0, 0], [0, 0], [
-                0.5 *
-                (self.B_continuous[1, 0] + self.B_continuous[3, 0]) / gravity,
-                0.5 *
-                (self.B_continuous[1, 1] + self.B_continuous[3, 1]) / gravity
-            ]])
+            self.D = numpy.matrix(
+                [[0, 0], [0, 0], [0, 0],
+                 [
+                     0.5 * (self.B_continuous[1, 0] + self.B_continuous[3, 0]) /
+                     gravity,
+                     0.5 * (self.B_continuous[1, 1] + self.B_continuous[3, 1]) /
+                     gravity
+                 ]])
         else:
-            self.C = numpy.matrix(
-                [[1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0], [
-                    0, -0.5 / drivetrain_params.robot_radius, 0,
-                    0.5 / drivetrain_params.robot_radius, 0, 0, 0
-                ]])
+            self.C = numpy.matrix([[1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0],
+                                   [
+                                       0, -0.5 / drivetrain_params.robot_radius,
+                                       0, 0.5 / drivetrain_params.robot_radius,
+                                       0, 0, 0
+                                   ]])
 
             self.D = numpy.matrix([[0, 0], [0, 0], [0, 0]])
 
@@ -337,12 +342,12 @@
         q_encoder_uncertainty = 2.00
 
         self.Q = numpy.matrix(
-            [[(q_pos**2.0), 0.0, 0.0, 0.0, 0.0, 0.0,
-              0.0], [0.0, (q_vel**2.0), 0.0, 0.0, 0.0, 0.0,
-                     0.0], [0.0, 0.0, (q_pos**2.0), 0.0, 0.0, 0.0,
-                            0.0], [0.0, 0.0, 0.0, (q_vel**2.0), 0.0, 0.0, 0.0],
-             [0.0, 0.0, 0.0, 0.0, (q_voltage**2.0), 0.0,
-              0.0], [0.0, 0.0, 0.0, 0.0, 0.0, (q_voltage**2.0), 0.0],
+            [[(q_pos**2.0), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+             [0.0, (q_vel**2.0), 0.0, 0.0, 0.0, 0.0, 0.0],
+             [0.0, 0.0, (q_pos**2.0), 0.0, 0.0, 0.0, 0.0],
+             [0.0, 0.0, 0.0, (q_vel**2.0), 0.0, 0.0, 0.0],
+             [0.0, 0.0, 0.0, 0.0, (q_voltage**2.0), 0.0, 0.0],
+             [0.0, 0.0, 0.0, 0.0, 0.0, (q_voltage**2.0), 0.0],
              [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, (q_encoder_uncertainty**2.0)]])
 
         r_pos = 0.0001
@@ -365,7 +370,8 @@
         # If we don't have an IMU, pad various matrices with zeros so that
         # we can still have 4 measurement outputs.
         if not self.has_imu:
-            self.KalmanGain = numpy.hstack((self.KalmanGain, numpy.matrix(numpy.zeros((7, 1)))))
+            self.KalmanGain = numpy.hstack((self.KalmanGain,
+                                            numpy.matrix(numpy.zeros((7, 1)))))
             self.C = numpy.vstack((self.C, numpy.matrix(numpy.zeros((1, 7)))))
             self.D = numpy.vstack((self.D, numpy.matrix(numpy.zeros((1, 2)))))
             Rtmp = numpy.zeros((4, 4))
@@ -401,8 +407,11 @@
         self.InitializeState()
 
 
-def WriteDrivetrain(drivetrain_files, kf_drivetrain_files, year_namespace,
-                    drivetrain_params, scalar_type='double'):
+def WriteDrivetrain(drivetrain_files,
+                    kf_drivetrain_files,
+                    year_namespace,
+                    drivetrain_params,
+                    scalar_type='double'):
 
     # Write the generated constants out to a file.
     drivetrain_low_low = Drivetrain(
@@ -569,8 +578,7 @@
     pylab.plot(range(200), close_loop_left, label='left position')
     pylab.plot(range(200), close_loop_right, label='right position')
     pylab.suptitle(
-        'Angular Move\nLeft position going to -1 and right position going to 1'
-    )
+        'Angular Move\nLeft position going to -1 and right position going to 1')
     pylab.legend(loc='center right')
     pylab.show()
 
diff --git a/frc971/control_loops/python/haptic_wheel.py b/frc971/control_loops/python/haptic_wheel.py
index 6c88e15..088b204 100755
--- a/frc971/control_loops/python/haptic_wheel.py
+++ b/frc971/control_loops/python/haptic_wheel.py
@@ -15,392 +15,443 @@
 
 gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
 gflags.DEFINE_string('data', None, 'If defined, plot the provided CAN data')
-gflags.DEFINE_bool('rerun_kf', False, 'If true, rerun the KF.  The torque in the data file will be interpreted as the commanded current.')
+gflags.DEFINE_bool(
+    'rerun_kf', False,
+    'If true, rerun the KF.  The torque in the data file will be interpreted as the commanded current.'
+)
+
 
 class SystemParams(object):
-  def __init__(self, J, G, kP, kD, kCompensationTimeconstant, q_pos, q_vel,
-               q_torque, current_limit):
-    self.J = J
-    self.G = G
-    self.q_pos = q_pos
-    self.q_vel = q_vel
-    self.q_torque = q_torque
-    self.kP = kP
-    self.kD = kD
-    self.kCompensationTimeconstant = kCompensationTimeconstant
-    self.r_pos = 0.001
-    self.current_limit = current_limit
 
-    #[15.0, 0.25],
-    #[10.0, 0.2],
-    #[5.0, 0.13],
-    #[3.0, 0.10],
-    #[2.0, 0.08],
-    #[1.0, 0.06],
-    #[0.5, 0.05],
-    #[0.25, 0.025],
+    def __init__(self, J, G, kP, kD, kCompensationTimeconstant, q_pos, q_vel,
+                 q_torque, current_limit):
+        self.J = J
+        self.G = G
+        self.q_pos = q_pos
+        self.q_vel = q_vel
+        self.q_torque = q_torque
+        self.kP = kP
+        self.kD = kD
+        self.kCompensationTimeconstant = kCompensationTimeconstant
+        self.r_pos = 0.001
+        self.current_limit = current_limit
 
-kWheel = SystemParams(J=0.0008,
-                      G=(1.25 + 0.02) / 0.35,
-                      q_pos=0.001,
-                      q_vel=0.20,
-                      q_torque=0.005,
-                      kP=7.0,
-                      kD=0.0,
-                      kCompensationTimeconstant=0.95,
-                      current_limit=4.5)
-kTrigger = SystemParams(J=0.00025,
-                        G=(0.925 * 2.0 + 0.02) / 0.35,
-                        q_pos=0.001,
-                        q_vel=0.1,
-                        q_torque=0.005,
-                        kP=120.0,
-                        kD=1.8,
-                        kCompensationTimeconstant=0.95,
-                        current_limit=3.0)
+        #[15.0, 0.25],
+        #[10.0, 0.2],
+        #[5.0, 0.13],
+        #[3.0, 0.10],
+        #[2.0, 0.08],
+        #[1.0, 0.06],
+        #[0.5, 0.05],
+        #[0.25, 0.025],
+
+
+kWheel = SystemParams(
+    J=0.0008,
+    G=(1.25 + 0.02) / 0.35,
+    q_pos=0.001,
+    q_vel=0.20,
+    q_torque=0.005,
+    kP=7.0,
+    kD=0.0,
+    kCompensationTimeconstant=0.95,
+    current_limit=4.5)
+kTrigger = SystemParams(
+    J=0.00025,
+    G=(0.925 * 2.0 + 0.02) / 0.35,
+    q_pos=0.001,
+    q_vel=0.1,
+    q_torque=0.005,
+    kP=120.0,
+    kD=1.8,
+    kCompensationTimeconstant=0.95,
+    current_limit=3.0)
+
 
 class HapticInput(control_loop.ControlLoop):
-  def __init__(self, params=None, name='HapticInput'):
-    # The defaults are for the steering wheel.
-    super(HapticInput, self).__init__(name)
-    motor = self.motor = control_loop.MN3510()
 
-    # Moment of inertia of the wheel in kg m^2
-    self.J = params.J
+    def __init__(self, params=None, name='HapticInput'):
+        # The defaults are for the steering wheel.
+        super(HapticInput, self).__init__(name)
+        motor = self.motor = control_loop.MN3510()
 
-    # Control loop time step
-    self.dt = 0.001
+        # Moment of inertia of the wheel in kg m^2
+        self.J = params.J
 
-    # Gear ratio from the motor to the input.
-    self.G = params.G
+        # Control loop time step
+        self.dt = 0.001
 
-    self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
-    self.A_continuous[1, 1] = 0
-    self.A_continuous[0, 1] = 1
+        # Gear ratio from the motor to the input.
+        self.G = params.G
 
-    self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
-    self.B_continuous[1, 0] = motor.Kt * self.G / self.J
+        self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
+        self.A_continuous[1, 1] = 0
+        self.A_continuous[0, 1] = 1
 
-    # State feedback matrices
-    # [position, angular velocity]
-    self.C = numpy.matrix([[1.0, 0.0]])
-    self.D = numpy.matrix([[0.0]])
+        self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+        self.B_continuous[1, 0] = motor.Kt * self.G / self.J
 
-    self.A, self.B = self.ContinuousToDiscrete(
-        self.A_continuous, self.B_continuous, self.dt)
+        # State feedback matrices
+        # [position, angular velocity]
+        self.C = numpy.matrix([[1.0, 0.0]])
+        self.D = numpy.matrix([[0.0]])
 
-    self.U_max = numpy.matrix([[2.5]])
-    self.U_min = numpy.matrix([[-2.5]])
+        self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+                                                   self.B_continuous, self.dt)
 
-    self.L = numpy.matrix([[0.0], [0.0]])
-    self.K = numpy.matrix([[0.0, 0.0]])
+        self.U_max = numpy.matrix([[2.5]])
+        self.U_min = numpy.matrix([[-2.5]])
 
-    self.InitializeState()
+        self.L = numpy.matrix([[0.0], [0.0]])
+        self.K = numpy.matrix([[0.0, 0.0]])
+
+        self.InitializeState()
 
 
 class IntegralHapticInput(HapticInput):
-  def __init__(self, params=None, name="IntegralHapticInput"):
-    super(IntegralHapticInput, self).__init__(name=name, params=params)
 
-    self.A_continuous_unaugmented = self.A_continuous
-    self.B_continuous_unaugmented = self.B_continuous
+    def __init__(self, params=None, name="IntegralHapticInput"):
+        super(IntegralHapticInput, self).__init__(name=name, params=params)
 
-    self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
-    self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
-    self.A_continuous[1, 2] = (1 / self.J)
+        self.A_continuous_unaugmented = self.A_continuous
+        self.B_continuous_unaugmented = self.B_continuous
 
-    self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
-    self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+        self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+        self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+        self.A_continuous[1, 2] = (1 / self.J)
 
-    self.C_unaugmented = self.C
-    self.C = numpy.matrix(numpy.zeros((1, 3)))
-    self.C[0:1, 0:2] = self.C_unaugmented
+        self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+        self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
 
-    self.A, self.B = self.ContinuousToDiscrete(
-        self.A_continuous, self.B_continuous, self.dt)
+        self.C_unaugmented = self.C
+        self.C = numpy.matrix(numpy.zeros((1, 3)))
+        self.C[0:1, 0:2] = self.C_unaugmented
 
-    self.Q = numpy.matrix([[(params.q_pos ** 2.0), 0.0, 0.0],
-                           [0.0, (params.q_vel ** 2.0), 0.0],
-                           [0.0, 0.0, (params.q_torque ** 2.0)]])
+        self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+                                                   self.B_continuous, self.dt)
 
-    self.R = numpy.matrix([[(params.r_pos ** 2.0)]])
+        self.Q = numpy.matrix([[(params.q_pos**2.0), 0.0, 0.0],
+                               [0.0, (params.q_vel**2.0), 0.0],
+                               [0.0, 0.0, (params.q_torque**2.0)]])
 
-    self.KalmanGain, self.Q_steady = controls.kalman(
-        A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
-    self.L = self.A * self.KalmanGain
+        self.R = numpy.matrix([[(params.r_pos**2.0)]])
 
-    self.K_unaugmented = self.K
-    self.K = numpy.matrix(numpy.zeros((1, 3)))
-    self.K[0, 0:2] = self.K_unaugmented
-    self.K[0, 2] = 1.0 / (self.motor.Kt / (self.motor.resistance))
+        self.KalmanGain, self.Q_steady = controls.kalman(
+            A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+        self.L = self.A * self.KalmanGain
 
-    self.InitializeState()
+        self.K_unaugmented = self.K
+        self.K = numpy.matrix(numpy.zeros((1, 3)))
+        self.K[0, 0:2] = self.K_unaugmented
+        self.K[0, 2] = 1.0 / (self.motor.Kt / (self.motor.resistance))
+
+        self.InitializeState()
+
 
 def ReadCan(filename):
-  """Reads the candump in filename and returns the 4 fields."""
-  trigger = []
-  trigger_velocity = []
-  trigger_torque = []
-  trigger_current = []
-  wheel = []
-  wheel_velocity = []
-  wheel_torque = []
-  wheel_current = []
+    """Reads the candump in filename and returns the 4 fields."""
+    trigger = []
+    trigger_velocity = []
+    trigger_torque = []
+    trigger_current = []
+    wheel = []
+    wheel_velocity = []
+    wheel_torque = []
+    wheel_current = []
 
-  trigger_request_time = [0.0]
-  trigger_request_current = [0.0]
-  wheel_request_time = [0.0]
-  wheel_request_current = [0.0]
+    trigger_request_time = [0.0]
+    trigger_request_current = [0.0]
+    wheel_request_time = [0.0]
+    wheel_request_current = [0.0]
 
-  with open(filename, 'r') as fd:
-    for line in fd:
-      data = line.split()
-      can_id = int(data[1], 16)
-      if can_id == 0:
-        data = [int(d, 16) for d in data[3:]]
-        trigger.append(((data[0] + (data[1] << 8)) - 32768) / 32768.0)
-        trigger_velocity.append(((data[2] + (data[3] << 8)) - 32768) / 32768.0)
-        trigger_torque.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
-        trigger_current.append(((data[6] + ((data[7] & 0x3f) << 8)) - 8192) / 8192.0)
-      elif can_id == 1:
-        data = [int(d, 16) for d in data[3:]]
-        wheel.append(((data[0] + (data[1] << 8)) - 32768) / 32768.0)
-        wheel_velocity.append(((data[2] + (data[3] << 8)) - 32768) / 32768.0)
-        wheel_torque.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
-        wheel_current.append(((data[6] + ((data[7] & 0x3f) << 8)) - 8192) / 8192.0)
-      elif can_id == 2:
-        data = [int(d, 16) for d in data[3:]]
-        trigger_request_current.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
-        trigger_request_time.append(len(trigger) * 0.001)
-      elif can_id == 3:
-        data = [int(d, 16) for d in data[3:]]
-        wheel_request_current.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
-        wheel_request_time.append(len(wheel) * 0.001)
+    with open(filename, 'r') as fd:
+        for line in fd:
+            data = line.split()
+            can_id = int(data[1], 16)
+            if can_id == 0:
+                data = [int(d, 16) for d in data[3:]]
+                trigger.append(((data[0] + (data[1] << 8)) - 32768) / 32768.0)
+                trigger_velocity.append(
+                    ((data[2] + (data[3] << 8)) - 32768) / 32768.0)
+                trigger_torque.append(
+                    ((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+                trigger_current.append(
+                    ((data[6] + ((data[7] & 0x3f) << 8)) - 8192) / 8192.0)
+            elif can_id == 1:
+                data = [int(d, 16) for d in data[3:]]
+                wheel.append(((data[0] + (data[1] << 8)) - 32768) / 32768.0)
+                wheel_velocity.append(
+                    ((data[2] + (data[3] << 8)) - 32768) / 32768.0)
+                wheel_torque.append(
+                    ((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+                wheel_current.append(
+                    ((data[6] + ((data[7] & 0x3f) << 8)) - 8192) / 8192.0)
+            elif can_id == 2:
+                data = [int(d, 16) for d in data[3:]]
+                trigger_request_current.append(
+                    ((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+                trigger_request_time.append(len(trigger) * 0.001)
+            elif can_id == 3:
+                data = [int(d, 16) for d in data[3:]]
+                wheel_request_current.append(
+                    ((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+                wheel_request_time.append(len(wheel) * 0.001)
 
-  trigger_data_time = numpy.arange(0, len(trigger)) * 0.001
-  wheel_data_time = numpy.arange(0, len(wheel)) * 0.001
+    trigger_data_time = numpy.arange(0, len(trigger)) * 0.001
+    wheel_data_time = numpy.arange(0, len(wheel)) * 0.001
 
-  # Extend out the data in the interpolation table.
-  trigger_request_time.append(trigger_data_time[-1])
-  trigger_request_current.append(trigger_request_current[-1])
-  wheel_request_time.append(wheel_data_time[-1])
-  wheel_request_current.append(wheel_request_current[-1])
+    # Extend out the data in the interpolation table.
+    trigger_request_time.append(trigger_data_time[-1])
+    trigger_request_current.append(trigger_request_current[-1])
+    wheel_request_time.append(wheel_data_time[-1])
+    wheel_request_current.append(wheel_request_current[-1])
 
-  return (trigger_data_time, wheel_data_time, trigger, wheel, trigger_velocity,
-          wheel_velocity, trigger_torque, wheel_torque, trigger_current,
-          wheel_current, trigger_request_time, trigger_request_current,
-          wheel_request_time, wheel_request_current)
+    return (trigger_data_time, wheel_data_time, trigger, wheel,
+            trigger_velocity, wheel_velocity, trigger_torque, wheel_torque,
+            trigger_current, wheel_current, trigger_request_time,
+            trigger_request_current, wheel_request_time, wheel_request_current)
 
-def rerun_and_plot_kf(data_time, data_radians, data_current, data_request_current,
-                      params, run_correct=True):
-  kf_velocity = []
-  dt_velocity = []
-  kf_position = []
-  adjusted_position = []
-  last_angle = None
-  haptic_observer = IntegralHapticInput(params=params)
 
-  # Parameter sweep J.
-  num_kf = 1
-  min_J = max_J = params.J
+def rerun_and_plot_kf(data_time,
+                      data_radians,
+                      data_current,
+                      data_request_current,
+                      params,
+                      run_correct=True):
+    kf_velocity = []
+    dt_velocity = []
+    kf_position = []
+    adjusted_position = []
+    last_angle = None
+    haptic_observer = IntegralHapticInput(params=params)
 
-  # J = 0.0022
-  #num_kf = 15
-  #min_J = min_J / 2.0
-  #max_J = max_J * 2.0
-  initial_velocity = (data_radians[1] - data_radians[0]) * 1000.0
+    # Parameter sweep J.
+    num_kf = 1
+    min_J = max_J = params.J
 
-  def DupParamsWithJ(params, J):
-    p = copy.copy(params)
-    p.J = J
-    return p
-  haptic_observers = [IntegralHapticInput(params=DupParamsWithJ(params, j)) for j in
-                      numpy.logspace(numpy.log10(min_J),
-                                     numpy.log10(max_J), num=num_kf)]
-  # Initialize all the KF's.
-  haptic_observer.X_hat[1, 0] = initial_velocity
-  haptic_observer.X_hat[0, 0] = data_radians[0]
-  for observer in haptic_observers:
-    observer.X_hat[1, 0] = initial_velocity
-    observer.X_hat[0, 0] = data_radians[0]
+    # J = 0.0022
+    #num_kf = 15
+    #min_J = min_J / 2.0
+    #max_J = max_J * 2.0
+    initial_velocity = (data_radians[1] - data_radians[0]) * 1000.0
 
-  last_request_current = data_request_current[0]
-  kf_torques = [[] for i in xrange(num_kf)]
-  for angle, current, request_current in zip(data_radians, data_current,
-                                             data_request_current):
-    # Predict and correct all the parameter swept observers.
-    for i, observer in enumerate(haptic_observers):
-      observer.Y = numpy.matrix([[angle]])
-      if run_correct:
-        observer.CorrectObserver(numpy.matrix([[current]]))
-      kf_torques[i].append(-observer.X_hat[2, 0])
-      observer.PredictObserver(numpy.matrix([[current]]))
-      observer.PredictObserver(numpy.matrix([[current]]))
+    def DupParamsWithJ(params, J):
+        p = copy.copy(params)
+        p.J = J
+        return p
 
-    # Predict and correct the main observer.
-    haptic_observer.Y = numpy.matrix([[angle]])
-    if run_correct:
-      haptic_observer.CorrectObserver(numpy.matrix([[current]]))
-    kf_position.append(haptic_observer.X_hat[0, 0])
-    adjusted_position.append(kf_position[-1] - last_request_current / params.kP)
-    last_request_current = last_request_current * params.kCompensationTimeconstant + request_current * (1.0 - params.kCompensationTimeconstant)
-    kf_velocity.append(haptic_observer.X_hat[1, 0])
-    if last_angle is None:
-      last_angle = angle
-    dt_velocity.append((angle - last_angle) / 0.001)
+    haptic_observers = [
+        IntegralHapticInput(params=DupParamsWithJ(params, j))
+        for j in numpy.logspace(
+            numpy.log10(min_J), numpy.log10(max_J), num=num_kf)
+    ]
+    # Initialize all the KF's.
+    haptic_observer.X_hat[1, 0] = initial_velocity
+    haptic_observer.X_hat[0, 0] = data_radians[0]
+    for observer in haptic_observers:
+        observer.X_hat[1, 0] = initial_velocity
+        observer.X_hat[0, 0] = data_radians[0]
 
-    haptic_observer.PredictObserver(numpy.matrix([[current]]))
-    last_angle = angle
+    last_request_current = data_request_current[0]
+    kf_torques = [[] for i in xrange(num_kf)]
+    for angle, current, request_current in zip(data_radians, data_current,
+                                               data_request_current):
+        # Predict and correct all the parameter swept observers.
+        for i, observer in enumerate(haptic_observers):
+            observer.Y = numpy.matrix([[angle]])
+            if run_correct:
+                observer.CorrectObserver(numpy.matrix([[current]]))
+            kf_torques[i].append(-observer.X_hat[2, 0])
+            observer.PredictObserver(numpy.matrix([[current]]))
+            observer.PredictObserver(numpy.matrix([[current]]))
 
-  # Plot the wheel observers.
-  fig, ax1 = pylab.subplots()
-  ax1.plot(data_time, data_radians, '.', label='wheel')
-  ax1.plot(data_time, dt_velocity, '.', label='dt_velocity')
-  ax1.plot(data_time, kf_velocity, '.', label='kf_velocity')
-  ax1.plot(data_time, kf_position, '.', label='kf_position')
-  ax1.plot(data_time, adjusted_position, '.', label='adjusted_position')
+        # Predict and correct the main observer.
+        haptic_observer.Y = numpy.matrix([[angle]])
+        if run_correct:
+            haptic_observer.CorrectObserver(numpy.matrix([[current]]))
+        kf_position.append(haptic_observer.X_hat[0, 0])
+        adjusted_position.append(kf_position[-1] -
+                                 last_request_current / params.kP)
+        last_request_current = last_request_current * params.kCompensationTimeconstant + request_current * (
+            1.0 - params.kCompensationTimeconstant)
+        kf_velocity.append(haptic_observer.X_hat[1, 0])
+        if last_angle is None:
+            last_angle = angle
+        dt_velocity.append((angle - last_angle) / 0.001)
 
-  ax2 = ax1.twinx()
-  ax2.plot(data_time, data_current, label='data_current')
-  ax2.plot(data_time, data_request_current, label='request_current')
+        haptic_observer.PredictObserver(numpy.matrix([[current]]))
+        last_angle = angle
 
-  for i, kf_torque in enumerate(kf_torques):
-    ax2.plot(data_time, kf_torque,
-               label='-kf_torque[%f]' % haptic_observers[i].J)
-  fig.tight_layout()
-  ax1.legend(loc=3)
-  ax2.legend(loc=4)
+    # Plot the wheel observers.
+    fig, ax1 = pylab.subplots()
+    ax1.plot(data_time, data_radians, '.', label='wheel')
+    ax1.plot(data_time, dt_velocity, '.', label='dt_velocity')
+    ax1.plot(data_time, kf_velocity, '.', label='kf_velocity')
+    ax1.plot(data_time, kf_position, '.', label='kf_position')
+    ax1.plot(data_time, adjusted_position, '.', label='adjusted_position')
 
-def plot_input(data_time, data_radians, data_velocity, data_torque,
-               data_current, params, run_correct=True):
-  dt_velocity = []
-  last_angle = None
-  initial_velocity = (data_radians[1] - data_radians[0]) * 1000.0
+    ax2 = ax1.twinx()
+    ax2.plot(data_time, data_current, label='data_current')
+    ax2.plot(data_time, data_request_current, label='request_current')
 
-  for angle in data_radians:
-    if last_angle is None:
-      last_angle = angle
-    dt_velocity.append((angle - last_angle) / 0.001)
+    for i, kf_torque in enumerate(kf_torques):
+        ax2.plot(
+            data_time,
+            kf_torque,
+            label='-kf_torque[%f]' % haptic_observers[i].J)
+    fig.tight_layout()
+    ax1.legend(loc=3)
+    ax2.legend(loc=4)
 
-    last_angle = angle
 
-  # Plot the wheel observers.
-  fig, ax1 = pylab.subplots()
-  ax1.plot(data_time, data_radians, '.', label='angle')
-  ax1.plot(data_time, data_velocity, '-', label='velocity')
-  ax1.plot(data_time, dt_velocity, '.', label='dt_velocity')
+def plot_input(data_time,
+               data_radians,
+               data_velocity,
+               data_torque,
+               data_current,
+               params,
+               run_correct=True):
+    dt_velocity = []
+    last_angle = None
+    initial_velocity = (data_radians[1] - data_radians[0]) * 1000.0
 
-  ax2 = ax1.twinx()
-  ax2.plot(data_time, data_torque, label='data_torque')
-  ax2.plot(data_time, data_current, label='data_current')
-  fig.tight_layout()
-  ax1.legend(loc=3)
-  ax2.legend(loc=4)
+    for angle in data_radians:
+        if last_angle is None:
+            last_angle = angle
+        dt_velocity.append((angle - last_angle) / 0.001)
+
+        last_angle = angle
+
+    # Plot the wheel observers.
+    fig, ax1 = pylab.subplots()
+    ax1.plot(data_time, data_radians, '.', label='angle')
+    ax1.plot(data_time, data_velocity, '-', label='velocity')
+    ax1.plot(data_time, dt_velocity, '.', label='dt_velocity')
+
+    ax2 = ax1.twinx()
+    ax2.plot(data_time, data_torque, label='data_torque')
+    ax2.plot(data_time, data_current, label='data_current')
+    fig.tight_layout()
+    ax1.legend(loc=3)
+    ax2.legend(loc=4)
+
 
 def main(argv):
-  if FLAGS.plot:
-    if FLAGS.data is None:
-      haptic_wheel = HapticInput()
-      haptic_wheel_controller = IntegralHapticInput()
-      observer_haptic_wheel = IntegralHapticInput()
-      observer_haptic_wheel.X_hat[2, 0] = 0.01
+    if FLAGS.plot:
+        if FLAGS.data is None:
+            haptic_wheel = HapticInput()
+            haptic_wheel_controller = IntegralHapticInput()
+            observer_haptic_wheel = IntegralHapticInput()
+            observer_haptic_wheel.X_hat[2, 0] = 0.01
 
-      R = numpy.matrix([[0.0], [0.0], [0.0]])
+            R = numpy.matrix([[0.0], [0.0], [0.0]])
 
-      control_loop.TestSingleIntegralAxisSquareWave(
-          R, 1.0, haptic_wheel, haptic_wheel_controller, observer_haptic_wheel)
+            control_loop.TestSingleIntegralAxisSquareWave(
+                R, 1.0, haptic_wheel, haptic_wheel_controller,
+                observer_haptic_wheel)
+        else:
+            # Read the CAN trace in.
+            trigger_data_time, wheel_data_time, trigger, wheel, trigger_velocity, \
+                wheel_velocity, trigger_torque, wheel_torque, trigger_current, \
+                wheel_current, trigger_request_time, trigger_request_current, \
+                wheel_request_time, wheel_request_current = ReadCan(FLAGS.data)
+
+            wheel_radians = [w * numpy.pi * (338.16 / 360.0) for w in wheel]
+            wheel_velocity = [w * 50.0 for w in wheel_velocity]
+            wheel_torque = [w / 2.0 for w in wheel_torque]
+            wheel_current = [w * 10.0 for w in wheel_current]
+            wheel_request_current = [w * 2.0 for w in wheel_request_current]
+            resampled_wheel_request_current = scipy.interpolate.interp1d(
+                wheel_request_time, wheel_request_current,
+                kind="zero")(wheel_data_time)
+
+            trigger_radians = [t * numpy.pi * (45.0 / 360.0) for t in trigger]
+            trigger_velocity = [t * 50.0 for t in trigger_velocity]
+            trigger_torque = [t / 2.0 for t in trigger_torque]
+            trigger_current = [t * 10.0 for t in trigger_current]
+            trigger_request_current = [t * 2.0 for t in trigger_request_current]
+            resampled_trigger_request_current = scipy.interpolate.interp1d(
+                trigger_request_time, trigger_request_current,
+                kind="zero")(trigger_data_time)
+
+            if FLAGS.rerun_kf:
+                rerun_and_plot_kf(
+                    trigger_data_time,
+                    trigger_radians,
+                    trigger_current,
+                    resampled_trigger_request_current,
+                    kTrigger,
+                    run_correct=True)
+                rerun_and_plot_kf(
+                    wheel_data_time,
+                    wheel_radians,
+                    wheel_current,
+                    resampled_wheel_request_current,
+                    kWheel,
+                    run_correct=True)
+            else:
+                plot_input(trigger_data_time, trigger_radians, trigger_velocity,
+                           trigger_torque, trigger_current, kTrigger)
+                plot_input(wheel_data_time, wheel_radians, wheel_velocity,
+                           wheel_torque, wheel_current, kWheel)
+            pylab.show()
+
+        return
+
+    if len(argv) != 9:
+        glog.fatal('Expected .h file name and .cc file name')
     else:
-      # Read the CAN trace in.
-      trigger_data_time, wheel_data_time, trigger, wheel, trigger_velocity, \
-          wheel_velocity, trigger_torque, wheel_torque, trigger_current, \
-          wheel_current, trigger_request_time, trigger_request_current, \
-          wheel_request_time, wheel_request_current = ReadCan(FLAGS.data)
+        namespaces = ['frc971', 'control_loops', 'drivetrain']
+        for name, params, filenames in [('HapticWheel', kWheel, argv[1:5]),
+                                        ('HapticTrigger', kTrigger, argv[5:9])]:
+            haptic_input = HapticInput(params=params, name=name)
+            loop_writer = control_loop.ControlLoopWriter(
+                name, [haptic_input],
+                namespaces=namespaces,
+                scalar_type='float')
+            loop_writer.Write(filenames[0], filenames[1])
 
-      wheel_radians = [w * numpy.pi * (338.16 / 360.0) for w in wheel]
-      wheel_velocity = [w * 50.0 for w in wheel_velocity]
-      wheel_torque = [w / 2.0 for w in wheel_torque]
-      wheel_current = [w * 10.0 for w in wheel_current]
-      wheel_request_current = [w * 2.0 for w in wheel_request_current]
-      resampled_wheel_request_current = scipy.interpolate.interp1d(
-          wheel_request_time, wheel_request_current, kind="zero")(wheel_data_time)
+            integral_haptic_input = IntegralHapticInput(
+                params=params, name='Integral' + name)
+            integral_loop_writer = control_loop.ControlLoopWriter(
+                'Integral' + name, [integral_haptic_input],
+                namespaces=namespaces,
+                scalar_type='float')
 
-      trigger_radians = [t * numpy.pi * (45.0 / 360.0) for t in trigger]
-      trigger_velocity = [t * 50.0 for t in trigger_velocity]
-      trigger_torque = [t / 2.0 for t in trigger_torque]
-      trigger_current = [t * 10.0 for t in trigger_current]
-      trigger_request_current = [t * 2.0 for t in trigger_request_current]
-      resampled_trigger_request_current = scipy.interpolate.interp1d(
-          trigger_request_time, trigger_request_current, kind="zero")(trigger_data_time)
+            integral_loop_writer.AddConstant(
+                control_loop.Constant("k" + name + "Dt", "%f",
+                                      integral_haptic_input.dt))
+            integral_loop_writer.AddConstant(
+                control_loop.Constant("k" + name + "FreeCurrent", "%f",
+                                      integral_haptic_input.motor.free_current))
+            integral_loop_writer.AddConstant(
+                control_loop.Constant("k" + name + "StallTorque", "%f",
+                                      integral_haptic_input.motor.stall_torque))
+            integral_loop_writer.AddConstant(
+                control_loop.Constant("k" + name + "J", "%f",
+                                      integral_haptic_input.J))
+            integral_loop_writer.AddConstant(
+                control_loop.Constant("k" + name + "R", "%f",
+                                      integral_haptic_input.motor.resistance))
+            integral_loop_writer.AddConstant(
+                control_loop.Constant("k" + name + "T", "%f",
+                                      integral_haptic_input.motor.Kt))
+            integral_loop_writer.AddConstant(
+                control_loop.Constant("k" + name + "V", "%f",
+                                      integral_haptic_input.motor.Kv))
+            integral_loop_writer.AddConstant(
+                control_loop.Constant("k" + name + "P", "%f", params.kP))
+            integral_loop_writer.AddConstant(
+                control_loop.Constant("k" + name + "D", "%f", params.kD))
+            integral_loop_writer.AddConstant(
+                control_loop.Constant("k" + name + "G", "%f", params.G))
+            integral_loop_writer.AddConstant(
+                control_loop.Constant("k" + name + "CurrentLimit", "%f",
+                                      params.current_limit))
 
-      if FLAGS.rerun_kf:
-        rerun_and_plot_kf(trigger_data_time, trigger_radians, trigger_current,
-                          resampled_trigger_request_current, kTrigger, run_correct=True)
-        rerun_and_plot_kf(wheel_data_time, wheel_radians, wheel_current,
-                          resampled_wheel_request_current, kWheel, run_correct=True)
-      else:
-        plot_input(trigger_data_time, trigger_radians, trigger_velocity,
-                   trigger_torque, trigger_current, kTrigger)
-        plot_input(wheel_data_time, wheel_radians, wheel_velocity, wheel_torque,
-                   wheel_current, kWheel)
-      pylab.show()
-
-    return
-
-  if len(argv) != 9:
-    glog.fatal('Expected .h file name and .cc file name')
-  else:
-    namespaces = ['frc971', 'control_loops', 'drivetrain']
-    for name, params, filenames in [('HapticWheel', kWheel, argv[1:5]),
-                                    ('HapticTrigger', kTrigger, argv[5:9])]:
-      haptic_input = HapticInput(params=params, name=name)
-      loop_writer = control_loop.ControlLoopWriter(name, [haptic_input],
-                                                   namespaces=namespaces,
-                                                   scalar_type='float')
-      loop_writer.Write(filenames[0], filenames[1])
-
-      integral_haptic_input = IntegralHapticInput(params=params,
-                                                  name='Integral' + name)
-      integral_loop_writer = control_loop.ControlLoopWriter(
-          'Integral' + name, [integral_haptic_input], namespaces=namespaces,
-          scalar_type='float')
-
-      integral_loop_writer.AddConstant(
-          control_loop.Constant("k" + name + "Dt", "%f",
-                                integral_haptic_input.dt))
-      integral_loop_writer.AddConstant(
-          control_loop.Constant("k" + name + "FreeCurrent", "%f",
-                                integral_haptic_input.motor.free_current))
-      integral_loop_writer.AddConstant(
-          control_loop.Constant("k" + name + "StallTorque", "%f",
-                                integral_haptic_input.motor.stall_torque))
-      integral_loop_writer.AddConstant(
-          control_loop.Constant("k" + name + "J", "%f",
-                                integral_haptic_input.J))
-      integral_loop_writer.AddConstant(
-          control_loop.Constant("k" + name + "R", "%f",
-                                integral_haptic_input.motor.resistance))
-      integral_loop_writer.AddConstant(
-          control_loop.Constant("k" + name + "T", "%f",
-                                integral_haptic_input.motor.Kt))
-      integral_loop_writer.AddConstant(
-          control_loop.Constant("k" + name + "V", "%f",
-                                integral_haptic_input.motor.Kv))
-      integral_loop_writer.AddConstant(
-          control_loop.Constant("k" + name + "P", "%f",
-                                params.kP))
-      integral_loop_writer.AddConstant(
-          control_loop.Constant("k" + name + "D", "%f",
-                                params.kD))
-      integral_loop_writer.AddConstant(
-          control_loop.Constant("k" + name + "G", "%f",
-                                params.G))
-      integral_loop_writer.AddConstant(
-          control_loop.Constant("k" + name + "CurrentLimit", "%f",
-                                params.current_limit))
-
-      integral_loop_writer.Write(filenames[2], filenames[3])
+            integral_loop_writer.Write(filenames[2], filenames[3])
 
 
 if __name__ == '__main__':
-  argv = FLAGS(sys.argv)
-  sys.exit(main(argv))
+    argv = FLAGS(sys.argv)
+    sys.exit(main(argv))
diff --git a/frc971/control_loops/python/linear_system.py b/frc971/control_loops/python/linear_system.py
index e1f55cc..175fffa 100755
--- a/frc971/control_loops/python/linear_system.py
+++ b/frc971/control_loops/python/linear_system.py
@@ -9,6 +9,7 @@
 
 
 class LinearSystemParams(object):
+
     def __init__(self,
                  name,
                  motor,
@@ -37,6 +38,7 @@
 
 
 class LinearSystem(control_loop.ControlLoop):
+
     def __init__(self, params, name='LinearSystem'):
         super(LinearSystem, self).__init__(name)
         self.params = params
@@ -57,10 +59,9 @@
         # State is [position, velocity]
         # Input is [Voltage]
         C1 = self.motor.Kt / (self.G * self.G * self.radius * self.radius *
-                              self.motor.resistance * self.mass *
-                              self.motor.Kv)
-        C2 = self.motor.Kt / (self.G * self.radius * self.motor.resistance *
-                              self.mass)
+                              self.motor.resistance * self.mass * self.motor.Kv)
+        C2 = self.motor.Kt / (
+            self.G * self.radius * self.motor.resistance * self.mass)
 
         self.A_continuous = numpy.matrix([[0, 1], [0, -C1]])
 
@@ -79,9 +80,10 @@
         glog.debug('Controllability of %d',
                    numpy.linalg.matrix_rank(controllability))
         glog.debug('Mass: %f', self.mass)
-        glog.debug('Stall force: %f', self.motor.stall_torque / self.G / self.radius)
-        glog.debug('Stall acceleration: %f', self.motor.stall_torque / self.G /
-                   self.radius / self.mass)
+        glog.debug('Stall force: %f',
+                   self.motor.stall_torque / self.G / self.radius)
+        glog.debug('Stall acceleration: %f',
+                   self.motor.stall_torque / self.G / self.radius / self.mass)
 
         glog.debug('Free speed is %f',
                    -self.B_continuous[1, 0] / self.A_continuous[1, 1] * 12.0)
@@ -122,6 +124,7 @@
 
 
 class IntegralLinearSystem(LinearSystem):
+
     def __init__(self, params, name='IntegralLinearSystem'):
         super(IntegralLinearSystem, self).__init__(params, name=name)
 
@@ -142,10 +145,9 @@
         self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
                                                    self.B_continuous, self.dt)
 
-        self.Q = numpy.matrix(
-            [[(self.params.kalman_q_pos**2.0), 0.0, 0.0],
-             [0.0, (self.params.kalman_q_vel**2.0), 0.0],
-             [0.0, 0.0, (self.params.kalman_q_voltage**2.0)]])
+        self.Q = numpy.matrix([[(self.params.kalman_q_pos**2.0), 0.0, 0.0],
+                               [0.0, (self.params.kalman_q_vel**2.0), 0.0],
+                               [0.0, 0.0, (self.params.kalman_q_voltage**2.0)]])
 
         self.R = numpy.matrix([[(self.params.kalman_r_position**2.0)]])
 
@@ -203,8 +205,8 @@
 
     vbat = 12.0
 
-    goal = numpy.concatenate(
-        (plant.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
+    goal = numpy.concatenate((plant.X, numpy.matrix(numpy.zeros((1, 1)))),
+                             axis=0)
 
     profile = TrapezoidProfile(plant.dt)
     profile.set_maximum_acceleration(max_acceleration)
@@ -263,8 +265,7 @@
         goal = controller.A * goal + controller.B * ff_U
 
         if U[0, 0] != U_uncapped[0, 0]:
-            profile.MoveCurrentState(
-                numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
+            profile.MoveCurrentState(numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
 
     glog.debug('Time: %f', t_plot[-1])
     glog.debug('goal_error %s', repr(end_goal - goal))
@@ -378,11 +379,11 @@
     loop_writer = control_loop.ControlLoopWriter(
         linear_system.name, [linear_system], namespaces=year_namespaces)
     loop_writer.AddConstant(
-        control_loop.Constant('kFreeSpeed', '%f', linear_system.motor.
-                              free_speed))
+        control_loop.Constant('kFreeSpeed', '%f',
+                              linear_system.motor.free_speed))
     loop_writer.AddConstant(
-        control_loop.Constant('kOutputRatio', '%f', linear_system.G *
-                              linear_system.radius))
+        control_loop.Constant('kOutputRatio', '%f',
+                              linear_system.G * linear_system.radius))
     loop_writer.AddConstant(
         control_loop.Constant('kRadius', '%f', linear_system.radius))
     loop_writer.Write(plant_files[0], plant_files[1])
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 91dba3c..06a182e 100644
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -10,516 +10,565 @@
 
 import glog
 
+
 def CoerceGoal(region, K, w, R):
-  """Intersects a line with a region, and finds the closest point to R.
+    """Intersects a line with a region, and finds the closest point to R.
 
-  Finds a point that is closest to R inside the region, and on the line
-  defined by K X = w.  If it is not possible to find a point on the line,
-  finds a point that is inside the region and closest to the line.  This
-  function assumes that
+    Finds a point that is closest to R inside the region, and on the line
+    defined by K X = w.  If it is not possible to find a point on the line,
+    finds a point that is inside the region and closest to the line.  This
+    function assumes that
 
-  Args:
-    region: HPolytope, the valid goal region.
-    K: numpy.matrix (2 x 1), the matrix for the equation [K1, K2] [x1; x2] = w
-    w: float, the offset in the equation above.
-    R: numpy.matrix (2 x 1), the point to be closest to.
+    Args:
+        region: HPolytope, the valid goal region.
+        K: numpy.matrix (2 x 1), the matrix for the equation [K1, K2] [x1; x2] = w
+        w: float, the offset in the equation above.
+        R: numpy.matrix (2 x 1), the point to be closest to.
 
-  Returns:
-    numpy.matrix (2 x 1), the point.
-  """
-  return DoCoerceGoal(region, K, w, R)[0]
+    Returns:
+        numpy.matrix (2 x 1), the point.
+    """
+    return DoCoerceGoal(region, K, w, R)[0]
+
 
 def DoCoerceGoal(region, K, w, R):
-  if region.IsInside(R):
-    return (R, True)
+    if region.IsInside(R):
+        return (R, True)
 
-  perpendicular_vector = K.T / numpy.linalg.norm(K)
-  parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
-                                  [-perpendicular_vector[0, 0]]])
+    perpendicular_vector = K.T / numpy.linalg.norm(K)
+    parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
+                                    [-perpendicular_vector[0, 0]]])
 
-  # We want to impose the constraint K * X = w on the polytope H * X <= k.
-  # We do this by breaking X up into parallel and perpendicular components to
-  # the half plane.  This gives us the following equation.
-  #
-  #  parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) = X
-  #
-  # Then, substitute this into the polytope.
-  #
-  #  H * (parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) <= k
-  #
-  # Substitute K * X = w
-  #
-  # H * parallel * (parallel.T \dot X) + H * perpendicular * w <= k
-  #
-  # Move all the knowns to the right side.
-  #
-  # H * parallel * ([parallel1 parallel2] * X) <= k - H * perpendicular * w
-  #
-  # Let t = parallel.T \dot X, the component parallel to the surface.
-  #
-  # H * parallel * t <= k - H * perpendicular * w
-  #
-  # This is a polytope which we can solve, and use to figure out the range of X
-  # that we care about!
+    # We want to impose the constraint K * X = w on the polytope H * X <= k.
+    # We do this by breaking X up into parallel and perpendicular components to
+    # the half plane.  This gives us the following equation.
+    #
+    #  parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) = X
+    #
+    # Then, substitute this into the polytope.
+    #
+    #  H * (parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) <= k
+    #
+    # Substitute K * X = w
+    #
+    # H * parallel * (parallel.T \dot X) + H * perpendicular * w <= k
+    #
+    # Move all the knowns to the right side.
+    #
+    # H * parallel * ([parallel1 parallel2] * X) <= k - H * perpendicular * w
+    #
+    # Let t = parallel.T \dot X, the component parallel to the surface.
+    #
+    # H * parallel * t <= k - H * perpendicular * w
+    #
+    # This is a polytope which we can solve, and use to figure out the range of X
+    # that we care about!
 
-  t_poly = polytope.HPolytope(
-      region.H * parallel_vector,
-      region.k - region.H * perpendicular_vector * w)
+    t_poly = polytope.HPolytope(region.H * parallel_vector,
+                                region.k - region.H * perpendicular_vector * w)
 
-  vertices = t_poly.Vertices()
+    vertices = t_poly.Vertices()
 
-  if vertices.shape[0]:
-    # The region exists!
-    # Find the closest vertex
-    min_distance = numpy.infty
-    closest_point = None
-    for vertex in vertices:
-      point = parallel_vector * vertex + perpendicular_vector * w
-      length = numpy.linalg.norm(R - point)
-      if length < min_distance:
-        min_distance = length
-        closest_point = point
+    if vertices.shape[0]:
+        # The region exists!
+        # Find the closest vertex
+        min_distance = numpy.infty
+        closest_point = None
+        for vertex in vertices:
+            point = parallel_vector * vertex + perpendicular_vector * w
+            length = numpy.linalg.norm(R - point)
+            if length < min_distance:
+                min_distance = length
+                closest_point = point
 
-    return (closest_point, True)
-  else:
-    # Find the vertex of the space that is closest to the line.
-    region_vertices = region.Vertices()
-    min_distance = numpy.infty
-    closest_point = None
-    for vertex in region_vertices:
-      point = vertex.T
-      length = numpy.abs((perpendicular_vector.T * point)[0, 0])
-      if length < min_distance:
-        min_distance = length
-        closest_point = point
+        return (closest_point, True)
+    else:
+        # Find the vertex of the space that is closest to the line.
+        region_vertices = region.Vertices()
+        min_distance = numpy.infty
+        closest_point = None
+        for vertex in region_vertices:
+            point = vertex.T
+            length = numpy.abs((perpendicular_vector.T * point)[0, 0])
+            if length < min_distance:
+                min_distance = length
+                closest_point = point
 
-    return (closest_point, False)
+        return (closest_point, False)
+
 
 class VelocityDrivetrainModel(control_loop.ControlLoop):
-  def __init__(self, drivetrain_params, left_low=True, right_low=True,
-                   name="VelocityDrivetrainModel"):
-    super(VelocityDrivetrainModel, self).__init__(name)
-    self._drivetrain = frc971.control_loops.python.drivetrain.Drivetrain(
-                           left_low=left_low, right_low=right_low,
-                           drivetrain_params=drivetrain_params)
-    self.dt = drivetrain_params.dt
-    self.A_continuous = numpy.matrix(
-        [[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
-         [self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
 
-    self.B_continuous = numpy.matrix(
-        [[self._drivetrain.B_continuous[1, 0], self._drivetrain.B_continuous[1, 1]],
-         [self._drivetrain.B_continuous[3, 0], self._drivetrain.B_continuous[3, 1]]])
-    self.C = numpy.matrix(numpy.eye(2))
-    self.D = numpy.matrix(numpy.zeros((2, 2)))
+    def __init__(self,
+                 drivetrain_params,
+                 left_low=True,
+                 right_low=True,
+                 name="VelocityDrivetrainModel"):
+        super(VelocityDrivetrainModel, self).__init__(name)
+        self._drivetrain = frc971.control_loops.python.drivetrain.Drivetrain(
+            left_low=left_low,
+            right_low=right_low,
+            drivetrain_params=drivetrain_params)
+        self.dt = drivetrain_params.dt
+        self.A_continuous = numpy.matrix(
+            [[
+                self._drivetrain.A_continuous[1, 1],
+                self._drivetrain.A_continuous[1, 3]
+            ],
+             [
+                 self._drivetrain.A_continuous[3, 1],
+                 self._drivetrain.A_continuous[3, 3]
+             ]])
 
-    self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
-                                               self.B_continuous, self.dt)
+        self.B_continuous = numpy.matrix(
+            [[
+                self._drivetrain.B_continuous[1, 0],
+                self._drivetrain.B_continuous[1, 1]
+            ],
+             [
+                 self._drivetrain.B_continuous[3, 0],
+                 self._drivetrain.B_continuous[3, 1]
+             ]])
+        self.C = numpy.matrix(numpy.eye(2))
+        self.D = numpy.matrix(numpy.zeros((2, 2)))
 
-    # FF * X = U (steady state)
-    self.FF = self.B.I * (numpy.eye(2) - self.A)
+        self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+                                                   self.B_continuous, self.dt)
 
-    self.PlaceControllerPoles(drivetrain_params.controller_poles)
-    # Build a kalman filter for the velocity.  We don't care what the gains
-    # are, but the hybrid kalman filter that we want to write to disk to get
-    # access to A_continuous and B_continuous needs this for completeness.
-    self.Q_continuous = numpy.matrix([[(0.5 ** 2.0), 0.0], [0.0, (0.5 ** 2.0)]])
-    self.R_continuous = numpy.matrix([[(0.1 ** 2.0), 0.0], [0.0, (0.1 ** 2.0)]])
-    self.PlaceObserverPoles(drivetrain_params.observer_poles)
-    _, _, self.Q, self.R = controls.kalmd(
-        A_continuous=self.A_continuous, B_continuous=self.B_continuous,
-        Q_continuous=self.Q_continuous, R_continuous=self.R_continuous,
-        dt=self.dt)
+        # FF * X = U (steady state)
+        self.FF = self.B.I * (numpy.eye(2) - self.A)
 
-    self.KalmanGain, self.P_steady_state = controls.kalman(
-        A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+        self.PlaceControllerPoles(drivetrain_params.controller_poles)
+        # Build a kalman filter for the velocity.  We don't care what the gains
+        # are, but the hybrid kalman filter that we want to write to disk to get
+        # access to A_continuous and B_continuous needs this for completeness.
+        self.Q_continuous = numpy.matrix([[(0.5**2.0), 0.0], [0.0, (0.5**2.0)]])
+        self.R_continuous = numpy.matrix([[(0.1**2.0), 0.0], [0.0, (0.1**2.0)]])
+        self.PlaceObserverPoles(drivetrain_params.observer_poles)
+        _, _, self.Q, self.R = controls.kalmd(
+            A_continuous=self.A_continuous,
+            B_continuous=self.B_continuous,
+            Q_continuous=self.Q_continuous,
+            R_continuous=self.R_continuous,
+            dt=self.dt)
 
-    self.G_high = self._drivetrain.G_high
-    self.G_low = self._drivetrain.G_low
-    self.resistance = self._drivetrain.resistance
-    self.r = self._drivetrain.r
-    self.Kv = self._drivetrain.Kv
-    self.Kt = self._drivetrain.Kt
+        self.KalmanGain, self.P_steady_state = controls.kalman(
+            A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
 
-    self.U_max = self._drivetrain.U_max
-    self.U_min = self._drivetrain.U_min
+        self.G_high = self._drivetrain.G_high
+        self.G_low = self._drivetrain.G_low
+        self.resistance = self._drivetrain.resistance
+        self.r = self._drivetrain.r
+        self.Kv = self._drivetrain.Kv
+        self.Kt = self._drivetrain.Kt
 
-  @property
-  def robot_radius_l(self):
-    return self._drivetrain.robot_radius_l
-  @property
-  def robot_radius_r(self):
-    return self._drivetrain.robot_radius_r
+        self.U_max = self._drivetrain.U_max
+        self.U_min = self._drivetrain.U_min
+
+    @property
+    def robot_radius_l(self):
+        return self._drivetrain.robot_radius_l
+
+    @property
+    def robot_radius_r(self):
+        return self._drivetrain.robot_radius_r
+
 
 class VelocityDrivetrain(object):
-  HIGH = 'high'
-  LOW = 'low'
-  SHIFTING_UP = 'up'
-  SHIFTING_DOWN = 'down'
+    HIGH = 'high'
+    LOW = 'low'
+    SHIFTING_UP = 'up'
+    SHIFTING_DOWN = 'down'
 
-  def __init__(self, drivetrain_params, name='VelocityDrivetrain'):
-    self.drivetrain_low_low = VelocityDrivetrainModel(
-        left_low=True, right_low=True, name=name + 'LowLow',
-        drivetrain_params=drivetrain_params)
-    self.drivetrain_low_high = VelocityDrivetrainModel(
-        left_low=True, right_low=False, name=name + 'LowHigh',
-        drivetrain_params=drivetrain_params)
-    self.drivetrain_high_low = VelocityDrivetrainModel(
-        left_low=False, right_low=True, name = name + 'HighLow',
-        drivetrain_params=drivetrain_params)
-    self.drivetrain_high_high = VelocityDrivetrainModel(
-        left_low=False, right_low=False, name = name + 'HighHigh',
-        drivetrain_params=drivetrain_params)
+    def __init__(self, drivetrain_params, name='VelocityDrivetrain'):
+        self.drivetrain_low_low = VelocityDrivetrainModel(
+            left_low=True,
+            right_low=True,
+            name=name + 'LowLow',
+            drivetrain_params=drivetrain_params)
+        self.drivetrain_low_high = VelocityDrivetrainModel(
+            left_low=True,
+            right_low=False,
+            name=name + 'LowHigh',
+            drivetrain_params=drivetrain_params)
+        self.drivetrain_high_low = VelocityDrivetrainModel(
+            left_low=False,
+            right_low=True,
+            name=name + 'HighLow',
+            drivetrain_params=drivetrain_params)
+        self.drivetrain_high_high = VelocityDrivetrainModel(
+            left_low=False,
+            right_low=False,
+            name=name + 'HighHigh',
+            drivetrain_params=drivetrain_params)
 
-    # X is [lvel, rvel]
-    self.X = numpy.matrix(
-        [[0.0],
-         [0.0]])
+        # X is [lvel, rvel]
+        self.X = numpy.matrix([[0.0], [0.0]])
 
-    self.U_poly = polytope.HPolytope(
-        numpy.matrix([[1, 0],
-                      [-1, 0],
-                      [0, 1],
-                      [0, -1]]),
-        numpy.matrix([[12],
-                      [12],
-                      [12],
-                      [12]]))
+        self.U_poly = polytope.HPolytope(
+            numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]]),
+            numpy.matrix([[12], [12], [12], [12]]))
 
-    self.U_max = numpy.matrix(
-        [[12.0],
-         [12.0]])
-    self.U_min = numpy.matrix(
-        [[-12.0000000000],
-         [-12.0000000000]])
+        self.U_max = numpy.matrix([[12.0], [12.0]])
+        self.U_min = numpy.matrix([[-12.0000000000], [-12.0000000000]])
 
-    self.dt = 0.00505
+        self.dt = 0.00505
 
-    self.R = numpy.matrix(
-        [[0.0],
-         [0.0]])
+        self.R = numpy.matrix([[0.0], [0.0]])
 
-    self.U_ideal = numpy.matrix(
-        [[0.0],
-         [0.0]])
+        self.U_ideal = numpy.matrix([[0.0], [0.0]])
 
-    # ttrust is the comprimise between having full throttle negative inertia,
-    # and having no throttle negative inertia.  A value of 0 is full throttle
-    # inertia.  A value of 1 is no throttle negative inertia.
-    self.ttrust = 1.0
+        # ttrust is the comprimise between having full throttle negative inertia,
+        # and having no throttle negative inertia.  A value of 0 is full throttle
+        # inertia.  A value of 1 is no throttle negative inertia.
+        self.ttrust = 1.0
 
-    self.left_gear = VelocityDrivetrain.LOW
-    self.right_gear = VelocityDrivetrain.LOW
-    self.left_shifter_position = 0.0
-    self.right_shifter_position = 0.0
-    self.left_cim = CIM()
-    self.right_cim = CIM()
+        self.left_gear = VelocityDrivetrain.LOW
+        self.right_gear = VelocityDrivetrain.LOW
+        self.left_shifter_position = 0.0
+        self.right_shifter_position = 0.0
+        self.left_cim = CIM()
+        self.right_cim = CIM()
 
-  def IsInGear(self, gear):
-    return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
+    def IsInGear(self, gear):
+        return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
 
-  def MotorRPM(self, shifter_position, velocity):
-    if shifter_position > 0.5:
-      return (velocity / self.CurrentDrivetrain().G_high /
-              self.CurrentDrivetrain().r)
-    else:
-      return (velocity / self.CurrentDrivetrain().G_low /
-              self.CurrentDrivetrain().r)
-
-  def CurrentDrivetrain(self):
-    if self.left_shifter_position > 0.5:
-      if self.right_shifter_position > 0.5:
-        return self.drivetrain_high_high
-      else:
-        return self.drivetrain_high_low
-    else:
-      if self.right_shifter_position > 0.5:
-        return self.drivetrain_low_high
-      else:
-        return self.drivetrain_low_low
-
-  def SimShifter(self, gear, shifter_position):
-    if gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.SHIFTING_UP:
-      shifter_position = min(shifter_position + 0.5, 1.0)
-    else:
-      shifter_position = max(shifter_position - 0.5, 0.0)
-
-    if shifter_position == 1.0:
-      gear = VelocityDrivetrain.HIGH
-    elif shifter_position == 0.0:
-      gear = VelocityDrivetrain.LOW
-
-    return gear, shifter_position
-
-  def ComputeGear(self, wheel_velocity, should_print=False, current_gear=False, gear_name=None):
-    high_omega = (wheel_velocity / self.CurrentDrivetrain().G_high /
-                  self.CurrentDrivetrain().r)
-    low_omega = (wheel_velocity / self.CurrentDrivetrain().G_low /
-                 self.CurrentDrivetrain().r)
-    #print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
-    high_torque = ((12.0 - high_omega / self.CurrentDrivetrain().Kv) *
-                   self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
-    low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
-                  self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
-    high_power = high_torque * high_omega
-    low_power = low_torque * low_omega
-    #if should_print:
-    #  print gear_name, "High omega", high_omega, "Low omega", low_omega
-    #  print gear_name, "High torque", high_torque, "Low torque", low_torque
-    #  print gear_name, "High power", high_power, "Low power", low_power
-
-    # Shift algorithm improvements.
-    # TODO(aschuh):
-    # It takes time to shift.  Shifting down for 1 cycle doesn't make sense
-    # because you will end up slower than without shifting.  Figure out how
-    # to include that info.
-    # If the driver is still in high gear, but isn't asking for the extra power
-    # from low gear, don't shift until he asks for it.
-    goal_gear_is_high = high_power > low_power
-    #goal_gear_is_high = True
-
-    if not self.IsInGear(current_gear):
-      glog.debug('%s Not in gear.', gear_name)
-      return current_gear
-    else:
-      is_high = current_gear is VelocityDrivetrain.HIGH
-      if is_high != goal_gear_is_high:
-        if goal_gear_is_high:
-          glog.debug('%s Shifting up.', gear_name)
-          return VelocityDrivetrain.SHIFTING_UP
+    def MotorRPM(self, shifter_position, velocity):
+        if shifter_position > 0.5:
+            return (velocity / self.CurrentDrivetrain().G_high /
+                    self.CurrentDrivetrain().r)
         else:
-          glog.debug('%s Shifting down.', gear_name)
-          return VelocityDrivetrain.SHIFTING_DOWN
-      else:
-        return current_gear
+            return (velocity / self.CurrentDrivetrain().G_low /
+                    self.CurrentDrivetrain().r)
 
-  def FilterVelocity(self, throttle):
-    # Invert the plant to figure out how the velocity filter would have to work
-    # out in order to filter out the forwards negative inertia.
-    # This math assumes that the left and right power and velocity are equal.
+    def CurrentDrivetrain(self):
+        if self.left_shifter_position > 0.5:
+            if self.right_shifter_position > 0.5:
+                return self.drivetrain_high_high
+            else:
+                return self.drivetrain_high_low
+        else:
+            if self.right_shifter_position > 0.5:
+                return self.drivetrain_low_high
+            else:
+                return self.drivetrain_low_low
 
-    # The throttle filter should filter such that the motor in the highest gear
-    # should be controlling the time constant.
-    # Do this by finding the index of FF that has the lowest value, and computing
-    # the sums using that index.
-    FF_sum = self.CurrentDrivetrain().FF.sum(axis=1)
-    min_FF_sum_index = numpy.argmin(FF_sum)
-    min_FF_sum = FF_sum[min_FF_sum_index, 0]
-    min_K_sum = self.CurrentDrivetrain().K[min_FF_sum_index, :].sum()
-    # Compute the FF sum for high gear.
-    high_min_FF_sum = self.drivetrain_high_high.FF[0, :].sum()
+    def SimShifter(self, gear, shifter_position):
+        if gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.SHIFTING_UP:
+            shifter_position = min(shifter_position + 0.5, 1.0)
+        else:
+            shifter_position = max(shifter_position - 0.5, 0.0)
 
-    # U = self.K[0, :].sum() * (R - x_avg) + self.FF[0, :].sum() * R
-    # throttle * 12.0 = (self.K[0, :].sum() + self.FF[0, :].sum()) * R
-    #                   - self.K[0, :].sum() * x_avg
+        if shifter_position == 1.0:
+            gear = VelocityDrivetrain.HIGH
+        elif shifter_position == 0.0:
+            gear = VelocityDrivetrain.LOW
 
-    # R = (throttle * 12.0 + self.K[0, :].sum() * x_avg) /
-    #     (self.K[0, :].sum() + self.FF[0, :].sum())
+        return gear, shifter_position
 
-    # U = (K + FF) * R - K * X
-    # (K + FF) ^-1 * (U + K * X) = R
+    def ComputeGear(self,
+                    wheel_velocity,
+                    should_print=False,
+                    current_gear=False,
+                    gear_name=None):
+        high_omega = (wheel_velocity / self.CurrentDrivetrain().G_high /
+                      self.CurrentDrivetrain().r)
+        low_omega = (wheel_velocity / self.CurrentDrivetrain().G_low /
+                     self.CurrentDrivetrain().r)
+        #print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
+        high_torque = (
+            (12.0 - high_omega / self.CurrentDrivetrain().Kv) *
+            self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
+        low_torque = (
+            (12.0 - low_omega / self.CurrentDrivetrain().Kv) *
+            self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
+        high_power = high_torque * high_omega
+        low_power = low_torque * low_omega
+        #if should_print:
+        #  print gear_name, "High omega", high_omega, "Low omega", low_omega
+        #  print gear_name, "High torque", high_torque, "Low torque", low_torque
+        #  print gear_name, "High power", high_power, "Low power", low_power
 
-    # Scale throttle by min_FF_sum / high_min_FF_sum.  This will make low gear
-    # have the same velocity goal as high gear, and so that the robot will hold
-    # the same speed for the same throttle for all gears.
-    adjusted_ff_voltage = numpy.clip(throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0)
-    return ((adjusted_ff_voltage + self.ttrust * min_K_sum * (self.X[0, 0] + self.X[1, 0]) / 2.0)
-            / (self.ttrust * min_K_sum + min_FF_sum))
+        # Shift algorithm improvements.
+        # TODO(aschuh):
+        # It takes time to shift.  Shifting down for 1 cycle doesn't make sense
+        # because you will end up slower than without shifting.  Figure out how
+        # to include that info.
+        # If the driver is still in high gear, but isn't asking for the extra power
+        # from low gear, don't shift until he asks for it.
+        goal_gear_is_high = high_power > low_power
+        #goal_gear_is_high = True
 
-  def Update(self, throttle, steering):
-    # Shift into the gear which sends the most power to the floor.
-    # This is the same as sending the most torque down to the floor at the
-    # wheel.
+        if not self.IsInGear(current_gear):
+            glog.debug('%s Not in gear.', gear_name)
+            return current_gear
+        else:
+            is_high = current_gear is VelocityDrivetrain.HIGH
+            if is_high != goal_gear_is_high:
+                if goal_gear_is_high:
+                    glog.debug('%s Shifting up.', gear_name)
+                    return VelocityDrivetrain.SHIFTING_UP
+                else:
+                    glog.debug('%s Shifting down.', gear_name)
+                    return VelocityDrivetrain.SHIFTING_DOWN
+            else:
+                return current_gear
 
-    self.left_gear = self.right_gear = True
-    if True:
-      self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
-                                        current_gear=self.left_gear,
-                                        gear_name="left")
-      self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
-                                         current_gear=self.right_gear,
-                                         gear_name="right")
-      if self.IsInGear(self.left_gear):
-        self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+    def FilterVelocity(self, throttle):
+        # Invert the plant to figure out how the velocity filter would have to work
+        # out in order to filter out the forwards negative inertia.
+        # This math assumes that the left and right power and velocity are equal.
 
-      if self.IsInGear(self.right_gear):
-        self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
+        # The throttle filter should filter such that the motor in the highest gear
+        # should be controlling the time constant.
+        # Do this by finding the index of FF that has the lowest value, and computing
+        # the sums using that index.
+        FF_sum = self.CurrentDrivetrain().FF.sum(axis=1)
+        min_FF_sum_index = numpy.argmin(FF_sum)
+        min_FF_sum = FF_sum[min_FF_sum_index, 0]
+        min_K_sum = self.CurrentDrivetrain().K[min_FF_sum_index, :].sum()
+        # Compute the FF sum for high gear.
+        high_min_FF_sum = self.drivetrain_high_high.FF[0, :].sum()
 
-    if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
-      # Filter the throttle to provide a nicer response.
-      fvel = self.FilterVelocity(throttle)
+        # U = self.K[0, :].sum() * (R - x_avg) + self.FF[0, :].sum() * R
+        # throttle * 12.0 = (self.K[0, :].sum() + self.FF[0, :].sum()) * R
+        #                   - self.K[0, :].sum() * x_avg
 
-      # Constant radius means that angualar_velocity / linear_velocity = constant.
-      # Compute the left and right velocities.
-      steering_velocity = numpy.abs(fvel) * steering
-      left_velocity = fvel - steering_velocity
-      right_velocity = fvel + steering_velocity
+        # R = (throttle * 12.0 + self.K[0, :].sum() * x_avg) /
+        #     (self.K[0, :].sum() + self.FF[0, :].sum())
 
-      # Write this constraint in the form of K * R = w
-      # angular velocity / linear velocity = constant
-      # (left - right) / (left + right) = constant
-      # left - right = constant * left + constant * right
+        # U = (K + FF) * R - K * X
+        # (K + FF) ^-1 * (U + K * X) = R
 
-      # (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) /
-      #  (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) =
-      #       constant
-      # (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant
-      # (-steering * sign(fvel)) = constant
-      # (-steering * sign(fvel)) * (left + right) = left - right
-      # (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0
+        # Scale throttle by min_FF_sum / high_min_FF_sum.  This will make low gear
+        # have the same velocity goal as high gear, and so that the robot will hold
+        # the same speed for the same throttle for all gears.
+        adjusted_ff_voltage = numpy.clip(
+            throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0)
+        return ((adjusted_ff_voltage + self.ttrust * min_K_sum *
+                 (self.X[0, 0] + self.X[1, 0]) / 2.0) /
+                (self.ttrust * min_K_sum + min_FF_sum))
 
-      equality_k = numpy.matrix(
-          [[1 + steering * numpy.sign(fvel), -(1 - steering * numpy.sign(fvel))]])
-      equality_w = 0.0
+    def Update(self, throttle, steering):
+        # Shift into the gear which sends the most power to the floor.
+        # This is the same as sending the most torque down to the floor at the
+        # wheel.
 
-      self.R[0, 0] = left_velocity
-      self.R[1, 0] = right_velocity
+        self.left_gear = self.right_gear = True
+        if True:
+            self.left_gear = self.ComputeGear(
+                self.X[0, 0],
+                should_print=True,
+                current_gear=self.left_gear,
+                gear_name="left")
+            self.right_gear = self.ComputeGear(
+                self.X[1, 0],
+                should_print=True,
+                current_gear=self.right_gear,
+                gear_name="right")
+            if self.IsInGear(self.left_gear):
+                self.left_cim.X[0, 0] = self.MotorRPM(
+                    self.left_shifter_position, self.X[0, 0])
 
-      # Construct a constraint on R by manipulating the constraint on U
-      # Start out with H * U <= k
-      # U = FF * R + K * (R - X)
-      # H * (FF * R + K * R - K * X) <= k
-      # H * (FF + K) * R <= k + H * K * X
-      R_poly = polytope.HPolytope(
-          self.U_poly.H * (self.CurrentDrivetrain().K + self.CurrentDrivetrain().FF),
-          self.U_poly.k + self.U_poly.H * self.CurrentDrivetrain().K * self.X)
+            if self.IsInGear(self.right_gear):
+                self.right_cim.X[0, 0] = self.MotorRPM(
+                    self.right_shifter_position, self.X[0, 0])
 
-      # Limit R back inside the box.
-      self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R)
+        if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+            # Filter the throttle to provide a nicer response.
+            fvel = self.FilterVelocity(throttle)
 
-      FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
-      self.U_ideal = self.CurrentDrivetrain().K * (self.boxed_R - self.X) + FF_volts
-    else:
-      glog.debug('Not all in gear')
-      if not self.IsInGear(self.left_gear) and not self.IsInGear(self.right_gear):
-        # TODO(austin): Use battery volts here.
-        R_left = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
-        self.U_ideal[0, 0] = numpy.clip(
-            self.left_cim.K * (R_left - self.left_cim.X) + R_left / self.left_cim.Kv,
-            self.left_cim.U_min, self.left_cim.U_max)
-        self.left_cim.Update(self.U_ideal[0, 0])
+            # Constant radius means that angualar_velocity / linear_velocity = constant.
+            # Compute the left and right velocities.
+            steering_velocity = numpy.abs(fvel) * steering
+            left_velocity = fvel - steering_velocity
+            right_velocity = fvel + steering_velocity
 
-        R_right = self.MotorRPM(self.right_shifter_position, self.X[1, 0])
-        self.U_ideal[1, 0] = numpy.clip(
-            self.right_cim.K * (R_right - self.right_cim.X) + R_right / self.right_cim.Kv,
-            self.right_cim.U_min, self.right_cim.U_max)
-        self.right_cim.Update(self.U_ideal[1, 0])
-      else:
-        assert False
+            # Write this constraint in the form of K * R = w
+            # angular velocity / linear velocity = constant
+            # (left - right) / (left + right) = constant
+            # left - right = constant * left + constant * right
 
-    self.U = numpy.clip(self.U_ideal, self.U_min, self.U_max)
+            # (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) /
+            #  (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) =
+            #       constant
+            # (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant
+            # (-steering * sign(fvel)) = constant
+            # (-steering * sign(fvel)) * (left + right) = left - right
+            # (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0
 
-    # TODO(austin): Model the robot as not accelerating when you shift...
-    # This hack only works when you shift at the same time.
-    if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
-      self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
+            equality_k = numpy.matrix([[
+                1 + steering * numpy.sign(fvel),
+                -(1 - steering * numpy.sign(fvel))
+            ]])
+            equality_w = 0.0
 
-    self.left_gear, self.left_shifter_position = self.SimShifter(
-        self.left_gear, self.left_shifter_position)
-    self.right_gear, self.right_shifter_position = self.SimShifter(
-        self.right_gear, self.right_shifter_position)
+            self.R[0, 0] = left_velocity
+            self.R[1, 0] = right_velocity
 
-    glog.debug('U is %s %s', str(self.U[0, 0]), str(self.U[1, 0]))
-    glog.debug('Left shifter %s %d Right shifter %s %d',
-               self.left_gear, self.left_shifter_position,
-               self.right_gear, self.right_shifter_position)
+            # Construct a constraint on R by manipulating the constraint on U
+            # Start out with H * U <= k
+            # U = FF * R + K * (R - X)
+            # H * (FF * R + K * R - K * X) <= k
+            # H * (FF + K) * R <= k + H * K * X
+            R_poly = polytope.HPolytope(
+                self.U_poly.H *
+                (self.CurrentDrivetrain().K + self.CurrentDrivetrain().FF),
+                self.U_poly.k +
+                self.U_poly.H * self.CurrentDrivetrain().K * self.X)
 
-def WritePolyDrivetrain(drivetrain_files, motor_files, hybrid_files,
-                        year_namespace, drivetrain_params,
+            # Limit R back inside the box.
+            self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R)
+
+            FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
+            self.U_ideal = self.CurrentDrivetrain().K * (
+                self.boxed_R - self.X) + FF_volts
+        else:
+            glog.debug('Not all in gear')
+            if not self.IsInGear(self.left_gear) and not self.IsInGear(
+                    self.right_gear):
+                # TODO(austin): Use battery volts here.
+                R_left = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+                self.U_ideal[0, 0] = numpy.clip(
+                    self.left_cim.K * (R_left - self.left_cim.X) +
+                    R_left / self.left_cim.Kv, self.left_cim.U_min,
+                    self.left_cim.U_max)
+                self.left_cim.Update(self.U_ideal[0, 0])
+
+                R_right = self.MotorRPM(self.right_shifter_position,
+                                        self.X[1, 0])
+                self.U_ideal[1, 0] = numpy.clip(
+                    self.right_cim.K * (R_right - self.right_cim.X) +
+                    R_right / self.right_cim.Kv, self.right_cim.U_min,
+                    self.right_cim.U_max)
+                self.right_cim.Update(self.U_ideal[1, 0])
+            else:
+                assert False
+
+        self.U = numpy.clip(self.U_ideal, self.U_min, self.U_max)
+
+        # TODO(austin): Model the robot as not accelerating when you shift...
+        # This hack only works when you shift at the same time.
+        if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+            self.X = self.CurrentDrivetrain(
+            ).A * self.X + self.CurrentDrivetrain().B * self.U
+
+        self.left_gear, self.left_shifter_position = self.SimShifter(
+            self.left_gear, self.left_shifter_position)
+        self.right_gear, self.right_shifter_position = self.SimShifter(
+            self.right_gear, self.right_shifter_position)
+
+        glog.debug('U is %s %s', str(self.U[0, 0]), str(self.U[1, 0]))
+        glog.debug('Left shifter %s %d Right shifter %s %d', self.left_gear,
+                   self.left_shifter_position, self.right_gear,
+                   self.right_shifter_position)
+
+
+def WritePolyDrivetrain(drivetrain_files,
+                        motor_files,
+                        hybrid_files,
+                        year_namespace,
+                        drivetrain_params,
                         scalar_type='double'):
-  vdrivetrain = VelocityDrivetrain(drivetrain_params)
-  hybrid_vdrivetrain = VelocityDrivetrain(drivetrain_params,
-                                          name="HybridVelocityDrivetrain")
-  if isinstance(year_namespace, list):
-    namespaces = year_namespace
-  else:
-    namespaces = [year_namespace, 'control_loops', 'drivetrain']
-  dog_loop_writer = control_loop.ControlLoopWriter(
-      "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
-                     vdrivetrain.drivetrain_low_high,
-                     vdrivetrain.drivetrain_high_low,
-                     vdrivetrain.drivetrain_high_high],
-                     namespaces=namespaces,
-                     scalar_type=scalar_type)
+    vdrivetrain = VelocityDrivetrain(drivetrain_params)
+    hybrid_vdrivetrain = VelocityDrivetrain(
+        drivetrain_params, name="HybridVelocityDrivetrain")
+    if isinstance(year_namespace, list):
+        namespaces = year_namespace
+    else:
+        namespaces = [year_namespace, 'control_loops', 'drivetrain']
+    dog_loop_writer = control_loop.ControlLoopWriter(
+        "VelocityDrivetrain", [
+            vdrivetrain.drivetrain_low_low, vdrivetrain.drivetrain_low_high,
+            vdrivetrain.drivetrain_high_low, vdrivetrain.drivetrain_high_high
+        ],
+        namespaces=namespaces,
+        scalar_type=scalar_type)
 
-  dog_loop_writer.Write(drivetrain_files[0], drivetrain_files[1])
+    dog_loop_writer.Write(drivetrain_files[0], drivetrain_files[1])
 
-  hybrid_loop_writer = control_loop.ControlLoopWriter(
-      "HybridVelocityDrivetrain", [hybrid_vdrivetrain.drivetrain_low_low,
-                     hybrid_vdrivetrain.drivetrain_low_high,
-                     hybrid_vdrivetrain.drivetrain_high_low,
-                     hybrid_vdrivetrain.drivetrain_high_high],
-                     namespaces=namespaces,
-                     scalar_type=scalar_type,
-                     plant_type='StateFeedbackHybridPlant',
-                     observer_type='HybridKalman')
+    hybrid_loop_writer = control_loop.ControlLoopWriter(
+        "HybridVelocityDrivetrain", [
+            hybrid_vdrivetrain.drivetrain_low_low,
+            hybrid_vdrivetrain.drivetrain_low_high,
+            hybrid_vdrivetrain.drivetrain_high_low,
+            hybrid_vdrivetrain.drivetrain_high_high
+        ],
+        namespaces=namespaces,
+        scalar_type=scalar_type,
+        plant_type='StateFeedbackHybridPlant',
+        observer_type='HybridKalman')
 
-  hybrid_loop_writer.Write(hybrid_files[0], hybrid_files[1])
+    hybrid_loop_writer.Write(hybrid_files[0], hybrid_files[1])
 
-  cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()], scalar_type=scalar_type)
+    cim_writer = control_loop.ControlLoopWriter(
+        "CIM", [CIM()], scalar_type=scalar_type)
 
-  cim_writer.Write(motor_files[0], motor_files[1])
+    cim_writer.Write(motor_files[0], motor_files[1])
+
 
 def PlotPolyDrivetrainMotions(drivetrain_params):
-  vdrivetrain = VelocityDrivetrain(drivetrain_params)
-  vl_plot = []
-  vr_plot = []
-  ul_plot = []
-  ur_plot = []
-  radius_plot = []
-  t_plot = []
-  left_gear_plot = []
-  right_gear_plot = []
-  vdrivetrain.left_shifter_position = 0.0
-  vdrivetrain.right_shifter_position = 0.0
-  vdrivetrain.left_gear = VelocityDrivetrain.LOW
-  vdrivetrain.right_gear = VelocityDrivetrain.LOW
+    vdrivetrain = VelocityDrivetrain(drivetrain_params)
+    vl_plot = []
+    vr_plot = []
+    ul_plot = []
+    ur_plot = []
+    radius_plot = []
+    t_plot = []
+    left_gear_plot = []
+    right_gear_plot = []
+    vdrivetrain.left_shifter_position = 0.0
+    vdrivetrain.right_shifter_position = 0.0
+    vdrivetrain.left_gear = VelocityDrivetrain.LOW
+    vdrivetrain.right_gear = VelocityDrivetrain.LOW
 
-  glog.debug('K is %s', str(vdrivetrain.CurrentDrivetrain().K))
+    glog.debug('K is %s', str(vdrivetrain.CurrentDrivetrain().K))
 
-  if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
-    glog.debug('Left is high')
-  else:
-    glog.debug('Left is low')
-  if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
-    glog.debug('Right is high')
-  else:
-    glog.debug('Right is low')
-
-  for t in numpy.arange(0, 1.7, vdrivetrain.dt):
-    if t < 0.5:
-      vdrivetrain.Update(throttle=0.00, steering=1.0)
-    elif t < 1.2:
-      vdrivetrain.Update(throttle=0.5, steering=1.0)
+    if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
+        glog.debug('Left is high')
     else:
-      vdrivetrain.Update(throttle=0.00, steering=1.0)
-    t_plot.append(t)
-    vl_plot.append(vdrivetrain.X[0, 0])
-    vr_plot.append(vdrivetrain.X[1, 0])
-    ul_plot.append(vdrivetrain.U[0, 0])
-    ur_plot.append(vdrivetrain.U[1, 0])
-    left_gear_plot.append((vdrivetrain.left_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
-    right_gear_plot.append((vdrivetrain.right_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
-
-    fwd_velocity = (vdrivetrain.X[1, 0] + vdrivetrain.X[0, 0]) / 2
-    turn_velocity = (vdrivetrain.X[1, 0] - vdrivetrain.X[0, 0])
-    if abs(fwd_velocity) < 0.0000001:
-      radius_plot.append(turn_velocity)
+        glog.debug('Left is low')
+    if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
+        glog.debug('Right is high')
     else:
-      radius_plot.append(turn_velocity / fwd_velocity)
+        glog.debug('Right is low')
 
-  # TODO(austin):
-  # Shifting compensation.
+    for t in numpy.arange(0, 1.7, vdrivetrain.dt):
+        if t < 0.5:
+            vdrivetrain.Update(throttle=0.00, steering=1.0)
+        elif t < 1.2:
+            vdrivetrain.Update(throttle=0.5, steering=1.0)
+        else:
+            vdrivetrain.Update(throttle=0.00, steering=1.0)
+        t_plot.append(t)
+        vl_plot.append(vdrivetrain.X[0, 0])
+        vr_plot.append(vdrivetrain.X[1, 0])
+        ul_plot.append(vdrivetrain.U[0, 0])
+        ur_plot.append(vdrivetrain.U[1, 0])
+        left_gear_plot.append(
+            (vdrivetrain.left_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
+        right_gear_plot.append(
+            (vdrivetrain.right_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
 
-  # Tighten the turn.
-  # Closed loop drive.
+        fwd_velocity = (vdrivetrain.X[1, 0] + vdrivetrain.X[0, 0]) / 2
+        turn_velocity = (vdrivetrain.X[1, 0] - vdrivetrain.X[0, 0])
+        if abs(fwd_velocity) < 0.0000001:
+            radius_plot.append(turn_velocity)
+        else:
+            radius_plot.append(turn_velocity / fwd_velocity)
 
-  pylab.plot(t_plot, vl_plot, label='left velocity')
-  pylab.plot(t_plot, vr_plot, label='right velocity')
-  pylab.plot(t_plot, ul_plot, label='left voltage')
-  pylab.plot(t_plot, ur_plot, label='right voltage')
-  pylab.plot(t_plot, radius_plot, label='radius')
-  pylab.plot(t_plot, left_gear_plot, label='left gear high')
-  pylab.plot(t_plot, right_gear_plot, label='right gear high')
-  pylab.legend()
-  pylab.show()
+    # TODO(austin):
+    # Shifting compensation.
+
+    # Tighten the turn.
+    # Closed loop drive.
+
+    pylab.plot(t_plot, vl_plot, label='left velocity')
+    pylab.plot(t_plot, vr_plot, label='right velocity')
+    pylab.plot(t_plot, ul_plot, label='left voltage')
+    pylab.plot(t_plot, ur_plot, label='right voltage')
+    pylab.plot(t_plot, radius_plot, label='radius')
+    pylab.plot(t_plot, left_gear_plot, label='left gear high')
+    pylab.plot(t_plot, right_gear_plot, label='right gear high')
+    pylab.legend()
+    pylab.show()
diff --git a/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py b/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py
index caa3082..839677e 100644
--- a/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py
+++ b/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py
@@ -39,11 +39,9 @@
 
     # Write the generated constants out to a file.
     if len(argv) != 5:
-        glog.fatal(
-            'Expected .h file name and .cc file name for the \
+        glog.fatal('Expected .h file name and .cc file name for the \
             static_zeroing_single_dof_profiled_subsystem_test and integral \
-            static_zeroing_single_dof_profiled_subsystem_test.'
-        )
+            static_zeroing_single_dof_profiled_subsystem_test.')
     else:
         namespaces = ['frc971', 'control_loops']
         linear_system.WriteLinearSystem(kIntake, argv[1:3], argv[3:5],