diff --git a/y2014/control_loops/drivetrain/drivetrain.cc b/y2014/control_loops/drivetrain/drivetrain.cc
index c538894..5717f36 100644
--- a/y2014/control_loops/drivetrain/drivetrain.cc
+++ b/y2014/control_loops/drivetrain/drivetrain.cc
@@ -33,7 +33,8 @@
     ::y2014::control_loops::DrivetrainQueue *my_drivetrain)
     : aos::controls::ControlLoop<::y2014::control_loops::DrivetrainQueue>(
           my_drivetrain),
-      kf_(::y2014::control_loops::drivetrain::MakeKFDrivetrainLoop()) {
+      kf_(::y2014::control_loops::drivetrain::MakeKFDrivetrainLoop()),
+      dt_openloop_(&kf_) {
   ::aos::controls::HPolytope<0>::Init();
 }
 
@@ -49,6 +50,16 @@
   }
   no_position_.Print();
 
+  kf_.set_controller_index(dt_openloop_.controller_index());
+
+  {
+    Eigen::Matrix<double, 3, 1> Y;
+    Y << position->left_encoder, position->right_encoder, last_gyro_rate_;
+    kf_.Correct(Y);
+    integrated_kf_heading_ +=
+        kDt * (kf_.X_hat(3, 0) - kf_.X_hat(1, 0)) / (kRobotRadius * 2.0);
+  }
+
   bool control_loop_driving = false;
   if (goal) {
     double wheel = goal->steering;
@@ -125,14 +136,6 @@
   left_voltage *= scalar;
   right_voltage *= scalar;
 
-  kf_.set_controller_index(dt_openloop_.controller_index());
-
-  Eigen::Matrix<double, 3, 1> Y;
-  Y << position->left_encoder, position->right_encoder, last_gyro_rate_;
-  kf_.Correct(Y);
-  integrated_kf_heading_ +=
-      kDt * (kf_.X_hat(3, 0) - kf_.X_hat(1, 0)) / (kRobotRadius * 2.0);
-
   // To validate, look at the following:
 
   // Observed - dx/dt velocity for left, right.
diff --git a/y2014/control_loops/drivetrain/drivetrain.h b/y2014/control_loops/drivetrain/drivetrain.h
index 7ecc09f..489b4ca 100644
--- a/y2014/control_loops/drivetrain/drivetrain.h
+++ b/y2014/control_loops/drivetrain/drivetrain.h
@@ -38,9 +38,9 @@
   double last_gyro_heading_ = 0.0;
   double last_gyro_rate_ = 0.0;
 
+  StateFeedbackLoop<7, 2, 3> kf_;
   PolyDrivetrain dt_openloop_;
   DrivetrainMotorsSS dt_closedloop_;
-  StateFeedbackLoop<7, 2, 3> kf_;
 
   double last_left_voltage_ = 0;
   double last_right_voltage_ = 0;
diff --git a/y2014/control_loops/drivetrain/polydrivetrain.cc b/y2014/control_loops/drivetrain/polydrivetrain.cc
index 16ca10a..02d0658 100644
--- a/y2014/control_loops/drivetrain/polydrivetrain.cc
+++ b/y2014/control_loops/drivetrain/polydrivetrain.cc
@@ -23,8 +23,9 @@
 using ::y2014::control_loops::CIMLogging;
 using ::frc971::control_loops::CoerceGoal;
 
-PolyDrivetrain::PolyDrivetrain()
-    : U_Poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
+PolyDrivetrain::PolyDrivetrain(StateFeedbackLoop<7, 2, 3> *kf)
+    : kf_(kf),
+      U_Poly_((Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
                /*[*/ -1, 0 /*]*/,
                /*[*/ 0, 1 /*]*/,
                /*[*/ 0, -1 /*]]*/).finished(),
diff --git a/y2014/control_loops/drivetrain/polydrivetrain.h b/y2014/control_loops/drivetrain/polydrivetrain.h
index d57ff4c..5cd7fff 100644
--- a/y2014/control_loops/drivetrain/polydrivetrain.h
+++ b/y2014/control_loops/drivetrain/polydrivetrain.h
@@ -32,7 +32,7 @@
   // Torque constant
   static constexpr double Kt = drivetrain::kT;
 
-  PolyDrivetrain();
+  PolyDrivetrain(StateFeedbackLoop<7, 2, 3> *kf);
 
   int controller_index() const { return loop_->controller_index(); }
 
@@ -65,6 +65,7 @@
   void SendMotors(::y2014::control_loops::DrivetrainQueue::Output *output);
 
  private:
+  StateFeedbackLoop<7, 2, 3> *kf_;
   const ::aos::controls::HPolytope<2> U_Poly_;
 
   ::std::unique_ptr<StateFeedbackLoop<2, 2, 2>> loop_;