diff --git a/y2014/control_loops/python/claw.py b/y2014/control_loops/python/claw.py
index 77ccf14..488d792 100755
--- a/y2014/control_loops/python/claw.py
+++ b/y2014/control_loops/python/claw.py
@@ -51,8 +51,8 @@
         # Input is [bottom power, top power - bottom power * J_top / J_bottom]
         # Motor time constants. difference_bottom refers to the constant for how the
         # bottom velocity affects the difference of the top and bottom velocities.
-        self.common_motor_constant = -self.Kt / self.Kv / (
-            self.G * self.G * self.R)
+        self.common_motor_constant = -self.Kt / self.Kv / (self.G * self.G *
+                                                           self.R)
         self.bottom_bottom = self.common_motor_constant / self.J_bottom
         self.difference_bottom = -self.common_motor_constant * (
             1 / self.J_bottom - 1 / self.J_top)
@@ -96,7 +96,8 @@
                                                    self.B_continuous, self.dt)
 
         self.A_bottom, self.B_bottom = controls.c2d(self.A_bottom_cont,
-                                                    self.B_bottom_cont, self.dt)
+                                                    self.B_bottom_cont,
+                                                    self.dt)
         self.A_diff, self.B_diff = controls.c2d(self.A_diff_cont,
                                                 self.B_diff_cont, self.dt)
 
@@ -135,12 +136,12 @@
         glog.debug(str(lstsq_A))
         glog.debug('det %s', str(numpy.linalg.det(lstsq_A)))
 
-        out_x = numpy.linalg.lstsq(
-            lstsq_A,
-            numpy.matrix([[self.A[1, 2]], [self.A[3, 2]]]),
-            rcond=None)[0]
-        self.K[1, 2] = -lstsq_A[0, 0] * (
-            self.K[0, 2] - out_x[0]) / lstsq_A[0, 1] + out_x[1]
+        out_x = numpy.linalg.lstsq(lstsq_A,
+                                   numpy.matrix([[self.A[1, 2]],
+                                                 [self.A[3, 2]]]),
+                                   rcond=None)[0]
+        self.K[1, 2] = -lstsq_A[0, 0] * (self.K[0, 2] -
+                                         out_x[0]) / lstsq_A[0, 1] + out_x[1]
 
         glog.debug('K unaugmented')
         glog.debug(str(self.K))
@@ -181,8 +182,9 @@
         X_ss[2, 0] = 1 / (1 - A[2, 2]) * B[2, 0] * U[0, 0]
         #X_ss[3, 0] = X_ss[3, 0] * A[3, 3] + X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
         #X_ss[3, 0] * (1 - A[3, 3]) = X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
-        X_ss[3, 0] = 1 / (1 - A[3, 3]) * (
-            X_ss[2, 0] * A[3, 2] + B[3, 1] * U[1, 0] + B[3, 0] * U[0, 0])
+        X_ss[3,
+             0] = 1 / (1 - A[3, 3]) * (X_ss[2, 0] * A[3, 2] +
+                                       B[3, 1] * U[1, 0] + B[3, 0] * U[0, 0])
         #X_ss[3, 0] = 1 / (1 - A[3, 3]) / (1 - A[2, 2]) * B[2, 0] * U[0, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
         X_ss[0, 0] = A[0, 2] * X_ss[2, 0] + B[0, 0] * U[0, 0]
         X_ss[1, 0] = (A[1, 2] * X_ss[2, 0]) + (A[1, 3] * X_ss[3, 0]) + (
@@ -247,7 +249,8 @@
         self.rpl = .05
         self.ipl = 0.008
         self.PlaceObserverPoles([
-            self.rpl + 1j * self.ipl, 0.10, 0.09, self.rpl - 1j * self.ipl, 0.90
+            self.rpl + 1j * self.ipl, 0.10, 0.09, self.rpl - 1j * self.ipl,
+            0.90
         ])
         #print "A is"
         #print self.A
@@ -284,8 +287,8 @@
         numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]]),
         numpy.matrix([[12], [12], [12], [12]]))
 
-    if (bottom_u > claw.U_max[0, 0] or bottom_u < claw.U_min[0, 0] or
-            top_u > claw.U_max[0, 0] or top_u < claw.U_min[0, 0]):
+    if (bottom_u > claw.U_max[0, 0] or bottom_u < claw.U_min[0, 0]
+            or top_u > claw.U_max[0, 0] or top_u < claw.U_min[0, 0]):
 
         position_K = K[:, 0:2]
         velocity_K = K[:, 2:]
@@ -501,8 +504,8 @@
     else:
         namespaces = ['y2014', 'control_loops', 'claw']
         claw = Claw('Claw')
-        loop_writer = control_loop.ControlLoopWriter(
-            'Claw', [claw], namespaces=namespaces)
+        loop_writer = control_loop.ControlLoopWriter('Claw', [claw],
+                                                     namespaces=namespaces)
         loop_writer.AddConstant(
             control_loop.Constant('kClawMomentOfInertiaRatio', '%f',
                                   claw.J_top / claw.J_bottom))
diff --git a/y2014/control_loops/python/drivetrain.py b/y2014/control_loops/python/drivetrain.py
index cac236f..d72c222 100755
--- a/y2014/control_loops/python/drivetrain.py
+++ b/y2014/control_loops/python/drivetrain.py
@@ -11,30 +11,32 @@
 
 gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
 
-kDrivetrain = drivetrain.DrivetrainParams(J = 2.8,
-                                          mass = 68,
-                                          robot_radius = 0.647998644 / 2.0,
-                                          wheel_radius = .04445,
-                                          G_high = 28.0 / 50.0 * 18.0 / 50.0,
-                                          G_low = 18.0 / 60.0 * 18.0 / 50.0,
-                                          q_pos_low = 0.12,
-                                          q_pos_high = 0.12,
-                                          q_vel_low = 1.0,
-                                          q_vel_high = 1.0,
-                                          has_imu = False,
-                                          dt = 0.005,
-                                          controller_poles = [0.7, 0.7])
+kDrivetrain = drivetrain.DrivetrainParams(J=2.8,
+                                          mass=68,
+                                          robot_radius=0.647998644 / 2.0,
+                                          wheel_radius=.04445,
+                                          G_high=28.0 / 50.0 * 18.0 / 50.0,
+                                          G_low=18.0 / 60.0 * 18.0 / 50.0,
+                                          q_pos_low=0.12,
+                                          q_pos_high=0.12,
+                                          q_vel_low=1.0,
+                                          q_vel_high=1.0,
+                                          has_imu=False,
+                                          dt=0.005,
+                                          controller_poles=[0.7, 0.7])
+
 
 def main(argv):
-  argv = FLAGS(argv)
+    argv = FLAGS(argv)
 
-  if FLAGS.plot:
-    drivetrain.PlotDrivetrainMotions(kDrivetrain)
-  elif len(argv) != 5:
-    print("Expected .h file name and .cc file name")
-  else:
-    # Write the generated constants out to a file.
-    drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2014', kDrivetrain)
+    if FLAGS.plot:
+        drivetrain.PlotDrivetrainMotions(kDrivetrain)
+    elif len(argv) != 5:
+        print("Expected .h file name and .cc file name")
+    else:
+        # Write the generated constants out to a file.
+        drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2014', kDrivetrain)
+
 
 if __name__ == '__main__':
-  sys.exit(main(sys.argv))
+    sys.exit(main(sys.argv))
diff --git a/y2014/control_loops/python/dt_mpc.py b/y2014/control_loops/python/dt_mpc.py
index 0c229c1..2f13807 100755
--- a/y2014/control_loops/python/dt_mpc.py
+++ b/y2014/control_loops/python/dt_mpc.py
@@ -13,8 +13,9 @@
 #
 # Inital algorithm from http://www.ece.ufrgs.br/~fetter/icma05_608.pdf
 
+
 def cartesian_to_polar(X_cartesian):
-  """Converts a cartesian coordinate to polar.
+    """Converts a cartesian coordinate to polar.
 
   Args:
     X_cartesian, numpy.matrix[3, 1] with x, y, theta as rows.
@@ -22,13 +23,13 @@
   Returns:
     numpy.matrix[3, 1] with e, phi, alpha as rows.
   """
-  phi = numpy.arctan2(X_cartesian[1, 0], X_cartesian[0, 0])
-  return numpy.matrix([[numpy.hypot(X_cartesian[0, 0], X_cartesian[1, 0])],
-                       [phi],
-                       [X_cartesian[2, 0] - phi]])
+    phi = numpy.arctan2(X_cartesian[1, 0], X_cartesian[0, 0])
+    return numpy.matrix([[numpy.hypot(X_cartesian[0, 0], X_cartesian[1, 0])],
+                         [phi], [X_cartesian[2, 0] - phi]])
+
 
 def polar_to_cartesian(X_polar):
-  """Converts a polar coordinate to cartesian.
+    """Converts a polar coordinate to cartesian.
 
   Args:
     X_polar, numpy.matrix[3, 1] with e, phi, alpha as rows.
@@ -36,12 +37,13 @@
   Returns:
     numpy.matrix[3, 1] with x, y, theta as rows.
   """
-  return numpy.matrix([[X_polar[0, 0] * numpy.cos(X_polar[1, 0])],
-                       [X_polar[0, 0] * numpy.sin(X_polar[1, 0])],
-                       [X_polar[1, 0] + X_polar[2, 0]]])
+    return numpy.matrix([[X_polar[0, 0] * numpy.cos(X_polar[1, 0])],
+                         [X_polar[0, 0] * numpy.sin(X_polar[1, 0])],
+                         [X_polar[1, 0] + X_polar[2, 0]]])
+
 
 def simulate_dynamics(X_cartesian, U):
-  """Calculates the robot location after 1 timestep.
+    """Calculates the robot location after 1 timestep.
 
   Args:
     X_cartesian, numpy.matrix[3, 1] with the starting location in
@@ -51,15 +53,15 @@
   Returns:
     numpy.matrix[3, 1] with the cartesian coordinate.
   """
-  X_cartesian += numpy.matrix(
-      [[U[0, 0] * numpy.cos(X_cartesian[2, 0]) * dt],
-       [U[0, 0] * numpy.sin(X_cartesian[2, 0]) * dt],
-       [U[1, 0] * dt]])
+    X_cartesian += numpy.matrix([[U[0, 0] * numpy.cos(X_cartesian[2, 0]) * dt],
+                                 [U[0, 0] * numpy.sin(X_cartesian[2, 0]) * dt],
+                                 [U[1, 0] * dt]])
 
-  return X_cartesian
+    return X_cartesian
+
 
 def U_from_array(U_array):
-  """Converts the U array from the optimizer to a bunch of column vectors.
+    """Converts the U array from the optimizer to a bunch of column vectors.
 
   Args:
     U_array, numpy.array[N] The U coordinates in v, av, v, av, ...
@@ -67,10 +69,11 @@
   Returns:
     numpy.matrix[2, N/2] with [[v, v, v, ...], [av, av, av, ...]]
   """
-  return numpy.matrix(U_array).reshape((2, -1), order='F')
+    return numpy.matrix(U_array).reshape((2, -1), order='F')
+
 
 def U_to_array(U_matrix):
-  """Converts the U matrix to the U array for the optimizer.
+    """Converts the U matrix to the U array for the optimizer.
 
   Args:
     U_matrix, numpy.matrix[2, N/2] with [[v, v, v, ...], [av, av, av, ...]]
@@ -78,10 +81,11 @@
   Returns:
     numpy.array[N] The U coordinates in v, av, v, av, ...
   """
-  return numpy.array(U_matrix.reshape((1, -1), order='F'))
+    return numpy.array(U_matrix.reshape((1, -1), order='F'))
+
 
 def cost(U_array, X_cartesian):
-  """Computes the cost given the inital position and the U array.
+    """Computes the cost given the inital position and the U array.
 
   Args:
     U_array: numpy.array[N] The U coordinates.
@@ -91,91 +95,93 @@
   Returns:
     double, The quadratic cost of evaluating U.
   """
-  X_cartesian_mod = X_cartesian.copy()
-  U_matrix = U_from_array(U_array)
-  my_cost = 0
-  Q = numpy.matrix([[0.01, 0, 0],
-                    [0, 0.01, 0],
-                    [0, 0, 0.01]]) / dt / dt
-  R = numpy.matrix([[0.001, 0],
-                    [0, 0.001]]) / dt / dt
-  for U in U_matrix.T:
-    # TODO(austin): Let it go to the point from either side.
-    U = U.T
-    X_cartesian_mod = simulate_dynamics(X_cartesian_mod, U)
-    X_current_polar = cartesian_to_polar(X_cartesian_mod)
-    my_cost += U.T * R * U + X_current_polar.T * Q * X_current_polar
+    X_cartesian_mod = X_cartesian.copy()
+    U_matrix = U_from_array(U_array)
+    my_cost = 0
+    Q = numpy.matrix([[0.01, 0, 0], [0, 0.01, 0], [0, 0, 0.01]]) / dt / dt
+    R = numpy.matrix([[0.001, 0], [0, 0.001]]) / dt / dt
+    for U in U_matrix.T:
+        # TODO(austin): Let it go to the point from either side.
+        U = U.T
+        X_cartesian_mod = simulate_dynamics(X_cartesian_mod, U)
+        X_current_polar = cartesian_to_polar(X_cartesian_mod)
+        my_cost += U.T * R * U + X_current_polar.T * Q * X_current_polar
 
-  return my_cost
+    return my_cost
+
 
 if __name__ == '__main__':
-  X_cartesian = numpy.matrix([[-1.0],
-                              [-1.0],
-                              [0.0]])
-  x_array = []
-  y_array = []
-  theta_array = []
+    X_cartesian = numpy.matrix([[-1.0], [-1.0], [0.0]])
+    x_array = []
+    y_array = []
+    theta_array = []
 
-  e_array = []
-  phi_array = []
-  alpha_array = []
+    e_array = []
+    phi_array = []
+    alpha_array = []
 
-  cost_array = []
+    cost_array = []
 
-  time_array = []
-  u0_array = []
-  u1_array = []
+    time_array = []
+    u0_array = []
+    u1_array = []
 
-  num_steps = 20
+    num_steps = 20
 
-  U_array = numpy.zeros((num_steps * 2))
-  for i in range(400):
-    print('Iteration', i)
-    # Solve the NMPC
-    U_array, fx, _, _, _ = scipy.optimize.fmin_slsqp(
-        cost, U_array.copy(), bounds = [(-1, 1), (-7, 7)] * num_steps,
-        args=(X_cartesian,), iter=500, full_output=True)
-    U_matrix = U_from_array(U_array)
+    U_array = numpy.zeros((num_steps * 2))
+    for i in range(400):
+        print('Iteration', i)
+        # Solve the NMPC
+        U_array, fx, _, _, _ = scipy.optimize.fmin_slsqp(cost,
+                                                         U_array.copy(),
+                                                         bounds=[(-1, 1),
+                                                                 (-7, 7)] *
+                                                         num_steps,
+                                                         args=(X_cartesian, ),
+                                                         iter=500,
+                                                         full_output=True)
+        U_matrix = U_from_array(U_array)
 
-    # Simulate the dynamics
-    X_cartesian = simulate_dynamics(X_cartesian, U_matrix[:, 0])
+        # Simulate the dynamics
+        X_cartesian = simulate_dynamics(X_cartesian, U_matrix[:, 0])
 
-    # Save variables for plotting.
-    cost_array.append(fx[0, 0])
-    u0_array.append(U_matrix[0, 0])
-    u1_array.append(U_matrix[1, 0])
-    x_array.append(X_cartesian[0, 0])
-    y_array.append(X_cartesian[1, 0])
-    theta_array.append(X_cartesian[2, 0])
-    time_array.append(i * dt)
-    X_polar = cartesian_to_polar(X_cartesian)
-    e_array.append(X_polar[0, 0])
-    phi_array.append(X_polar[1, 0])
-    alpha_array.append(X_polar[2, 0])
+        # Save variables for plotting.
+        cost_array.append(fx[0, 0])
+        u0_array.append(U_matrix[0, 0])
+        u1_array.append(U_matrix[1, 0])
+        x_array.append(X_cartesian[0, 0])
+        y_array.append(X_cartesian[1, 0])
+        theta_array.append(X_cartesian[2, 0])
+        time_array.append(i * dt)
+        X_polar = cartesian_to_polar(X_cartesian)
+        e_array.append(X_polar[0, 0])
+        phi_array.append(X_polar[1, 0])
+        alpha_array.append(X_polar[2, 0])
 
-    U_array = U_to_array(numpy.hstack((U_matrix[:, 1:], numpy.matrix([[0], [0]]))))
+        U_array = U_to_array(
+            numpy.hstack((U_matrix[:, 1:], numpy.matrix([[0], [0]]))))
 
-    if fx < 1e-05:
-      print('Cost is', fx, 'after', i, 'cycles, finishing early')
-      break
+        if fx < 1e-05:
+            print('Cost is', fx, 'after', i, 'cycles, finishing early')
+            break
 
-  # Plot
-  pylab.figure('xy')
-  pylab.plot(x_array, y_array, label = 'trajectory')
+    # Plot
+    pylab.figure('xy')
+    pylab.plot(x_array, y_array, label='trajectory')
 
-  pylab.figure('time')
-  pylab.plot(time_array, x_array, label='x')
-  pylab.plot(time_array, y_array, label='y')
-  pylab.plot(time_array, theta_array, label = 'theta')
-  pylab.plot(time_array, e_array, label='e')
-  pylab.plot(time_array, phi_array, label='phi')
-  pylab.plot(time_array, alpha_array, label='alpha')
-  pylab.plot(time_array, cost_array, label='cost')
-  pylab.legend()
+    pylab.figure('time')
+    pylab.plot(time_array, x_array, label='x')
+    pylab.plot(time_array, y_array, label='y')
+    pylab.plot(time_array, theta_array, label='theta')
+    pylab.plot(time_array, e_array, label='e')
+    pylab.plot(time_array, phi_array, label='phi')
+    pylab.plot(time_array, alpha_array, label='alpha')
+    pylab.plot(time_array, cost_array, label='cost')
+    pylab.legend()
 
-  pylab.figure('u')
-  pylab.plot(time_array, u0_array, label='u0')
-  pylab.plot(time_array, u1_array, label='u1')
-  pylab.legend()
+    pylab.figure('u')
+    pylab.plot(time_array, u0_array, label='u0')
+    pylab.plot(time_array, u1_array, label='u1')
+    pylab.legend()
 
-  pylab.show()
+    pylab.show()
diff --git a/y2014/control_loops/python/extended_lqr.py b/y2014/control_loops/python/extended_lqr.py
index b3f2372..699dd30 100755
--- a/y2014/control_loops/python/extended_lqr.py
+++ b/y2014/control_loops/python/extended_lqr.py
@@ -17,8 +17,9 @@
 num_inputs = 2
 x_hat_initial = numpy.matrix([[0.10], [1.0], [0.0]])
 
+
 def dynamics(X, U):
-  """Calculates the dynamics for a 2 wheeled robot.
+    """Calculates the dynamics for a 2 wheeled robot.
 
   Args:
     X, numpy.matrix(3, 1), The state.  [x, y, theta]
@@ -27,29 +28,33 @@
   Returns:
     numpy.matrix(3, 1), The derivative of the dynamics.
   """
-  #return numpy.matrix([[X[1, 0]],
-  #                     [X[2, 0]],
-  #                     [U[0, 0]]])
-  return numpy.matrix([[(U[0, 0] + U[1, 0]) * numpy.cos(X[2, 0]) / 2.0],
-                       [(U[0, 0] + U[1, 0]) * numpy.sin(X[2, 0]) / 2.0],
-                       [(U[1, 0] - U[0, 0]) / width]])
+    #return numpy.matrix([[X[1, 0]],
+    #                     [X[2, 0]],
+    #                     [U[0, 0]]])
+    return numpy.matrix([[(U[0, 0] + U[1, 0]) * numpy.cos(X[2, 0]) / 2.0],
+                         [(U[0, 0] + U[1, 0]) * numpy.sin(X[2, 0]) / 2.0],
+                         [(U[1, 0] - U[0, 0]) / width]])
+
 
 def RungeKutta(f, x, dt):
-  """4th order RungeKutta integration of F starting at X."""
-  a = f(x)
-  b = f(x + dt / 2.0 * a)
-  c = f(x + dt / 2.0 * b)
-  d = f(x + dt * c)
-  return x + dt * (a + 2.0 * b + 2.0 * c + d) / 6.0
+    """4th order RungeKutta integration of F starting at X."""
+    a = f(x)
+    b = f(x + dt / 2.0 * a)
+    c = f(x + dt / 2.0 * b)
+    d = f(x + dt * c)
+    return x + dt * (a + 2.0 * b + 2.0 * c + d) / 6.0
+
 
 def discrete_dynamics(X, U):
-  return RungeKutta(lambda startingX: dynamics(startingX, U), X, dt)
+    return RungeKutta(lambda startingX: dynamics(startingX, U), X, dt)
+
 
 def inverse_discrete_dynamics(X, U):
-  return RungeKutta(lambda startingX: -dynamics(startingX, U), X, dt)
+    return RungeKutta(lambda startingX: -dynamics(startingX, U), X, dt)
+
 
 def numerical_jacobian_x(fn, X, U, epsilon=1e-4):
-  """Numerically estimates the jacobian around X, U in X.
+    """Numerically estimates the jacobian around X, U in X.
 
   Args:
     fn: A function of X, U.
@@ -62,20 +67,21 @@
     numpy.matrix(num_states, num_states), The jacobian of fn with X as the
       variable.
   """
-  num_states = X.shape[0]
-  nominal = fn(X, U)
-  answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_states)))
-  # It's more expensive, but +- epsilon will be more reliable
-  for i in range(0, num_states):
-    dX_plus = X.copy()
-    dX_plus[i] += epsilon
-    dX_minus = X.copy()
-    dX_minus[i] -= epsilon
-    answer[:, i] = (fn(dX_plus, U) - fn(dX_minus, U)) / epsilon / 2.0
-  return answer
+    num_states = X.shape[0]
+    nominal = fn(X, U)
+    answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_states)))
+    # It's more expensive, but +- epsilon will be more reliable
+    for i in range(0, num_states):
+        dX_plus = X.copy()
+        dX_plus[i] += epsilon
+        dX_minus = X.copy()
+        dX_minus[i] -= epsilon
+        answer[:, i] = (fn(dX_plus, U) - fn(dX_minus, U)) / epsilon / 2.0
+    return answer
+
 
 def numerical_jacobian_u(fn, X, U, epsilon=1e-4):
-  """Numerically estimates the jacobian around X, U in U.
+    """Numerically estimates the jacobian around X, U in U.
 
   Args:
     fn: A function of X, U.
@@ -88,48 +94,56 @@
     numpy.matrix(num_states, num_inputs), The jacobian of fn with U as the
       variable.
   """
-  num_states = X.shape[0]
-  num_inputs = U.shape[0]
-  nominal = fn(X, U)
-  answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_inputs)))
-  for i in range(0, num_inputs):
-    dU_plus = U.copy()
-    dU_plus[i] += epsilon
-    dU_minus = U.copy()
-    dU_minus[i] -= epsilon
-    answer[:, i] = (fn(X, dU_plus) - fn(X, dU_minus)) / epsilon / 2.0
-  return answer
+    num_states = X.shape[0]
+    num_inputs = U.shape[0]
+    nominal = fn(X, U)
+    answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_inputs)))
+    for i in range(0, num_inputs):
+        dU_plus = U.copy()
+        dU_plus[i] += epsilon
+        dU_minus = U.copy()
+        dU_minus[i] -= epsilon
+        answer[:, i] = (fn(X, dU_plus) - fn(X, dU_minus)) / epsilon / 2.0
+    return answer
+
 
 def numerical_jacobian_x_x(fn, X, U):
-  return numerical_jacobian_x(
-      lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T, X, U)
+    return numerical_jacobian_x(
+        lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T,
+        X, U)
+
 
 def numerical_jacobian_x_u(fn, X, U):
-  return numerical_jacobian_x(
-      lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T, X, U)
+    return numerical_jacobian_x(
+        lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T,
+        X, U)
+
 
 def numerical_jacobian_u_x(fn, X, U):
-  return numerical_jacobian_u(
-      lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T, X, U)
+    return numerical_jacobian_u(
+        lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T,
+        X, U)
+
 
 def numerical_jacobian_u_u(fn, X, U):
-  return numerical_jacobian_u(
-      lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T, X, U)
+    return numerical_jacobian_u(
+        lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T,
+        X, U)
+
 
 # Simple implementation for a quadratic cost function.
 class CostFunction:
-  def __init__(self):
-    self.num_states = num_states
-    self.num_inputs = num_inputs
-    self.dt = dt
-    self.Q = numpy.matrix([[0.1, 0, 0],
-                           [0, 0.6, 0],
-                           [0, 0, 0.1]]) / dt / dt
-    self.R = numpy.matrix([[0.40, 0],
-                           [0, 0.40]]) / dt / dt
 
-  def estimate_Q_final(self, X_hat):
-    """Returns the quadraticized final Q around X_hat.
+    def __init__(self):
+        self.num_states = num_states
+        self.num_inputs = num_inputs
+        self.dt = dt
+        self.Q = numpy.matrix([[0.1, 0, 0], [0, 0.6, 0], [0, 0, 0.1]
+                               ]) / dt / dt
+        self.R = numpy.matrix([[0.40, 0], [0, 0.40]]) / dt / dt
+
+    def estimate_Q_final(self, X_hat):
+        """Returns the quadraticized final Q around X_hat.
 
     This is calculated by evaluating partial^2 cost(X_hat) / (partial X * partial X)
 
@@ -139,11 +153,11 @@
     Result:
       numpy.matrix(self.num_states, self.num_states)
     """
-    zero_U = numpy.matrix(numpy.zeros((self.num_inputs, 1)))
-    return numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
+        zero_U = numpy.matrix(numpy.zeros((self.num_inputs, 1)))
+        return numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
 
-  def estimate_partial_cost_partial_x_final(self, X_hat):
-    """Returns \frac{\partial cost}{\partial X}(X_hat) for the final cost.
+    def estimate_partial_cost_partial_x_final(self, X_hat):
+        """Returns \frac{\partial cost}{\partial X}(X_hat) for the final cost.
 
     Args:
       X_hat: numpy.matrix(self.num_states, 1), The state to quadraticize around.
@@ -151,14 +165,17 @@
     Result:
       numpy.matrix(self.num_states, 1)
     """
-    return numerical_jacobian_x(self.final_cost, X_hat, numpy.matrix(numpy.zeros((self.num_inputs, 1)))).T
+        return numerical_jacobian_x(
+            self.final_cost, X_hat,
+            numpy.matrix(numpy.zeros((self.num_inputs, 1)))).T
 
-  def estimate_q_final(self, X_hat):
-    """Returns q evaluated at X_hat for the final cost function."""
-    return self.estimate_partial_cost_partial_x_final(X_hat) - self.estimate_Q_final(X_hat) * X_hat
+    def estimate_q_final(self, X_hat):
+        """Returns q evaluated at X_hat for the final cost function."""
+        return self.estimate_partial_cost_partial_x_final(
+            X_hat) - self.estimate_Q_final(X_hat) * X_hat
 
-  def final_cost(self, X, U):
-    """Computes the final cost of being at X
+    def final_cost(self, X, U):
+        """Computes the final cost of being at X
 
     Args:
       X: numpy.matrix(self.num_states, 1)
@@ -167,10 +184,10 @@
     Returns:
       numpy.matrix(1, 1), The quadratic cost of being at X
     """
-    return X.T * self.Q * X * 1000
+        return X.T * self.Q * X * 1000
 
-  def cost(self, X, U):
-    """Computes the incremental cost given a position and U.
+    def cost(self, X, U):
+        """Computes the incremental cost given a position and U.
 
     Args:
       X: numpy.matrix(self.num_states, 1)
@@ -179,250 +196,334 @@
     Returns:
       numpy.matrix(1, 1), The quadratic cost of evaluating U.
     """
-    return U.T * self.R * U + X.T * self.Q * X
+        return U.T * self.R * U + X.T * self.Q * X
+
 
 cost_fn_obj = CostFunction()
 
-S_bar_t = [numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)]
+S_bar_t = [
+    numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)
+]
 s_bar_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
 s_scalar_bar_t = [numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)]
 
-L_t = [numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)]
+L_t = [
+    numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)
+]
 l_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
-L_bar_t = [numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)]
+L_bar_t = [
+    numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)
+]
 l_bar_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
 
-S_t = [numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)]
+S_t = [
+    numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)
+]
 s_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
 s_scalar_t = [numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)]
 
-
-last_x_hat_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
+last_x_hat_t = [
+    numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)
+]
 
 for a in range(15):
-  x_hat = x_hat_initial
-  u_t = L_t[0] * x_hat + l_t[0]
-  S_bar_t[0] = numpy.matrix(numpy.zeros((num_states, num_states)))
-  s_bar_t[0] = numpy.matrix(numpy.zeros((num_states, 1)))
-  s_scalar_bar_t[0] = numpy.matrix([[0]])
+    x_hat = x_hat_initial
+    u_t = L_t[0] * x_hat + l_t[0]
+    S_bar_t[0] = numpy.matrix(numpy.zeros((num_states, num_states)))
+    s_bar_t[0] = numpy.matrix(numpy.zeros((num_states, 1)))
+    s_scalar_bar_t[0] = numpy.matrix([[0]])
 
-  last_x_hat_t[0] = x_hat_initial
+    last_x_hat_t[0] = x_hat_initial
 
-  Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat_initial, u_t)
-  P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat_initial, u_t)
-  R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat_initial, u_t)
+    Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat_initial, u_t)
+    P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat_initial, u_t)
+    R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat_initial, u_t)
 
-  q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat_initial, u_t).T - Q_t * x_hat_initial - P_t.T * u_t
-  r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat_initial, u_t).T - P_t * x_hat_initial - R_t * u_t
+    q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat_initial,
+                               u_t).T - Q_t * x_hat_initial - P_t.T * u_t
+    r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat_initial,
+                               u_t).T - P_t * x_hat_initial - R_t * u_t
 
-  q_scalar_t = cost_fn_obj.cost(x_hat_initial, u_t) - 0.5 * (x_hat_initial.T * (Q_t * x_hat_initial + P_t.T * u_t) + u_t.T * (P_t * x_hat_initial + R_t * u_t)) - x_hat_initial.T * q_t - u_t.T * r_t
+    q_scalar_t = cost_fn_obj.cost(
+        x_hat_initial,
+        u_t) - 0.5 * (x_hat_initial.T *
+                      (Q_t * x_hat_initial + P_t.T * u_t) + u_t.T *
+                      (P_t * x_hat_initial + R_t * u_t)
+                      ) - x_hat_initial.T * q_t - u_t.T * r_t
 
-  start_A_t = numerical_jacobian_x(discrete_dynamics, x_hat_initial, u_t)
-  start_B_t = numerical_jacobian_u(discrete_dynamics, x_hat_initial, u_t)
-  x_hat_next = discrete_dynamics(x_hat_initial, u_t)
-  start_c_t = x_hat_next - start_A_t * x_hat_initial - start_B_t * u_t
+    start_A_t = numerical_jacobian_x(discrete_dynamics, x_hat_initial, u_t)
+    start_B_t = numerical_jacobian_u(discrete_dynamics, x_hat_initial, u_t)
+    x_hat_next = discrete_dynamics(x_hat_initial, u_t)
+    start_c_t = x_hat_next - start_A_t * x_hat_initial - start_B_t * u_t
 
-  B_svd_u, B_svd_sigma_diag, B_svd_v = numpy.linalg.svd(start_B_t)
-  B_svd_sigma = numpy.matrix(numpy.zeros(start_B_t.shape))
-  B_svd_sigma[0:B_svd_sigma_diag.shape[0], 0:B_svd_sigma_diag.shape[0]] = numpy.diag(B_svd_sigma_diag)
+    B_svd_u, B_svd_sigma_diag, B_svd_v = numpy.linalg.svd(start_B_t)
+    B_svd_sigma = numpy.matrix(numpy.zeros(start_B_t.shape))
+    B_svd_sigma[0:B_svd_sigma_diag.shape[0],
+                0:B_svd_sigma_diag.shape[0]] = numpy.diag(B_svd_sigma_diag)
 
-  B_svd_sigma_inv = numpy.matrix(numpy.zeros(start_B_t.shape)).T
-  B_svd_sigma_inv[0:B_svd_sigma_diag.shape[0], 0:B_svd_sigma_diag.shape[0]] = numpy.linalg.inv(numpy.diag(B_svd_sigma_diag))
-  B_svd_inv = B_svd_v.T * B_svd_sigma_inv * B_svd_u.T
+    B_svd_sigma_inv = numpy.matrix(numpy.zeros(start_B_t.shape)).T
+    B_svd_sigma_inv[0:B_svd_sigma_diag.shape[0],
+                    0:B_svd_sigma_diag.shape[0]] = numpy.linalg.inv(
+                        numpy.diag(B_svd_sigma_diag))
+    B_svd_inv = B_svd_v.T * B_svd_sigma_inv * B_svd_u.T
 
-  L_bar_t[1] = B_svd_inv
-  l_bar_t[1] = -B_svd_inv * (start_A_t * x_hat_initial + start_c_t)
+    L_bar_t[1] = B_svd_inv
+    l_bar_t[1] = -B_svd_inv * (start_A_t * x_hat_initial + start_c_t)
 
-  S_bar_t[1] = L_bar_t[1].T * R_t * L_bar_t[1]
+    S_bar_t[1] = L_bar_t[1].T * R_t * L_bar_t[1]
 
-  TotalS_1 = start_B_t.T * S_t[1] * start_B_t + R_t
-  Totals_1 = start_B_t.T * S_t[1] * (start_c_t + start_A_t * x_hat_initial) + start_B_t.T * s_t[1] + P_t * x_hat_initial + r_t
-  Totals_scalar_1 = 0.5 * (start_c_t.T + x_hat_initial.T * start_A_t.T) * S_t[1] * (start_c_t + start_A_t * x_hat_initial) + s_scalar_t[1] + x_hat_initial.T * q_t + q_scalar_t + 0.5 * x_hat_initial.T * Q_t * x_hat_initial + (start_c_t.T + x_hat_initial.T * start_A_t.T) * s_t[1]
+    TotalS_1 = start_B_t.T * S_t[1] * start_B_t + R_t
+    Totals_1 = start_B_t.T * S_t[1] * (
+        start_c_t + start_A_t *
+        x_hat_initial) + start_B_t.T * s_t[1] + P_t * x_hat_initial + r_t
+    Totals_scalar_1 = 0.5 * (
+        start_c_t.T + x_hat_initial.T * start_A_t.T
+    ) * S_t[1] * (start_c_t + start_A_t * x_hat_initial) + s_scalar_t[
+        1] + x_hat_initial.T * q_t + q_scalar_t + 0.5 * x_hat_initial.T * Q_t * x_hat_initial + (
+            start_c_t.T + x_hat_initial.T * start_A_t.T) * s_t[1]
 
-  optimal_u_1 = -numpy.linalg.solve(TotalS_1, Totals_1)
-  optimal_x_1 = start_A_t * x_hat_initial + start_B_t * optimal_u_1 + start_c_t
+    optimal_u_1 = -numpy.linalg.solve(TotalS_1, Totals_1)
+    optimal_x_1 = start_A_t * x_hat_initial + start_B_t * optimal_u_1 + start_c_t
 
-  S_bar_1_eigh_eigenvalues, S_bar_1_eigh_eigenvectors = numpy.linalg.eigh(S_bar_t[1])
-  S_bar_1_eigh = numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues))
-  S_bar_1_eigh_eigenvalues_stiff = S_bar_1_eigh_eigenvalues.copy()
-  for i in range(S_bar_1_eigh_eigenvalues_stiff.shape[0]):
-    if abs(S_bar_1_eigh_eigenvalues_stiff[i]) < 1e-8:
-      S_bar_1_eigh_eigenvalues_stiff[i] = max(S_bar_1_eigh_eigenvalues_stiff) * 1.0
+    S_bar_1_eigh_eigenvalues, S_bar_1_eigh_eigenvectors = numpy.linalg.eigh(
+        S_bar_t[1])
+    S_bar_1_eigh = numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues))
+    S_bar_1_eigh_eigenvalues_stiff = S_bar_1_eigh_eigenvalues.copy()
+    for i in range(S_bar_1_eigh_eigenvalues_stiff.shape[0]):
+        if abs(S_bar_1_eigh_eigenvalues_stiff[i]) < 1e-8:
+            S_bar_1_eigh_eigenvalues_stiff[i] = max(
+                S_bar_1_eigh_eigenvalues_stiff) * 1.0
 
-  #print 'eigh eigenvalues of S bar', S_bar_1_eigh_eigenvalues
-  #print 'eigh eigenvectors of S bar', S_bar_1_eigh_eigenvectors.T
+    #print 'eigh eigenvalues of S bar', S_bar_1_eigh_eigenvalues
+    #print 'eigh eigenvectors of S bar', S_bar_1_eigh_eigenvectors.T
 
-  #print 'S bar eig recreate', S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues)) * S_bar_1_eigh_eigenvectors.T
-  #print 'S bar eig recreate error', (S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues)) * S_bar_1_eigh_eigenvectors.T - S_bar_t[1])
+    #print 'S bar eig recreate', S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues)) * S_bar_1_eigh_eigenvectors.T
+    #print 'S bar eig recreate error', (S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues)) * S_bar_1_eigh_eigenvectors.T - S_bar_t[1])
 
-  S_bar_stiff = S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues_stiff)) * S_bar_1_eigh_eigenvectors.T
+    S_bar_stiff = S_bar_1_eigh_eigenvectors * numpy.matrix(
+        numpy.diag(
+            S_bar_1_eigh_eigenvalues_stiff)) * S_bar_1_eigh_eigenvectors.T
 
-  print 'Min u', -numpy.linalg.solve(TotalS_1, Totals_1)
-  print 'Min x_hat', optimal_x_1
-  s_bar_t[1] = -s_t[1] - (S_bar_stiff + S_t[1]) * optimal_x_1
-  s_scalar_bar_t[1] = 0.5 * (optimal_u_1.T * TotalS_1 * optimal_u_1 - optimal_x_1.T * (S_bar_stiff + S_t[1]) * optimal_x_1) + optimal_u_1.T * Totals_1 - optimal_x_1.T * (s_bar_t[1] + s_t[1]) - s_scalar_t[1] + Totals_scalar_1
+    print 'Min u', -numpy.linalg.solve(TotalS_1, Totals_1)
+    print 'Min x_hat', optimal_x_1
+    s_bar_t[1] = -s_t[1] - (S_bar_stiff + S_t[1]) * optimal_x_1
+    s_scalar_bar_t[1] = 0.5 * (
+        optimal_u_1.T * TotalS_1 * optimal_u_1 - optimal_x_1.T *
+        (S_bar_stiff + S_t[1]) *
+        optimal_x_1) + optimal_u_1.T * Totals_1 - optimal_x_1.T * (
+            s_bar_t[1] + s_t[1]) - s_scalar_t[1] + Totals_scalar_1
 
-  print 'optimal_u_1', optimal_u_1
-  print 'TotalS_1', TotalS_1
-  print 'Totals_1', Totals_1
-  print 'Totals_scalar_1', Totals_scalar_1
-  print 'overall cost 1',  0.5 * (optimal_u_1.T * TotalS_1 * optimal_u_1) + optimal_u_1.T * Totals_1 + Totals_scalar_1
-  print 'overall cost 0',  0.5 * (x_hat_initial.T * S_t[0] * x_hat_initial) +  x_hat_initial.T * s_t[0] + s_scalar_t[0]
+    print 'optimal_u_1', optimal_u_1
+    print 'TotalS_1', TotalS_1
+    print 'Totals_1', Totals_1
+    print 'Totals_scalar_1', Totals_scalar_1
+    print 'overall cost 1', 0.5 * (
+        optimal_u_1.T * TotalS_1 *
+        optimal_u_1) + optimal_u_1.T * Totals_1 + Totals_scalar_1
+    print 'overall cost 0', 0.5 * (x_hat_initial.T * S_t[0] * x_hat_initial
+                                   ) + x_hat_initial.T * s_t[0] + s_scalar_t[0]
 
-  print 't forward 0'
-  print 'x_hat_initial[ 0]: %s' % (x_hat_initial)
-  print 'x_hat[%2d]: %s' % (0, x_hat.T)
-  print 'x_hat_next[%2d]: %s' % (0, x_hat_next.T)
-  print 'u[%2d]: %s' % (0, u_t.T)
-  print ('L[ 0]: %s' % (L_t[0],)).replace('\n', '\n        ')
-  print ('l[ 0]: %s' % (l_t[0],)).replace('\n', '\n        ')
+    print 't forward 0'
+    print 'x_hat_initial[ 0]: %s' % (x_hat_initial)
+    print 'x_hat[%2d]: %s' % (0, x_hat.T)
+    print 'x_hat_next[%2d]: %s' % (0, x_hat_next.T)
+    print 'u[%2d]: %s' % (0, u_t.T)
+    print('L[ 0]: %s' % (L_t[0], )).replace('\n', '\n        ')
+    print('l[ 0]: %s' % (l_t[0], )).replace('\n', '\n        ')
 
-  print ('A_t[%2d]: %s' % (0, start_A_t)).replace('\n', '\n         ')
-  print ('B_t[%2d]: %s' % (0, start_B_t)).replace('\n', '\n         ')
-  print ('c_t[%2d]: %s' % (0, start_c_t)).replace('\n', '\n         ')
+    print('A_t[%2d]: %s' % (0, start_A_t)).replace('\n', '\n         ')
+    print('B_t[%2d]: %s' % (0, start_B_t)).replace('\n', '\n         ')
+    print('c_t[%2d]: %s' % (0, start_c_t)).replace('\n', '\n         ')
 
-  # TODO(austin): optimal_x_1 is x_hat
-  x_hat = -numpy.linalg.solve((S_t[1] + S_bar_stiff), (s_t[1] + s_bar_t[1]))
-  print 'new xhat', x_hat
+    # TODO(austin): optimal_x_1 is x_hat
+    x_hat = -numpy.linalg.solve((S_t[1] + S_bar_stiff), (s_t[1] + s_bar_t[1]))
+    print 'new xhat', x_hat
 
-  S_bar_t[1] = S_bar_stiff
+    S_bar_t[1] = S_bar_stiff
 
-  last_x_hat_t[1] = x_hat
+    last_x_hat_t[1] = x_hat
 
-  for t in range(1, l):
-    print 't forward', t
-    u_t = L_t[t] * x_hat + l_t[t]
+    for t in range(1, l):
+        print 't forward', t
+        u_t = L_t[t] * x_hat + l_t[t]
 
-    x_hat_next = discrete_dynamics(x_hat, u_t)
-    A_bar_t = numerical_jacobian_x(inverse_discrete_dynamics, x_hat_next, u_t)
-    B_bar_t = numerical_jacobian_u(inverse_discrete_dynamics, x_hat_next, u_t)
-    c_bar_t = x_hat - A_bar_t * x_hat_next - B_bar_t * u_t
+        x_hat_next = discrete_dynamics(x_hat, u_t)
+        A_bar_t = numerical_jacobian_x(inverse_discrete_dynamics, x_hat_next,
+                                       u_t)
+        B_bar_t = numerical_jacobian_u(inverse_discrete_dynamics, x_hat_next,
+                                       u_t)
+        c_bar_t = x_hat - A_bar_t * x_hat_next - B_bar_t * u_t
 
-    print 'x_hat[%2d]: %s' % (t, x_hat.T)
-    print 'x_hat_next[%2d]: %s' % (t, x_hat_next.T)
-    print ('L[%2d]: %s' % (t, L_t[t],)).replace('\n', '\n        ')
-    print ('l[%2d]: %s' % (t, l_t[t],)).replace('\n', '\n        ')
-    print 'u[%2d]: %s' % (t, u_t.T)
+        print 'x_hat[%2d]: %s' % (t, x_hat.T)
+        print 'x_hat_next[%2d]: %s' % (t, x_hat_next.T)
+        print('L[%2d]: %s' % (
+            t,
+            L_t[t],
+        )).replace('\n', '\n        ')
+        print('l[%2d]: %s' % (
+            t,
+            l_t[t],
+        )).replace('\n', '\n        ')
+        print 'u[%2d]: %s' % (t, u_t.T)
 
-    print ('A_bar_t[%2d]: %s' % (t, A_bar_t)).replace('\n', '\n             ')
-    print ('B_bar_t[%2d]: %s' % (t, B_bar_t)).replace('\n', '\n             ')
-    print ('c_bar_t[%2d]: %s' % (t, c_bar_t)).replace('\n', '\n             ')
+        print('A_bar_t[%2d]: %s' % (t, A_bar_t)).replace(
+            '\n', '\n             ')
+        print('B_bar_t[%2d]: %s' % (t, B_bar_t)).replace(
+            '\n', '\n             ')
+        print('c_bar_t[%2d]: %s' % (t, c_bar_t)).replace(
+            '\n', '\n             ')
 
-    Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat, u_t)
-    P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat, u_t)
-    R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat, u_t)
+        Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat, u_t)
+        P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat, u_t)
+        R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat, u_t)
 
-    q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat, u_t).T - Q_t * x_hat - P_t.T * u_t
-    r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat, u_t).T - P_t * x_hat - R_t * u_t
+        q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat,
+                                   u_t).T - Q_t * x_hat - P_t.T * u_t
+        r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat,
+                                   u_t).T - P_t * x_hat - R_t * u_t
 
-    q_scalar_t = cost_fn_obj.cost(x_hat, u_t) - 0.5 * (x_hat.T * (Q_t * x_hat + P_t.T * u_t) + u_t.T * (P_t * x_hat + R_t * u_t)) - x_hat.T * q_t - u_t.T * r_t
+        q_scalar_t = cost_fn_obj.cost(x_hat, u_t) - 0.5 * (
+            x_hat.T * (Q_t * x_hat + P_t.T * u_t) + u_t.T *
+            (P_t * x_hat + R_t * u_t)) - x_hat.T * q_t - u_t.T * r_t
 
-    C_bar_t = B_bar_t.T * (S_bar_t[t] + Q_t) * A_bar_t + P_t * A_bar_t
-    D_bar_t = A_bar_t.T * (S_bar_t[t] + Q_t) * A_bar_t
-    E_bar_t = B_bar_t.T * (S_bar_t[t] + Q_t) * B_bar_t + R_t + P_t * B_bar_t + B_bar_t.T * P_t.T
-    d_bar_t = A_bar_t.T * (s_bar_t[t] + q_t) + A_bar_t.T * (S_bar_t[t] + Q_t) * c_bar_t
-    e_bar_t = r_t + P_t * c_bar_t + B_bar_t.T * s_bar_t[t] + B_bar_t.T * (S_bar_t[t] + Q_t) * c_bar_t
+        C_bar_t = B_bar_t.T * (S_bar_t[t] + Q_t) * A_bar_t + P_t * A_bar_t
+        D_bar_t = A_bar_t.T * (S_bar_t[t] + Q_t) * A_bar_t
+        E_bar_t = B_bar_t.T * (
+            S_bar_t[t] +
+            Q_t) * B_bar_t + R_t + P_t * B_bar_t + B_bar_t.T * P_t.T
+        d_bar_t = A_bar_t.T * (s_bar_t[t] + q_t) + A_bar_t.T * (S_bar_t[t] +
+                                                                Q_t) * c_bar_t
+        e_bar_t = r_t + P_t * c_bar_t + B_bar_t.T * s_bar_t[t] + B_bar_t.T * (
+            S_bar_t[t] + Q_t) * c_bar_t
 
-    L_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * C_bar_t
-    l_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * e_bar_t
+        L_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * C_bar_t
+        l_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * e_bar_t
 
-    S_bar_t[t + 1] = D_bar_t + C_bar_t.T * L_bar_t[t + 1]
-    s_bar_t[t + 1] = d_bar_t + C_bar_t.T * l_bar_t[t + 1]
-    s_scalar_bar_t[t + 1] = -0.5 * e_bar_t.T * numpy.linalg.inv(E_bar_t) * e_bar_t + 0.5 * c_bar_t.T * (S_bar_t[t] + Q_t) * c_bar_t + c_bar_t.T * s_bar_t[t] + c_bar_t.T * q_t + s_scalar_bar_t[t] + q_scalar_t
+        S_bar_t[t + 1] = D_bar_t + C_bar_t.T * L_bar_t[t + 1]
+        s_bar_t[t + 1] = d_bar_t + C_bar_t.T * l_bar_t[t + 1]
+        s_scalar_bar_t[t + 1] = -0.5 * e_bar_t.T * numpy.linalg.inv(
+            E_bar_t) * e_bar_t + 0.5 * c_bar_t.T * (
+                S_bar_t[t] + Q_t) * c_bar_t + c_bar_t.T * s_bar_t[
+                    t] + c_bar_t.T * q_t + s_scalar_bar_t[t] + q_scalar_t
 
-    x_hat = -numpy.linalg.solve((S_t[t + 1] + S_bar_t[t + 1]), (s_t[t + 1] + s_bar_t[t + 1]))
+        x_hat = -numpy.linalg.solve((S_t[t + 1] + S_bar_t[t + 1]),
+                                    (s_t[t + 1] + s_bar_t[t + 1]))
 
-  S_t[l] = cost_fn_obj.estimate_Q_final(x_hat)
-  s_t[l] = cost_fn_obj.estimate_q_final(x_hat)
-  x_hat = -numpy.linalg.inv(S_t[l] + S_bar_t[l]) * (s_t[l] + s_bar_t[l])
+    S_t[l] = cost_fn_obj.estimate_Q_final(x_hat)
+    s_t[l] = cost_fn_obj.estimate_q_final(x_hat)
+    x_hat = -numpy.linalg.inv(S_t[l] + S_bar_t[l]) * (s_t[l] + s_bar_t[l])
 
-  for t in reversed(range(l)):
-    print 't backward', t
-    # TODO(austin): I don't think we can use L_t like this here.
-    # I think we are off by 1 somewhere...
-    u_t = L_bar_t[t + 1] * x_hat + l_bar_t[t + 1]
+    for t in reversed(range(l)):
+        print 't backward', t
+        # TODO(austin): I don't think we can use L_t like this here.
+        # I think we are off by 1 somewhere...
+        u_t = L_bar_t[t + 1] * x_hat + l_bar_t[t + 1]
 
-    x_hat_prev = inverse_discrete_dynamics(x_hat, u_t)
-    print 'x_hat[%2d]: %s' % (t, x_hat.T)
-    print 'x_hat_prev[%2d]: %s' % (t, x_hat_prev.T)
-    print ('L_bar[%2d]: %s' % (t + 1, L_bar_t[t + 1])).replace('\n', '\n            ')
-    print ('l_bar[%2d]: %s' % (t + 1, l_bar_t[t + 1])).replace('\n', '\n            ')
-    print 'u[%2d]: %s' % (t, u_t.T)
-    # Now compute the linearized A, B, and C
-    # Start by doing it numerically, and then optimize.
-    A_t = numerical_jacobian_x(discrete_dynamics, x_hat_prev, u_t)
-    B_t = numerical_jacobian_u(discrete_dynamics, x_hat_prev, u_t)
-    c_t = x_hat - A_t * x_hat_prev - B_t * u_t
+        x_hat_prev = inverse_discrete_dynamics(x_hat, u_t)
+        print 'x_hat[%2d]: %s' % (t, x_hat.T)
+        print 'x_hat_prev[%2d]: %s' % (t, x_hat_prev.T)
+        print('L_bar[%2d]: %s' % (t + 1, L_bar_t[t + 1])).replace(
+            '\n', '\n            ')
+        print('l_bar[%2d]: %s' % (t + 1, l_bar_t[t + 1])).replace(
+            '\n', '\n            ')
+        print 'u[%2d]: %s' % (t, u_t.T)
+        # Now compute the linearized A, B, and C
+        # Start by doing it numerically, and then optimize.
+        A_t = numerical_jacobian_x(discrete_dynamics, x_hat_prev, u_t)
+        B_t = numerical_jacobian_u(discrete_dynamics, x_hat_prev, u_t)
+        c_t = x_hat - A_t * x_hat_prev - B_t * u_t
 
-    print ('A_t[%2d]: %s' % (t, A_t)).replace('\n', '\n         ')
-    print ('B_t[%2d]: %s' % (t, B_t)).replace('\n', '\n         ')
-    print ('c_t[%2d]: %s' % (t, c_t)).replace('\n', '\n         ')
+        print('A_t[%2d]: %s' % (t, A_t)).replace('\n', '\n         ')
+        print('B_t[%2d]: %s' % (t, B_t)).replace('\n', '\n         ')
+        print('c_t[%2d]: %s' % (t, c_t)).replace('\n', '\n         ')
 
-    Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat_prev, u_t)
-    P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat_prev, u_t)
-    P_T_t = numerical_jacobian_u_x(cost_fn_obj.cost, x_hat_prev, u_t)
-    R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat_prev, u_t)
+        Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat_prev, u_t)
+        P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat_prev, u_t)
+        P_T_t = numerical_jacobian_u_x(cost_fn_obj.cost, x_hat_prev, u_t)
+        R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat_prev, u_t)
 
-    q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat_prev, u_t).T - Q_t * x_hat_prev - P_T_t * u_t
-    r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat_prev, u_t).T - P_t * x_hat_prev - R_t * u_t
+        q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat_prev,
+                                   u_t).T - Q_t * x_hat_prev - P_T_t * u_t
+        r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat_prev,
+                                   u_t).T - P_t * x_hat_prev - R_t * u_t
 
-    q_scalar_t = cost_fn_obj.cost(x_hat_prev, u_t) - 0.5 * (x_hat_prev.T * (Q_t * x_hat_prev + P_t.T * u_t) + u_t.T * (P_t * x_hat_prev + R_t * u_t)) - x_hat_prev.T * q_t - u_t.T * r_t
+        q_scalar_t = cost_fn_obj.cost(x_hat_prev, u_t) - 0.5 * (
+            x_hat_prev.T * (Q_t * x_hat_prev + P_t.T * u_t) + u_t.T *
+            (P_t * x_hat_prev + R_t * u_t)) - x_hat_prev.T * q_t - u_t.T * r_t
 
-    C_t = P_t + B_t.T * S_t[t + 1] * A_t
-    D_t = Q_t + A_t.T * S_t[t + 1] * A_t
-    E_t = R_t + B_t.T * S_t[t + 1] * B_t
-    d_t = q_t + A_t.T * s_t[t + 1] + A_t.T * S_t[t + 1] * c_t
-    e_t = r_t + B_t.T * s_t[t + 1] + B_t.T * S_t[t + 1] * c_t
-    L_t[t] = -numpy.linalg.inv(E_t) * C_t
-    l_t[t] = -numpy.linalg.inv(E_t) * e_t
-    s_t[t] = d_t + C_t.T * l_t[t]
-    S_t[t] = D_t + C_t.T * L_t[t]
-    s_scalar_t[t] = q_scalar_t - 0.5 * e_t.T * numpy.linalg.inv(E_t) * e_t + 0.5 * c_t.T * S_t[t + 1] * c_t + c_t.T * s_t[t + 1] + s_scalar_t[t + 1]
+        C_t = P_t + B_t.T * S_t[t + 1] * A_t
+        D_t = Q_t + A_t.T * S_t[t + 1] * A_t
+        E_t = R_t + B_t.T * S_t[t + 1] * B_t
+        d_t = q_t + A_t.T * s_t[t + 1] + A_t.T * S_t[t + 1] * c_t
+        e_t = r_t + B_t.T * s_t[t + 1] + B_t.T * S_t[t + 1] * c_t
+        L_t[t] = -numpy.linalg.inv(E_t) * C_t
+        l_t[t] = -numpy.linalg.inv(E_t) * e_t
+        s_t[t] = d_t + C_t.T * l_t[t]
+        S_t[t] = D_t + C_t.T * L_t[t]
+        s_scalar_t[t] = q_scalar_t - 0.5 * e_t.T * numpy.linalg.inv(
+            E_t) * e_t + 0.5 * c_t.T * S_t[t + 1] * c_t + c_t.T * s_t[
+                t + 1] + s_scalar_t[t + 1]
 
-    x_hat = -numpy.linalg.solve((S_t[t] + S_bar_t[t]), (s_t[t] + s_bar_t[t]))
-    if t == 0:
-      last_x_hat_t[t] = x_hat_initial
-    else:
-      last_x_hat_t[t] = x_hat
+        x_hat = -numpy.linalg.solve((S_t[t] + S_bar_t[t]),
+                                    (s_t[t] + s_bar_t[t]))
+        if t == 0:
+            last_x_hat_t[t] = x_hat_initial
+        else:
+            last_x_hat_t[t] = x_hat
 
-  x_hat_t = [x_hat_initial]
+    x_hat_t = [x_hat_initial]
 
-  pylab.figure('states %d' % a)
-  pylab.ion()
-  for dim in range(num_states):
-    pylab.plot(numpy.arange(len(last_x_hat_t)),
-             [x_hat_loop[dim, 0] for x_hat_loop in last_x_hat_t], marker='o', label='Xhat[%d]'%dim)
-  pylab.legend()
-  pylab.draw()
+    pylab.figure('states %d' % a)
+    pylab.ion()
+    for dim in range(num_states):
+        pylab.plot(numpy.arange(len(last_x_hat_t)),
+                   [x_hat_loop[dim, 0] for x_hat_loop in last_x_hat_t],
+                   marker='o',
+                   label='Xhat[%d]' % dim)
+    pylab.legend()
+    pylab.draw()
 
-  pylab.figure('xy %d' % a)
-  pylab.ion()
-  pylab.plot([x_hat_loop[0, 0] for x_hat_loop in last_x_hat_t],
-             [x_hat_loop[1, 0] for x_hat_loop in last_x_hat_t], marker='o', label='trajectory')
-  pylab.legend()
-  pylab.draw()
+    pylab.figure('xy %d' % a)
+    pylab.ion()
+    pylab.plot([x_hat_loop[0, 0] for x_hat_loop in last_x_hat_t],
+               [x_hat_loop[1, 0] for x_hat_loop in last_x_hat_t],
+               marker='o',
+               label='trajectory')
+    pylab.legend()
+    pylab.draw()
 
 final_u_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
 cost_to_go = []
 cost_to_come = []
 cost = []
 for t in range(l):
-  cost_to_go.append((0.5 * last_x_hat_t[t].T * S_t[t] * last_x_hat_t[t] + last_x_hat_t[t].T * s_t[t] + s_scalar_t[t])[0, 0])
-  cost_to_come.append((0.5 * last_x_hat_t[t].T * S_bar_t[t] * last_x_hat_t[t] + last_x_hat_t[t].T * s_bar_t[t] + s_scalar_bar_t[t])[0, 0])
-  cost.append(cost_to_go[-1] + cost_to_come[-1])
-  final_u_t[t] = L_t[t] * last_x_hat_t[t] + l_t[t]
+    cost_to_go.append((0.5 * last_x_hat_t[t].T * S_t[t] * last_x_hat_t[t] +
+                       last_x_hat_t[t].T * s_t[t] + s_scalar_t[t])[0, 0])
+    cost_to_come.append(
+        (0.5 * last_x_hat_t[t].T * S_bar_t[t] * last_x_hat_t[t] +
+         last_x_hat_t[t].T * s_bar_t[t] + s_scalar_bar_t[t])[0, 0])
+    cost.append(cost_to_go[-1] + cost_to_come[-1])
+    final_u_t[t] = L_t[t] * last_x_hat_t[t] + l_t[t]
 
 for t in range(l):
-  A_t = numerical_jacobian_x(discrete_dynamics, last_x_hat_t[t], final_u_t[t])
-  B_t = numerical_jacobian_u(discrete_dynamics, last_x_hat_t[t], final_u_t[t])
-  c_t = discrete_dynamics(last_x_hat_t[t], final_u_t[t]) - A_t * last_x_hat_t[t] - B_t * final_u_t[t]
-  print("Infeasability at", t, "is", ((A_t * last_x_hat_t[t] + B_t * final_u_t[t] + c_t) - last_x_hat_t[t + 1]).T)
+    A_t = numerical_jacobian_x(discrete_dynamics, last_x_hat_t[t],
+                               final_u_t[t])
+    B_t = numerical_jacobian_u(discrete_dynamics, last_x_hat_t[t],
+                               final_u_t[t])
+    c_t = discrete_dynamics(
+        last_x_hat_t[t],
+        final_u_t[t]) - A_t * last_x_hat_t[t] - B_t * final_u_t[t]
+    print("Infeasability at", t, "is",
+          ((A_t * last_x_hat_t[t] + B_t * final_u_t[t] + c_t) -
+           last_x_hat_t[t + 1]).T)
 
 pylab.figure('u')
 samples = numpy.arange(len(final_u_t))
 for i in range(num_inputs):
-  pylab.plot(samples, [u[i, 0] for u in final_u_t], label='u[%d]' % i, marker='o')
-  pylab.legend()
+    pylab.plot(samples, [u[i, 0] for u in final_u_t],
+               label='u[%d]' % i,
+               marker='o')
+    pylab.legend()
 
 pylab.figure('cost')
 cost_samples = numpy.arange(len(cost))
diff --git a/y2014/control_loops/python/extended_lqr_derivation.py b/y2014/control_loops/python/extended_lqr_derivation.py
index 010c5de..6857654 100755
--- a/y2014/control_loops/python/extended_lqr_derivation.py
+++ b/y2014/control_loops/python/extended_lqr_derivation.py
@@ -6,7 +6,6 @@
 import random
 
 import sympy
-
 '''
 * `x_t1` means `x_{t + 1}`. Using `'x_t + 1'` as the symbol name makes the non-
   latex output really confusing, so not doing that.
@@ -57,351 +56,335 @@
 ub = sympy.MatrixSymbol('ubold', number_of_inputs, 1)
 
 CONSTANTS = set([
-    A_t, B_t, cb_t,
-    S_t1, sb_t1, s_t1,
-    A_tb, B_tb, cb_tb,
-    S_tb, sb_tb, s_tb,
-    P_t, Q_t, R_t, qb_t, rb_t, q_t,
-    ])
+    A_t,
+    B_t,
+    cb_t,
+    S_t1,
+    sb_t1,
+    s_t1,
+    A_tb,
+    B_tb,
+    cb_tb,
+    S_tb,
+    sb_tb,
+    s_tb,
+    P_t,
+    Q_t,
+    R_t,
+    qb_t,
+    rb_t,
+    q_t,
+])
 
 SYMMETRIC_CONSTANTS = set([
-    S_t1, S_tb,
-    Q_t, R_t,
-    ])
+    S_t1,
+    S_tb,
+    Q_t,
+    R_t,
+])
+
 
 def verify_equivalent(a, b, inverses={}):
-  def get_matrices(m):
-    matrices = m.atoms(sympy.MatrixSymbol)
-    new_matrices = set()
-    for matrix in matrices:
-      if matrix in inverses:
-        new_matrices.update(inverses[matrix].atoms(sympy.MatrixSymbol))
-    matrices.update(new_matrices)
-  a_matrices, b_matrices = get_matrices(a), get_matrices(b)
-  if a_matrices != b_matrices:
-    raise RuntimeError('matrices different: %s vs %s' % (a_matrices,
-                                                         b_matrices))
-  a_symbols, b_symbols = a.atoms(sympy.Symbol), b.atoms(sympy.Symbol)
-  if a_symbols != b_symbols:
-    raise RuntimeError('symbols different: %s vs %s' % (a_symbols, b_symbols))
-  if not a_symbols < DIMENSIONS:
-    raise RuntimeError('not sure what to do with %s' % (a_symbols - DIMENSIONS))
 
-  if a.shape != b.shape:
-    raise RuntimeError('Different shapes: %s and %s' % (a.shape, b.shape))
+    def get_matrices(m):
+        matrices = m.atoms(sympy.MatrixSymbol)
+        new_matrices = set()
+        for matrix in matrices:
+            if matrix in inverses:
+                new_matrices.update(inverses[matrix].atoms(sympy.MatrixSymbol))
+        matrices.update(new_matrices)
 
-  for _ in range(10):
-    subs_symbols = {s: random.randint(1, 5) for s in a_symbols}
+    a_matrices, b_matrices = get_matrices(a), get_matrices(b)
+    if a_matrices != b_matrices:
+        raise RuntimeError('matrices different: %s vs %s' %
+                           (a_matrices, b_matrices))
+    a_symbols, b_symbols = a.atoms(sympy.Symbol), b.atoms(sympy.Symbol)
+    if a_symbols != b_symbols:
+        raise RuntimeError('symbols different: %s vs %s' %
+                           (a_symbols, b_symbols))
+    if not a_symbols < DIMENSIONS:
+        raise RuntimeError('not sure what to do with %s' %
+                           (a_symbols - DIMENSIONS))
+
+    if a.shape != b.shape:
+        raise RuntimeError('Different shapes: %s and %s' % (a.shape, b.shape))
+
     for _ in range(10):
-      diff = a - b
-      subs_matrices = {}
-      def get_replacement(*args):
-        try:
-          m = sympy.MatrixSymbol(*args)
-          if m not in subs_matrices:
-            if m in inverses:
-              i = inverses[m].replace(sympy.MatrixSymbol, get_replacement, simultaneous=False)
-              i_evaled = sympy.ImmutableMatrix(i.rows, i.cols,
-                                               lambda x,y: i[x, y].evalf())
-              subs_matrices[m] = i_evaled.I
-            else:
-              rows = m.rows.subs(subs_symbols)
-              cols = m.cols.subs(subs_symbols)
-              new_m = sympy.ImmutableMatrix(rows, cols,
-                                            lambda i,j: random.uniform(-5, 5))
-              if m in SYMMETRIC_CONSTANTS:
-                if rows != cols:
-                  raise RuntimeError('Non-square symmetric matrix %s' % m)
-                def calculate_cell(i, j):
-                  if i > j:
-                    return new_m[i, j]
-                  else:
-                    return new_m[j, i]
-                new_m = sympy.ImmutableMatrix(rows, cols, calculate_cell)
-              subs_matrices[m] = new_m
-          return subs_matrices[m]
-        except AttributeError as e:
-          # Stupid sympy silently eats AttributeErrors and treats them as
-          # "no replacement"...
-          raise RuntimeError(e)
-      # subs fails when it tries doing matrix multiplies between fixed-size ones
-      # and the rest of the equation which still has the symbolic-sized ones.
-      # subs(simultaneous=True) wants to put dummies in for everything first,
-      # and Dummy().transpose() is broken.
-      # replace() has the same issue as subs with simultaneous being True.
-      # lambdify() has no idea what to do with the transposes if you replace all
-      # the matrices with ones of random sizes full of dummies.
-      diff = diff.replace(sympy.MatrixSymbol, get_replacement,
-                          simultaneous=False)
-      for row in range(diff.rows):
-        for col in range(diff.cols):
-          result = diff[row, col].evalf()
-          if abs(result) > 1e-7:
-            raise RuntimeError('difference at (%s, %s) is %s' % (row, col,
-                                                                 result))
+        subs_symbols = {s: random.randint(1, 5) for s in a_symbols}
+        for _ in range(10):
+            diff = a - b
+            subs_matrices = {}
+
+            def get_replacement(*args):
+                try:
+                    m = sympy.MatrixSymbol(*args)
+                    if m not in subs_matrices:
+                        if m in inverses:
+                            i = inverses[m].replace(sympy.MatrixSymbol,
+                                                    get_replacement,
+                                                    simultaneous=False)
+                            i_evaled = sympy.ImmutableMatrix(
+                                i.rows, i.cols, lambda x, y: i[x, y].evalf())
+                            subs_matrices[m] = i_evaled.I
+                        else:
+                            rows = m.rows.subs(subs_symbols)
+                            cols = m.cols.subs(subs_symbols)
+                            new_m = sympy.ImmutableMatrix(
+                                rows, cols, lambda i, j: random.uniform(-5, 5))
+                            if m in SYMMETRIC_CONSTANTS:
+                                if rows != cols:
+                                    raise RuntimeError(
+                                        'Non-square symmetric matrix %s' % m)
+
+                                def calculate_cell(i, j):
+                                    if i > j:
+                                        return new_m[i, j]
+                                    else:
+                                        return new_m[j, i]
+
+                                new_m = sympy.ImmutableMatrix(
+                                    rows, cols, calculate_cell)
+                            subs_matrices[m] = new_m
+                    return subs_matrices[m]
+                except AttributeError as e:
+                    # Stupid sympy silently eats AttributeErrors and treats them as
+                    # "no replacement"...
+                    raise RuntimeError(e)
+
+            # subs fails when it tries doing matrix multiplies between fixed-size ones
+            # and the rest of the equation which still has the symbolic-sized ones.
+            # subs(simultaneous=True) wants to put dummies in for everything first,
+            # and Dummy().transpose() is broken.
+            # replace() has the same issue as subs with simultaneous being True.
+            # lambdify() has no idea what to do with the transposes if you replace all
+            # the matrices with ones of random sizes full of dummies.
+            diff = diff.replace(sympy.MatrixSymbol,
+                                get_replacement,
+                                simultaneous=False)
+            for row in range(diff.rows):
+                for col in range(diff.cols):
+                    result = diff[row, col].evalf()
+                    if abs(result) > 1e-7:
+                        raise RuntimeError('difference at (%s, %s) is %s' %
+                                           (row, col, result))
+
 
 def verify_arguments(f, *args):
-  matrix_atoms = f.atoms(sympy.MatrixSymbol) - CONSTANTS
-  if matrix_atoms != set(args):
-    print('Arguments expected to be %s, but are %s, in:\n%s' % (
-        sorted(args), sorted(list(matrix_atoms)), f), file=sys.stderr)
-    raise RuntimeError
+    matrix_atoms = f.atoms(sympy.MatrixSymbol) - CONSTANTS
+    if matrix_atoms != set(args):
+        print('Arguments expected to be %s, but are %s, in:\n%s' %
+              (sorted(args), sorted(list(matrix_atoms)), f),
+              file=sys.stderr)
+        raise RuntimeError
+
 
 def make_c_t():
-  x_and_u = sympy.BlockMatrix(((xb,), (ub,)))
-  c_t = (half * x_and_u.transpose() *
-         sympy.BlockMatrix(((Q_t, P_t.T), (P_t, R_t))) * x_and_u +
-         x_and_u.transpose() * sympy.BlockMatrix(((qb_t,), (rb_t,))) +
-         q_t)
-  verify_arguments(c_t, xb, ub)
-  return c_t
+    x_and_u = sympy.BlockMatrix(((xb, ), (ub, )))
+    c_t = (half * x_and_u.transpose() * sympy.BlockMatrix(
+        ((Q_t, P_t.T),
+         (P_t, R_t))) * x_and_u + x_and_u.transpose() * sympy.BlockMatrix(
+             ((qb_t, ), (rb_t, ))) + q_t)
+    verify_arguments(c_t, xb, ub)
+    return c_t
+
 
 def check_backwards_cost():
-  g_t = A_t * xb_t + B_t * ub_t + cb_t
-  verify_arguments(g_t, xb_t, ub_t)
-  v_t1 = half * xb.transpose() * S_t1 * xb + xb.transpose() * sb_t1 + s_t1
-  verify_arguments(v_t1, xb)
-  v_t = (v_t1.subs(xb, g_t) + make_c_t()).subs({xb_t: xb, ub_t: ub})
-  verify_arguments(v_t, xb, ub)
+    g_t = A_t * xb_t + B_t * ub_t + cb_t
+    verify_arguments(g_t, xb_t, ub_t)
+    v_t1 = half * xb.transpose() * S_t1 * xb + xb.transpose() * sb_t1 + s_t1
+    verify_arguments(v_t1, xb)
+    v_t = (v_t1.subs(xb, g_t) + make_c_t()).subs({xb_t: xb, ub_t: ub})
+    verify_arguments(v_t, xb, ub)
 
-  v_t_for_cost = (
-      half * (
-          xb.transpose() * (A_t.transpose() * S_t1 * A_t + Q_t) * xb +
-          ub.transpose() * (B_t.transpose() * S_t1 * A_t + P_t) * xb +
-          xb.transpose() * (A_t.transpose() * S_t1 * B_t + P_t.T) * ub +
-          ub.transpose() * (B_t.transpose() * S_t1 * B_t + R_t) * ub) +
-      xb.transpose() * (A_t.transpose() * sb_t1 + qb_t) +
-      ub.transpose() * (B_t.transpose() * sb_t1 + rb_t) +
-      cb_t.transpose() * sb_t1 +
-      s_t1 + q_t +
-      half * (cb_t.transpose() * S_t1 * cb_t +
-    xb.transpose() * A_t.transpose() * S_t1 * cb_t +
-    ub.transpose() * B_t.transpose() * S_t1 * cb_t +
-    cb_t.transpose() * S_t1 * B_t * ub +
-              cb_t.transpose() * S_t1 * A_t * xb))
-  verify_equivalent(v_t, v_t_for_cost)
+    v_t_for_cost = (
+        half * (xb.transpose() *
+                (A_t.transpose() * S_t1 * A_t + Q_t) * xb + ub.transpose() *
+                (B_t.transpose() * S_t1 * A_t + P_t) * xb + xb.transpose() *
+                (A_t.transpose() * S_t1 * B_t + P_t.T) * ub + ub.transpose() *
+                (B_t.transpose() * S_t1 * B_t + R_t) * ub) + xb.transpose() *
+        (A_t.transpose() * sb_t1 + qb_t) + ub.transpose() *
+        (B_t.transpose() * sb_t1 + rb_t) + cb_t.transpose() * sb_t1 + s_t1 +
+        q_t + half *
+        (cb_t.transpose() * S_t1 * cb_t + xb.transpose() * A_t.transpose() *
+         S_t1 * cb_t + ub.transpose() * B_t.transpose() * S_t1 * cb_t +
+         cb_t.transpose() * S_t1 * B_t * ub +
+         cb_t.transpose() * S_t1 * A_t * xb))
+    verify_equivalent(v_t, v_t_for_cost)
 
-  v_t_now = (
-      half * (xb.T * (A_t.T * S_t1 * A_t + Q_t) * xb +
-              ub.T * (B_t.T * S_t1 * A_t + P_t) * xb +
-              xb.T * (A_t.T * S_t1 * B_t + P_t.T) * ub +
-              ub.T * (B_t.T * S_t1 * B_t + R_t) * ub) +
-      xb.T * (A_t.T * sb_t1 + qb_t) +
-      ub.T * (B_t.T * sb_t1 + rb_t) +
-      cb_t.T * sb_t1 + s_t1 + q_t +
-      half * (cb_t.T * S_t1 * cb_t +
-              xb.T * A_t.T * S_t1 * cb_t +
-              ub.T * B_t.T * S_t1 * cb_t +
-              cb_t.T * S_t1 * B_t * ub +
-              cb_t.T * S_t1 * A_t * xb))
+    v_t_now = (
+        half * (xb.T * (A_t.T * S_t1 * A_t + Q_t) * xb + ub.T *
+                (B_t.T * S_t1 * A_t + P_t) * xb + xb.T *
+                (A_t.T * S_t1 * B_t + P_t.T) * ub + ub.T *
+                (B_t.T * S_t1 * B_t + R_t) * ub) + xb.T *
+        (A_t.T * sb_t1 + qb_t) + ub.T * (B_t.T * sb_t1 + rb_t) +
+        cb_t.T * sb_t1 + s_t1 + q_t + half *
+        (cb_t.T * S_t1 * cb_t + xb.T * A_t.T * S_t1 * cb_t + ub.T * B_t.T *
+         S_t1 * cb_t + cb_t.T * S_t1 * B_t * ub + cb_t.T * S_t1 * A_t * xb))
 
-  v_t_now = (
-      half * (xb.T * (A_t.T * S_t1 * A_t + Q_t) * xb +
-              ub.T * (B_t.T * S_t1 * A_t + P_t) * xb +
-              xb.T * (A_t.T * S_t1 * B_t + P_t.T) * ub +
-              ub.T * (B_t.T * S_t1 * B_t + R_t) * ub) +
-      xb.T * (A_t.T * sb_t1 + qb_t + half * A_t.T * S_t1 * cb_t) +
-      ub.T * (B_t.T * sb_t1 + rb_t + half * B_t.T * S_t1 * cb_t) +
-      half * cb_t.T * S_t1 * (A_t * xb + B_t * ub + cb_t) +
-      cb_t.T * sb_t1 + s_t1 + q_t)
+    v_t_now = (half * (xb.T * (A_t.T * S_t1 * A_t + Q_t) * xb + ub.T *
+                       (B_t.T * S_t1 * A_t + P_t) * xb + xb.T *
+                       (A_t.T * S_t1 * B_t + P_t.T) * ub + ub.T *
+                       (B_t.T * S_t1 * B_t + R_t) * ub) + xb.T *
+               (A_t.T * sb_t1 + qb_t + half * A_t.T * S_t1 * cb_t) + ub.T *
+               (B_t.T * sb_t1 + rb_t + half * B_t.T * S_t1 * cb_t) +
+               half * cb_t.T * S_t1 * (A_t * xb + B_t * ub + cb_t) +
+               cb_t.T * sb_t1 + s_t1 + q_t)
 
-  v_t_now = (
-      half * (xb.T * (A_t.T * S_t1 * A_t + Q_t) * xb +
-              ub.T * (B_t.T * S_t1 * A_t + P_t) * xb +
-              xb.T * (A_t.T * S_t1 * B_t + P_t.T) * ub +
-              ub.T * (B_t.T * S_t1 * B_t + R_t) * ub) +
-      xb.T * (A_t.T * sb_t1 + qb_t + A_t.T * S_t1 * cb_t) +
-      ub.T * (B_t.T * sb_t1 + rb_t + B_t.T * S_t1 * cb_t) +
-      half * cb_t.T * S_t1 * cb_t +
-      cb_t.T * sb_t1 + s_t1 + q_t)
+    v_t_now = (half * (xb.T * (A_t.T * S_t1 * A_t + Q_t) * xb + ub.T *
+                       (B_t.T * S_t1 * A_t + P_t) * xb + xb.T *
+                       (A_t.T * S_t1 * B_t + P_t.T) * ub + ub.T *
+                       (B_t.T * S_t1 * B_t + R_t) * ub) + xb.T *
+               (A_t.T * sb_t1 + qb_t + A_t.T * S_t1 * cb_t) + ub.T *
+               (B_t.T * sb_t1 + rb_t + B_t.T * S_t1 * cb_t) +
+               half * cb_t.T * S_t1 * cb_t + cb_t.T * sb_t1 + s_t1 + q_t)
 
-  C_t = B_t.T * S_t1 * A_t + P_t
-  E_t = B_t.T * S_t1 * B_t + R_t
-  E_t_I = sympy.MatrixSymbol('E_t^-1', E_t.cols, E_t.rows)
-  L_t = -E_t_I * C_t
-  eb_t = B_t.T * S_t1 * cb_t + B_t.T * sb_t1 + rb_t
-  lb_t = -E_t_I * eb_t
-  D_t = A_t.T * S_t1 * A_t + Q_t
-  db_t = A_t.T * S_t1 * cb_t + A_t.T * sb_t1 + qb_t
+    C_t = B_t.T * S_t1 * A_t + P_t
+    E_t = B_t.T * S_t1 * B_t + R_t
+    E_t_I = sympy.MatrixSymbol('E_t^-1', E_t.cols, E_t.rows)
+    L_t = -E_t_I * C_t
+    eb_t = B_t.T * S_t1 * cb_t + B_t.T * sb_t1 + rb_t
+    lb_t = -E_t_I * eb_t
+    D_t = A_t.T * S_t1 * A_t + Q_t
+    db_t = A_t.T * S_t1 * cb_t + A_t.T * sb_t1 + qb_t
 
-  v_t_now = (
-      half * (xb.T * D_t * xb + ub.T * C_t * xb +
-              xb.T * C_t.T * ub + ub.T * E_t * ub) +
-      xb.T * db_t + ub.T * eb_t +
-      half * cb_t.T * S_t1 * cb_t +
-      cb_t.T * sb_t1 + s_t1 + q_t)
+    v_t_now = (half * (xb.T * D_t * xb + ub.T * C_t * xb + xb.T * C_t.T * ub +
+                       ub.T * E_t * ub) + xb.T * db_t + ub.T * eb_t +
+               half * cb_t.T * S_t1 * cb_t + cb_t.T * sb_t1 + s_t1 + q_t)
 
-  v_t_final = (
-      half * xb.T * (D_t + L_t.T * C_t + C_t.T * L_t + L_t.T * E_t * L_t) * xb +
-      xb.T * (C_t.T * lb_t + L_t.T * E_t * lb_t + db_t + L_t.T * eb_t) +
-      half * lb_t.T * E_t * lb_t +
-      lb_t.T * eb_t +
-      cb_t.T * sb_t1 + s_t1 + q_t + half * cb_t.T * S_t1 * cb_t
-      )
-  verify_arguments(v_t_final, xb, E_t_I)
-  verify_equivalent(v_t.subs(ub, L_t * xb + lb_t), v_t_final, {E_t_I: E_t})
+    v_t_final = (half * xb.T *
+                 (D_t + L_t.T * C_t + C_t.T * L_t + L_t.T * E_t * L_t) * xb +
+                 xb.T *
+                 (C_t.T * lb_t + L_t.T * E_t * lb_t + db_t + L_t.T * eb_t) +
+                 half * lb_t.T * E_t * lb_t + lb_t.T * eb_t + cb_t.T * sb_t1 +
+                 s_t1 + q_t + half * cb_t.T * S_t1 * cb_t)
+    verify_arguments(v_t_final, xb, E_t_I)
+    verify_equivalent(v_t.subs(ub, L_t * xb + lb_t), v_t_final, {E_t_I: E_t})
 
-  def v_t_from_s(this_S_t, this_sb_t, this_s_t):
-    return half * xb.T * this_S_t * xb + xb.T * this_sb_t + this_s_t
+    def v_t_from_s(this_S_t, this_sb_t, this_s_t):
+        return half * xb.T * this_S_t * xb + xb.T * this_sb_t + this_s_t
 
-  S_t_new_first = D_t + L_t.T * C_t + C_t.T * L_t + L_t.T * E_t * L_t
-  sb_t_new_first = db_t - C_t.T * E_t_I * eb_t
-  s_t_new_first = (half * lb_t.T * E_t * lb_t +
-                   lb_t.T * eb_t +
-                   cb_t.T * sb_t1 +
-                   s_t1 + q_t +
-                   half * cb_t.T * S_t1 * cb_t)
-  verify_equivalent(v_t_from_s(S_t_new_first, sb_t_new_first, s_t_new_first),
-                    v_t_final, {E_t_I: E_t})
+    S_t_new_first = D_t + L_t.T * C_t + C_t.T * L_t + L_t.T * E_t * L_t
+    sb_t_new_first = db_t - C_t.T * E_t_I * eb_t
+    s_t_new_first = (half * lb_t.T * E_t * lb_t + lb_t.T * eb_t +
+                     cb_t.T * sb_t1 + s_t1 + q_t + half * cb_t.T * S_t1 * cb_t)
+    verify_equivalent(v_t_from_s(S_t_new_first, sb_t_new_first, s_t_new_first),
+                      v_t_final, {E_t_I: E_t})
 
-  S_t_new_end = D_t - C_t.T * E_t_I * C_t
-  sb_t_new_end = db_t - C_t.T * E_t_I * eb_t
-  s_t_new_end = (q_t - half * eb_t.T * E_t_I * eb_t +
-                 half * cb_t.T * S_t1 * cb_t + cb_t.T * sb_t1 + s_t1)
-  verify_equivalent(v_t_from_s(S_t_new_end, sb_t_new_end, s_t_new_end),
-                    v_t_final, {E_t_I: E_t})
+    S_t_new_end = D_t - C_t.T * E_t_I * C_t
+    sb_t_new_end = db_t - C_t.T * E_t_I * eb_t
+    s_t_new_end = (q_t - half * eb_t.T * E_t_I * eb_t +
+                   half * cb_t.T * S_t1 * cb_t + cb_t.T * sb_t1 + s_t1)
+    verify_equivalent(v_t_from_s(S_t_new_end, sb_t_new_end, s_t_new_end),
+                      v_t_final, {E_t_I: E_t})
+
 
 def check_forwards_cost():
-  v_tb = half * xb.T * S_tb * xb + xb.T * sb_tb + s_tb
-  verify_arguments(v_tb, xb)
-  g_tb = A_tb * xb_t1 + B_tb * ub + cb_tb
-  verify_arguments(g_tb, xb_t1, ub)
-  c_t1b = make_c_t().subs(xb, g_tb)
-  verify_arguments(c_t1b, xb_t1, ub)
-  v_t1b = v_tb.subs(xb, g_tb) + c_t1b
-  verify_arguments(v_t1b, xb_t1, ub)
+    v_tb = half * xb.T * S_tb * xb + xb.T * sb_tb + s_tb
+    verify_arguments(v_tb, xb)
+    g_tb = A_tb * xb_t1 + B_tb * ub + cb_tb
+    verify_arguments(g_tb, xb_t1, ub)
+    c_t1b = make_c_t().subs(xb, g_tb)
+    verify_arguments(c_t1b, xb_t1, ub)
+    v_t1b = v_tb.subs(xb, g_tb) + c_t1b
+    verify_arguments(v_t1b, xb_t1, ub)
 
-  v_t1b_now = (
-      half * g_tb.T * S_tb * g_tb +
-      g_tb.T * sb_tb + s_tb +
-      half * (g_tb.T * Q_t * g_tb +
-              ub.T * P_t * g_tb +
-              g_tb.T * P_t.T * ub +
-              ub.T * R_t * ub) +
-      g_tb.T * qb_t + ub.T * rb_t + q_t)
+    v_t1b_now = (half * g_tb.T * S_tb * g_tb + g_tb.T * sb_tb + s_tb + half *
+                 (g_tb.T * Q_t * g_tb + ub.T * P_t * g_tb +
+                  g_tb.T * P_t.T * ub + ub.T * R_t * ub) + g_tb.T * qb_t +
+                 ub.T * rb_t + q_t)
 
-  v_t1b_for_cost = (
-      half * (xb_t1.T * A_tb.T * (S_tb + Q_t) * A_tb * xb_t1 +
-              xb_t1.T * A_tb.T * (S_tb + Q_t) * B_tb * ub +
-              xb_t1.T * A_tb.T * (S_tb + Q_t) * cb_tb +
-              ub.T * B_tb.T * (S_tb + Q_t) * A_tb * xb_t1 +
-              ub.T * B_tb.T * (S_tb + Q_t) * B_tb * ub +
-              ub.T * B_tb.T * (S_tb + Q_t) * cb_tb +
-              cb_tb.T * (S_tb + Q_t) * A_tb * xb_t1 +
-              cb_tb.T * (S_tb + Q_t) * B_tb * ub +
-              cb_tb.T * (S_tb + Q_t) * cb_tb) +
-      xb_t1.T * A_tb.T * sb_tb +
-      ub.T * B_tb.T * sb_tb +
-      cb_tb.T * sb_tb +
-      s_tb +
-      half * (ub.T * P_t * A_tb * xb_t1 +
-              ub.T * P_t * B_tb * ub +
-              ub.T * P_t * cb_tb) +
-      half * (xb_t1.T * A_tb.T * P_t.T * ub +
-              ub.T * B_tb.T * P_t.T * ub +
-              cb_tb.T * P_t.T * ub) +
-      half * ub.T * R_t * ub +
-      xb_t1.T * A_tb.T * qb_t + ub.T * B_tb.T * qb_t + cb_tb.T * qb_t +
-      ub.T * rb_t + q_t)
-  verify_equivalent(v_t1b, v_t1b_for_cost)
+    v_t1b_for_cost = (half * (xb_t1.T * A_tb.T *
+                              (S_tb + Q_t) * A_tb * xb_t1 + xb_t1.T * A_tb.T *
+                              (S_tb + Q_t) * B_tb * ub + xb_t1.T * A_tb.T *
+                              (S_tb + Q_t) * cb_tb + ub.T * B_tb.T *
+                              (S_tb + Q_t) * A_tb * xb_t1 + ub.T * B_tb.T *
+                              (S_tb + Q_t) * B_tb * ub + ub.T * B_tb.T *
+                              (S_tb + Q_t) * cb_tb + cb_tb.T *
+                              (S_tb + Q_t) * A_tb * xb_t1 + cb_tb.T *
+                              (S_tb + Q_t) * B_tb * ub + cb_tb.T *
+                              (S_tb + Q_t) * cb_tb) +
+                      xb_t1.T * A_tb.T * sb_tb + ub.T * B_tb.T * sb_tb +
+                      cb_tb.T * sb_tb + s_tb + half *
+                      (ub.T * P_t * A_tb * xb_t1 + ub.T * P_t * B_tb * ub +
+                       ub.T * P_t * cb_tb) + half *
+                      (xb_t1.T * A_tb.T * P_t.T * ub +
+                       ub.T * B_tb.T * P_t.T * ub + cb_tb.T * P_t.T * ub) +
+                      half * ub.T * R_t * ub + xb_t1.T * A_tb.T * qb_t +
+                      ub.T * B_tb.T * qb_t + cb_tb.T * qb_t + ub.T * rb_t +
+                      q_t)
+    verify_equivalent(v_t1b, v_t1b_for_cost)
 
-  S_and_Q = S_tb + Q_t
+    S_and_Q = S_tb + Q_t
 
-  v_t1b_now = (
-      half * (xb_t1.T * A_tb.T * S_and_Q * A_tb * xb_t1 +
-              xb_t1.T * A_tb.T * S_and_Q * B_tb * ub +
-              xb_t1.T * A_tb.T * S_and_Q * cb_tb +
-              ub.T * B_tb.T * S_and_Q * A_tb * xb_t1 +
-              ub.T * B_tb.T * S_and_Q * B_tb * ub +
-              ub.T * B_tb.T * S_and_Q * cb_tb +
-              cb_tb.T * S_and_Q * A_tb * xb_t1 +
-              cb_tb.T * S_and_Q * B_tb * ub +
-              cb_tb.T * S_and_Q * cb_tb) +
-      xb_t1.T * A_tb.T * sb_tb +
-      ub.T * B_tb.T * sb_tb +
-      cb_tb.T * sb_tb +
-      s_tb +
-      half * (ub.T * P_t * A_tb * xb_t1 +
-              ub.T * P_t * B_tb * ub +
-              ub.T * P_t * cb_tb) +
-      half * (xb_t1.T * A_tb.T * P_t.T * ub +
-              ub.T * B_tb.T * P_t.T * ub +
-              cb_tb.T * P_t.T * ub) +
-      half * ub.T * R_t * ub +
-      xb_t1.T * A_tb.T * qb_t +
-      ub.T * B_tb.T * qb_t +
-      cb_tb.T * qb_t +
-      ub.T * rb_t +
-      q_t)
+    v_t1b_now = (
+        half *
+        (xb_t1.T * A_tb.T * S_and_Q * A_tb * xb_t1 + xb_t1.T * A_tb.T *
+         S_and_Q * B_tb * ub + xb_t1.T * A_tb.T * S_and_Q * cb_tb + ub.T *
+         B_tb.T * S_and_Q * A_tb * xb_t1 + ub.T * B_tb.T * S_and_Q * B_tb * ub
+         + ub.T * B_tb.T * S_and_Q * cb_tb + cb_tb.T * S_and_Q * A_tb * xb_t1 +
+         cb_tb.T * S_and_Q * B_tb * ub + cb_tb.T * S_and_Q * cb_tb) +
+        xb_t1.T * A_tb.T * sb_tb + ub.T * B_tb.T * sb_tb + cb_tb.T * sb_tb +
+        s_tb + half * (ub.T * P_t * A_tb * xb_t1 + ub.T * P_t * B_tb * ub +
+                       ub.T * P_t * cb_tb) + half *
+        (xb_t1.T * A_tb.T * P_t.T * ub + ub.T * B_tb.T * P_t.T * ub +
+         cb_tb.T * P_t.T * ub) + half * ub.T * R_t * ub +
+        xb_t1.T * A_tb.T * qb_t + ub.T * B_tb.T * qb_t + cb_tb.T * qb_t +
+        ub.T * rb_t + q_t)
 
-  C_tb = B_tb.T * S_and_Q * A_tb + P_t * A_tb
-  E_tb = B_tb.T * S_and_Q * B_tb + B_tb.T * P_t.T + P_t * B_tb + R_t
-  E_tb_I = sympy.MatrixSymbol('Ebar_t^-1', E_tb.cols, E_tb.rows)
-  L_tb = -E_tb_I * C_tb
-  eb_tb = B_tb.T * S_and_Q * cb_tb + B_tb.T * sb_tb + P_t * cb_tb + B_tb.T * qb_t + rb_t
-  lb_tb = -E_tb_I * eb_tb
-  D_tb = A_tb.T * S_and_Q * A_tb
-  db_tb = A_tb.T * S_and_Q * cb_tb + A_tb.T * (sb_tb + qb_t)
+    C_tb = B_tb.T * S_and_Q * A_tb + P_t * A_tb
+    E_tb = B_tb.T * S_and_Q * B_tb + B_tb.T * P_t.T + P_t * B_tb + R_t
+    E_tb_I = sympy.MatrixSymbol('Ebar_t^-1', E_tb.cols, E_tb.rows)
+    L_tb = -E_tb_I * C_tb
+    eb_tb = B_tb.T * S_and_Q * cb_tb + B_tb.T * sb_tb + P_t * cb_tb + B_tb.T * qb_t + rb_t
+    lb_tb = -E_tb_I * eb_tb
+    D_tb = A_tb.T * S_and_Q * A_tb
+    db_tb = A_tb.T * S_and_Q * cb_tb + A_tb.T * (sb_tb + qb_t)
 
-  v_t1b_now = (
-      half * (xb_t1.T * D_tb * xb_t1 +
-              xb_t1.T * C_tb.T * ub +
-              ub.T * C_tb * xb_t1 +
-              ub.T * E_tb * ub) +
-      xb_t1.T * db_tb +
-      ub.T * eb_tb +
-      half * cb_tb.T * S_and_Q * cb_tb +
-      cb_tb.T * sb_tb +
-      cb_tb.T * qb_t +
-      s_tb + q_t)
+    v_t1b_now = (half * (xb_t1.T * D_tb * xb_t1 + xb_t1.T * C_tb.T * ub +
+                         ub.T * C_tb * xb_t1 + ub.T * E_tb * ub) +
+                 xb_t1.T * db_tb + ub.T * eb_tb +
+                 half * cb_tb.T * S_and_Q * cb_tb + cb_tb.T * sb_tb +
+                 cb_tb.T * qb_t + s_tb + q_t)
 
-  v_t1b_final = (
-      half * xb_t1.T * (D_tb - C_tb.T * E_tb_I * C_tb) * xb_t1 +
-      xb_t1.T * (db_tb - C_tb.T * E_tb_I * eb_tb) +
-      -half * eb_tb.T * E_tb_I * eb_tb +
-      half * cb_tb.T * S_and_Q * cb_tb +
-      cb_tb.T * sb_tb +
-      cb_tb.T * qb_t +
-      s_tb + q_t)
-  verify_arguments(v_t1b_final, xb_t1, E_tb_I)
-  verify_equivalent(v_t1b.subs(ub, -E_tb_I * C_tb * xb_t1 - E_tb_I * eb_tb),
-                    v_t1b_final, {E_tb_I: E_tb})
+    v_t1b_final = (half * xb_t1.T * (D_tb - C_tb.T * E_tb_I * C_tb) * xb_t1 +
+                   xb_t1.T * (db_tb - C_tb.T * E_tb_I * eb_tb) +
+                   -half * eb_tb.T * E_tb_I * eb_tb +
+                   half * cb_tb.T * S_and_Q * cb_tb + cb_tb.T * sb_tb +
+                   cb_tb.T * qb_t + s_tb + q_t)
+    verify_arguments(v_t1b_final, xb_t1, E_tb_I)
+    verify_equivalent(v_t1b.subs(ub, -E_tb_I * C_tb * xb_t1 - E_tb_I * eb_tb),
+                      v_t1b_final, {E_tb_I: E_tb})
+
 
 def check_forwards_u():
-  S_and_Q = S_tb + Q_t
+    S_and_Q = S_tb + Q_t
 
-  diff_start = (
-      half * (xb_t1.T * A_tb.T * S_and_Q * B_tb +
-              (B_tb.T * S_and_Q * A_tb * xb_t1).T +
-              2 * ub.T * B_tb.T * S_and_Q * B_tb +
-              (B_tb.T * S_and_Q * cb_tb).T +
-              cb_tb.T * S_and_Q * B_tb) +
-      sb_tb.T * B_tb +
-      half * (P_t * A_tb * xb_t1).T +
-      half * xb_t1.T * A_tb.T * P_t.T +
-      half * ub.T * (P_t * B_tb + B_tb.T * P_t.T) +
-      half * ub.T * (B_tb.T * P_t.T + P_t * B_tb) +
-      half * (P_t * cb_tb).T +
-      half * cb_tb.T * P_t.T +
-      ub.T * R_t +
-      (B_tb.T * qb_t).T + rb_t.T)
-  verify_arguments(diff_start, xb_t1, ub)
+    diff_start = (half *
+                  (xb_t1.T * A_tb.T * S_and_Q * B_tb +
+                   (B_tb.T * S_and_Q * A_tb * xb_t1).T +
+                   2 * ub.T * B_tb.T * S_and_Q * B_tb +
+                   (B_tb.T * S_and_Q * cb_tb).T + cb_tb.T * S_and_Q * B_tb) +
+                  sb_tb.T * B_tb + half * (P_t * A_tb * xb_t1).T +
+                  half * xb_t1.T * A_tb.T * P_t.T + half * ub.T *
+                  (P_t * B_tb + B_tb.T * P_t.T) + half * ub.T *
+                  (B_tb.T * P_t.T + P_t * B_tb) + half * (P_t * cb_tb).T +
+                  half * cb_tb.T * P_t.T + ub.T * R_t + (B_tb.T * qb_t).T +
+                  rb_t.T)
+    verify_arguments(diff_start, xb_t1, ub)
 
-  diff_end = (
-      xb_t1.T * (A_tb.T * S_and_Q * B_tb + A_tb.T * P_t.T) +
-      ub.T * (B_tb.T * S_and_Q * B_tb + B_tb.T * P_t.T + P_t * B_tb + R_t) +
-      cb_tb.T * S_and_Q * B_tb +
-      sb_tb.T * B_tb +
-      cb_tb.T * P_t.T +
-      qb_t.T * B_tb +
-      rb_t.T)
-  verify_equivalent(diff_start, diff_end)
+    diff_end = (xb_t1.T * (A_tb.T * S_and_Q * B_tb + A_tb.T * P_t.T) + ub.T *
+                (B_tb.T * S_and_Q * B_tb + B_tb.T * P_t.T + P_t * B_tb + R_t) +
+                cb_tb.T * S_and_Q * B_tb + sb_tb.T * B_tb + cb_tb.T * P_t.T +
+                qb_t.T * B_tb + rb_t.T)
+    verify_equivalent(diff_start, diff_end)
+
 
 def main():
-  sympy.init_printing(use_unicode=True)
-  check_backwards_cost()
-  check_forwards_cost()
-  check_forwards_u()
+    sympy.init_printing(use_unicode=True)
+    check_backwards_cost()
+    check_forwards_cost()
+    check_forwards_u()
+
 
 if __name__ == '__main__':
-  main()
+    main()
diff --git a/y2014/control_loops/python/polydrivetrain.py b/y2014/control_loops/python/polydrivetrain.py
index cdaee1b..05b6658 100755
--- a/y2014/control_loops/python/polydrivetrain.py
+++ b/y2014/control_loops/python/polydrivetrain.py
@@ -12,20 +12,22 @@
 FLAGS = gflags.FLAGS
 
 try:
-  gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+    gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
 except gflags.DuplicateFlagError:
-  pass
+    pass
+
 
 def main(argv):
-  if FLAGS.plot:
-    polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
-  elif len(argv) != 7:
-    glog.fatal('Expected .h file name and .cc file name')
-  else:
-    polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7], 'y2014',
-                                       drivetrain.kDrivetrain)
+    if FLAGS.plot:
+        polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+    elif len(argv) != 7:
+        glog.fatal('Expected .h file name and .cc file name')
+    else:
+        polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7],
+                                           'y2014', drivetrain.kDrivetrain)
+
 
 if __name__ == '__main__':
-  argv = FLAGS(sys.argv)
-  glog.init()
-  sys.exit(main(argv))
+    argv = FLAGS(sys.argv)
+    glog.init()
+    sys.exit(main(argv))
diff --git a/y2014/control_loops/python/polydrivetrain_test.py b/y2014/control_loops/python/polydrivetrain_test.py
index 8e0176e..a5bac4a 100755
--- a/y2014/control_loops/python/polydrivetrain_test.py
+++ b/y2014/control_loops/python/polydrivetrain_test.py
@@ -10,73 +10,72 @@
 
 
 class TestVelocityDrivetrain(unittest.TestCase):
-  def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
-    H = numpy.matrix([[1, 0],
-                      [-1, 0],
-                      [0, 1],
-                      [0, -1]])
-    K = numpy.matrix([[x1_max],
-                      [-x1_min],
-                      [x2_max],
-                      [-x2_min]])
-    return polytope.HPolytope(H, K)
 
-  def test_coerce_inside(self):
-    """Tests coercion when the point is inside the box."""
-    box = self.MakeBox(1, 2, 1, 2)
+    def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
+        H = numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]])
+        K = numpy.matrix([[x1_max], [-x1_min], [x2_max], [-x2_min]])
+        return polytope.HPolytope(H, K)
 
-    # x1 = x2
-    K = numpy.matrix([[1, -1]])
-    w = 0
+    def test_coerce_inside(self):
+        """Tests coercion when the point is inside the box."""
+        box = self.MakeBox(1, 2, 1, 2)
 
-    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w,
-                                                 numpy.matrix([[1.5], [1.5]])),
-                       numpy.matrix([[1.5], [1.5]]))
+        # x1 = x2
+        K = numpy.matrix([[1, -1]])
+        w = 0
 
-  def test_coerce_outside_intersect(self):
-    """Tests coercion when the line intersects the box."""
-    box = self.MakeBox(1, 2, 1, 2)
+        assert_array_equal(
+            polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[1.5], [1.5]])),
+            numpy.matrix([[1.5], [1.5]]))
 
-    # x1 = x2
-    K = numpy.matrix([[1, -1]])
-    w = 0
+    def test_coerce_outside_intersect(self):
+        """Tests coercion when the line intersects the box."""
+        box = self.MakeBox(1, 2, 1, 2)
 
-    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
-                       numpy.matrix([[2.0], [2.0]]))
+        # x1 = x2
+        K = numpy.matrix([[1, -1]])
+        w = 0
 
-  def test_coerce_outside_no_intersect(self):
-    """Tests coercion when the line does not intersect the box."""
-    box = self.MakeBox(3, 4, 1, 2)
+        assert_array_equal(
+            polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+            numpy.matrix([[2.0], [2.0]]))
 
-    # x1 = x2
-    K = numpy.matrix([[1, -1]])
-    w = 0
+    def test_coerce_outside_no_intersect(self):
+        """Tests coercion when the line does not intersect the box."""
+        box = self.MakeBox(3, 4, 1, 2)
 
-    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
-                       numpy.matrix([[3.0], [2.0]]))
+        # x1 = x2
+        K = numpy.matrix([[1, -1]])
+        w = 0
 
-  def test_coerce_middle_of_edge(self):
-    """Tests coercion when the line intersects the middle of an edge."""
-    box = self.MakeBox(0, 4, 1, 2)
+        assert_array_equal(
+            polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+            numpy.matrix([[3.0], [2.0]]))
 
-    # x1 = x2
-    K = numpy.matrix([[-1, 1]])
-    w = 0
+    def test_coerce_middle_of_edge(self):
+        """Tests coercion when the line intersects the middle of an edge."""
+        box = self.MakeBox(0, 4, 1, 2)
 
-    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
-                       numpy.matrix([[2.0], [2.0]]))
+        # x1 = x2
+        K = numpy.matrix([[-1, 1]])
+        w = 0
 
-  def test_coerce_perpendicular_line(self):
-    """Tests coercion when the line does not intersect and is in quadrant 2."""
-    box = self.MakeBox(1, 2, 1, 2)
+        assert_array_equal(
+            polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+            numpy.matrix([[2.0], [2.0]]))
 
-    # x1 = -x2
-    K = numpy.matrix([[1, 1]])
-    w = 0
+    def test_coerce_perpendicular_line(self):
+        """Tests coercion when the line does not intersect and is in quadrant 2."""
+        box = self.MakeBox(1, 2, 1, 2)
 
-    assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
-                       numpy.matrix([[1.0], [1.0]]))
+        # x1 = -x2
+        K = numpy.matrix([[1, 1]])
+        w = 0
+
+        assert_array_equal(
+            polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+            numpy.matrix([[1.0], [1.0]]))
 
 
 if __name__ == '__main__':
-  unittest.main()
+    unittest.main()
diff --git a/y2014/control_loops/python/shooter.py b/y2014/control_loops/python/shooter.py
index c3e8d86..e3ad903 100755
--- a/y2014/control_loops/python/shooter.py
+++ b/y2014/control_loops/python/shooter.py
@@ -264,8 +264,9 @@
 
     sprung_shooter = SprungShooterDeltaU()
     shooter = ShooterDeltaU()
-    loop_writer = control_loop.ControlLoopWriter(
-        "Shooter", [sprung_shooter, shooter], namespaces=namespaces)
+    loop_writer = control_loop.ControlLoopWriter("Shooter",
+                                                 [sprung_shooter, shooter],
+                                                 namespaces=namespaces)
 
     loop_writer.AddConstant(
         control_loop.Constant("kMaxExtension", "%f",