diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 72a32cc..05fc4a3 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -93,6 +93,32 @@
   ],
 )
 
+genrule(
+  name = 'genrule_down_estimator',
+  visibility = ['//visibility:private'],
+  cmd = '$(location //frc971/control_loops/python:down_estimator) $(OUTS)',
+  tools = [
+    '//frc971/control_loops/python:down_estimator',
+  ],
+  outs = [
+    'down_estimator.h',
+    'down_estimator.cc',
+  ],
+)
+
+cc_library(
+  name = 'down_estimator',
+  hdrs = [
+    'down_estimator.h',
+  ],
+  srcs = [
+    'down_estimator.cc',
+  ],
+  deps = [
+    '//frc971/control_loops:state_feedback_loop',
+  ],
+)
+
 cc_library(
   name = 'drivetrain_lib',
   srcs = [
@@ -102,12 +128,14 @@
     'drivetrain.h',
   ],
   deps = [
+    ':down_estimator',
     ':drivetrain_queue',
     ':gear',
     ':polydrivetrain',
     ':ssdrivetrain',
     '//aos/common/controls:control_loop',
     '//frc971/queues:gyro',
+    '//frc971/wpilib:imu_queue',
     '//aos/common/util:log_interval',
     '//aos/common/logging:queue_logging',
     '//aos/common/logging:matrix_logging',
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index b4c04a3..7c4f760 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -13,9 +13,11 @@
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/drivetrain/polydrivetrain.h"
 #include "frc971/control_loops/drivetrain/ssdrivetrain.h"
+#include "frc971/control_loops/drivetrain/down_estimator.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
 #include "frc971/queues/gyro.q.h"
 #include "frc971/shifter_hall_effect.h"
+#include "frc971/wpilib/imu.q.h"
 
 using frc971::sensors::gyro_reading;
 
@@ -32,11 +34,13 @@
       kf_(dt_config_.make_kf_drivetrain_loop()),
       dt_openloop_(dt_config_, &kf_),
       dt_closedloop_(dt_config_, &kf_, &integrated_kf_heading_),
+      down_estimator_(MakeDownEstimatorLoop()),
       left_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
       right_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
       left_high_requested_(dt_config_.default_high_gear),
       right_high_requested_(dt_config_.default_high_gear) {
   ::aos::controls::HPolytope<0>::Init();
+  down_U_.setZero();
 }
 
 int DrivetrainLoop::ControllerIndexFromGears() {
@@ -111,6 +115,36 @@
     LOG_STRUCT(DEBUG, "state", gear_logging);
   }
 
+  if (::frc971::imu_values.FetchLatest()) {
+    const double rate = -::frc971::imu_values->gyro_y;
+    const double accel_squared = ::frc971::imu_values->accelerometer_x *
+                                     ::frc971::imu_values->accelerometer_x +
+                                 ::frc971::imu_values->accelerometer_y *
+                                     ::frc971::imu_values->accelerometer_y +
+                                 ::frc971::imu_values->accelerometer_z *
+                                     ::frc971::imu_values->accelerometer_z;
+    const double angle = ::std::atan2(-::frc971::imu_values->accelerometer_x,
+                                      -::frc971::imu_values->accelerometer_z) +
+                         0.008;
+    if (accel_squared > 1.03 || accel_squared < 0.97) {
+      LOG(DEBUG, "New IMU value, rejecting reading\n");
+    } else {
+      // -y is our gyro.
+      // -z accel is down
+      // -x accel is the front of the robot pointed down.
+      Eigen::Matrix<double, 1, 1> Y;
+      Y << angle;
+      down_estimator_.Correct(Y);
+    }
+
+    LOG(DEBUG,
+        "New IMU value from ADIS16448, rate is %f, angle %f, fused %f, bias "
+        "%f\n",
+        rate, angle, down_estimator_.X_hat(0, 0), down_estimator_.X_hat(1, 0));
+    down_U_ << rate;
+  }
+  down_estimator_.UpdateObserver(down_U_);
+
   // TODO(austin): Signal the current gear to both loops.
 
   if (gyro_reading.FetchLatest()) {
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index a9541b7..69e9794 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -48,6 +48,9 @@
   PolyDrivetrain dt_openloop_;
   DrivetrainMotorsSS dt_closedloop_;
 
+  StateFeedbackLoop<2, 1, 1> down_estimator_;
+  Eigen::Matrix<double, 1, 1> down_U_;
+
   // Current gears for each drive side.
   Gear left_gear_;
   Gear right_gear_;
diff --git a/frc971/control_loops/python/down_estimator.py b/frc971/control_loops/python/down_estimator.py
index 1d776b3..6db8f34 100644
--- a/frc971/control_loops/python/down_estimator.py
+++ b/frc971/control_loops/python/down_estimator.py
@@ -10,6 +10,7 @@
 from matplotlib import pylab
 
 from frc971.control_loops.python import controls
+from frc971.control_loops.python import control_loop
 
 FLAGS = gflags.FLAGS
 
@@ -29,16 +30,23 @@
     self.B_continuous = numpy.matrix([[1],
                                       [0]])
 
-    self.A, self.B = c2d(self.A_continuous, self.B_continuous, dt):
+    self.A, self.B = self.ContinuousToDiscrete(
+        self.A_continuous, self.B_continuous, self.dt)
 
     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_noise ** 2.0)]])
+                           [0.0, 1.0 / (q_gyro_bias_noise ** 2.0)]])
 
-    r_accelerometer_angle_noise = 5e-3
+    r_accelerometer_angle_noise = 5e+7
     self.R = numpy.matrix([[(r_accelerometer_angle_noise ** 2.0)]])
 
     self.C = numpy.matrix([[1.0, 0.0]])
+    self.D = numpy.matrix([[0]])
+
+    self.U_max = numpy.matrix([[numpy.pi]])
+    self.U_min = numpy.matrix([[-numpy.pi]])
+    self.K = numpy.matrix(numpy.zeros((1, 2)))
 
     self.KalmanGain, self.Q_steady = controls.kalman(
         A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
@@ -100,5 +108,14 @@
     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 __name__ == '__main__':
   sys.exit(main(sys.argv))