diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index fffbe0d..4b63aec 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -208,7 +208,7 @@
     first = True
     for x in xrange(matrix.shape[0]):
       for y in xrange(matrix.shape[1]):
-	element = matrix[x, y]
+        element = matrix[x, y]
         if first:
           ans.append('  %s << ' % matrix_name)
           first = False
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index a40bfe2..967be3c 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -99,3 +99,31 @@
       diage[count, count] = (numpy.exp(eig * dt) - 1.0) / eig
 
   return (P * diag * numpy.linalg.inv(P), P * diage * numpy.linalg.inv(P) * B)
+
+def ctrb(A, B):
+  """Returns the controlability matrix.
+
+    This matrix must have full rank for all the states to be controlable.
+  """
+  n = A.shape[0]
+  output = B
+  intermediate = B
+  for i in xrange(0, n):
+    intermediate = A * intermediate
+    output = numpy.concatenate((output, intermediate), axis=1)
+
+  return output
+
+def dlqr(A, B, Q, R):
+  """Solves for the optimal lqr controller.
+
+    x(n+1) = A * x(n) + B * u(n)
+    J = sum(0, inf, x.T * Q * x + u.T * R * u)
+  """
+
+  # P = (A.T * P * A) - (A.T * P * B * numpy.linalg.inv(R + B.T * P *B) * (A.T * P.T * B).T + Q
+
+  P, rcond, w, S, T = slycot.sb02od(A.shape[0], B.shape[1], A, B, Q, R, 'D')
+
+  F = numpy.linalg.inv(R + B.T * P *B) * B.T * P * A
+  return F