diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 168f81d..d097ec8 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -337,7 +337,8 @@
 
   dt_closedloop_.Update(output != NULL && controller_type == 1);
 
-  dt_spline_.Update(output != NULL && controller_type == 2);
+  dt_spline_.Update(output != NULL && controller_type == 2,
+                    xytheta_state_.block<5, 1>(0, 0));
 
   switch (controller_type) {
     case 0:
@@ -389,6 +390,7 @@
 
     dt_openloop_.PopulateStatus(status);
     dt_closedloop_.PopulateStatus(status);
+    dt_spline_.PopulateStatus(status);
   }
 
   double left_voltage = 0.0;
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 88547d5..0f57bf9 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -111,6 +111,15 @@
     return angular;
   }
 
+  Eigen::Matrix<Scalar, 2, 2> Tlr_to_la() const {
+    return (::Eigen::Matrix<Scalar, 2, 2>() << 0.5, 0.5,
+            -1.0 / (2 * robot_radius), 1.0 / (2 * robot_radius)).finished();
+  }
+
+  Eigen::Matrix<Scalar, 2, 2> Tla_to_lr() const {
+    return Tlr_to_la().inverse();
+  }
+
   // Converts the linear and angular position, velocity to the top 4 states of
   // the robot state.
   Eigen::Matrix<Scalar, 4, 1> AngularLinearToLeftRight(
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 17da3ab..6c16667 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -13,6 +13,7 @@
 #include "frc971/control_loops/drivetrain/drivetrain.h"
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
 #include "frc971/control_loops/state_feedback_loop.h"
 #include "frc971/queues/gyro.q.h"
 #include "y2016/constants.h"
@@ -108,7 +109,15 @@
                              ".frc971.control_loops.drivetrain_queue.position",
                              ".frc971.control_loops.drivetrain_queue.output",
                              ".frc971.control_loops.drivetrain_queue.status"),
-        gyro_reading_(::frc971::sensors::gyro_reading.name()) {
+        gyro_reading_(::frc971::sensors::gyro_reading.name()),
+        velocity_drivetrain_(::std::unique_ptr<StateFeedbackLoop<
+            2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+            HybridKalman<2, 2, 2>>>(
+            new StateFeedbackLoop<2, 2, 2, double,
+                                  StateFeedbackHybridPlant<2, 2, 2>,
+                                  HybridKalman<2, 2, 2>>(
+                GetDrivetrainConfig()
+                    .make_hybrid_drivetrain_velocity_loop()))) {
     Reinitialize();
     last_U_.setZero();
   }
@@ -188,6 +197,16 @@
     U(0, 0) += drivetrain_plant_->left_voltage_offset();
     U(1, 0) += drivetrain_plant_->right_voltage_offset();
     drivetrain_plant_->Update(U);
+    double dt_float =
+        ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+            GetDrivetrainConfig().dt).count();
+    state = RungeKuttaU(
+        [this](const ::Eigen::Matrix<double, 5, 1> &X,
+               const ::Eigen::Matrix<double, 2, 1> &U) {
+          return ContinuousDynamics(velocity_drivetrain_->plant(),
+                                    GetDrivetrainConfig().Tlr_to_la(), X, U);
+        },
+        state, U, dt_float);
   }
 
   void set_left_voltage_offset(double left_voltage_offset) {
@@ -199,10 +218,18 @@
 
   ::std::unique_ptr<DrivetrainPlant> drivetrain_plant_;
 
+  ::Eigen::Matrix<double, 2, 1> GetPosition() const {
+    return state.block<2,1>(0,0);
+  }
+
  private:
   ::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
   ::aos::Queue<::frc971::sensors::GyroReading> gyro_reading_;
 
+  ::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
+  ::std::unique_ptr<
+      StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+                        HybridKalman<2, 2, 2>>> velocity_drivetrain_;
   double last_left_position_;
   double last_right_position_;
 
@@ -259,6 +286,12 @@
                 drivetrain_motor_plant_.GetRightPosition(), 1e-3);
   }
 
+  void VerifyNearPosition(::Eigen::Matrix<double, 2, 1> expected) {
+    auto actual = drivetrain_motor_plant_.GetPosition();
+    EXPECT_NEAR(actual(0), expected(0), 1e-2);
+    EXPECT_NEAR(actual(1), expected(1), 1e-2);
+  }
+
   virtual ~DrivetrainTest() { ::frc971::sensors::gyro_reading.Clear(); }
 };
 
@@ -526,6 +559,135 @@
   VerifyNearGoal();
 }
 
+// Tests that simple spline converges on a goal.
+TEST_F(DrivetrainTest, SplineSimple) {
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  goal->controller_type = 2;
+  goal->spline.spline_idx = 1;
+  goal->spline.spline_count = 1;
+  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+  goal.Send();
+  RunIteration();
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  start_goal->controller_type = 2;
+  start_goal->spline_handle = 1;
+  start_goal.Send();
+
+  RunForTime(chrono::milliseconds(2000));
+  VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+}
+
+// Tests that simple spline converges when it doesn't start where it thinks.
+TEST_F(DrivetrainTest, SplineOffset) {
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  goal->controller_type = 2;
+  goal->spline.spline_idx = 1;
+  goal->spline.spline_count = 1;
+  goal->spline.spline_x = {{0.5, 0.25, 0.5, 0.5, 0.75, 1.0}};
+  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+  goal.Send();
+  RunIteration();
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  start_goal->controller_type = 2;
+  start_goal->spline_handle = 1;
+  start_goal.Send();
+
+  RunForTime(chrono::milliseconds(2000));
+  VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+}
+
+// Tests that simple spline converges when it starts to the side of where it
+// thinks.
+TEST_F(DrivetrainTest, SplineSideOffset) {
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  goal->controller_type = 2;
+  goal->spline.spline_idx = 1;
+  goal->spline.spline_count = 1;
+  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+  goal->spline.spline_y = {{0.5, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+  goal.Send();
+  RunIteration();
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  start_goal->controller_type = 2;
+  start_goal->spline_handle = 1;
+  start_goal.Send();
+
+  RunForTime(chrono::milliseconds(2000));
+  VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+}
+
+// Tests that a multispline converges on a goal.
+TEST_F(DrivetrainTest, MultiSpline) {
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  goal->controller_type = 2;
+  goal->spline.spline_idx = 1;
+  goal->spline.spline_count = 2;
+  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+  goal.Send();
+  RunIteration();
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  start_goal->controller_type = 2;
+  start_goal->spline_handle = 1;
+  start_goal.Send();
+
+  RunForTime(chrono::milliseconds(4000));
+  VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 2.0).finished());
+}
+
+// Tests that several splines converges on a goal.
+TEST_F(DrivetrainTest, SequentialSplines) {
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  goal->controller_type = 2;
+  goal->spline.spline_idx = 1;
+  goal->spline.spline_count = 1;
+  goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+  goal->spline.spline_y = {{0.0, 0.0, 0.25 ,0.75, 1.0, 1.0}};
+  goal.Send();
+  RunIteration();
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> start_goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  start_goal->controller_type = 2;
+  start_goal->spline_handle = 1;
+  start_goal.Send();
+  RunForTime(chrono::milliseconds(2000));
+  VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 1.0).finished());
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> second_spline_goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  second_spline_goal->controller_type = 2;
+  second_spline_goal->spline.spline_idx = 2;
+  second_spline_goal->spline.spline_count = 1;
+  second_spline_goal->spline.spline_x = {{1.0, 1.25, 1.5, 1.5, 1.25, 1.0}};
+  second_spline_goal->spline.spline_y = {{1.0, 1.0, 1.25, 1.5, 1.75, 2.0}};
+  second_spline_goal.Send();
+  RunIteration();
+
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> second_start_goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  second_start_goal->controller_type = 2;
+  second_start_goal->spline_handle = 2;
+  second_start_goal.Send();
+
+  RunForTime(chrono::milliseconds(2000));
+  VerifyNearPosition((::Eigen::Matrix<double, 2, 1>() << 1.0, 2.0).finished());
+}
+
 ::aos::controls::HVPolytope<2, 4, 4> MakeBox(double x1_min, double x1_max,
                                              double x2_min, double x2_max) {
   Eigen::Matrix<double, 4, 2> box_H;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 0d6b6ae..b45f218 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -1,5 +1,7 @@
 #include "frc971/control_loops/drivetrain/splinedrivetrain.h"
 
+#include <iostream>
+
 #include "Eigen/Dense"
 
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
@@ -12,7 +14,17 @@
 namespace drivetrain {
 
 SplineDrivetrain::SplineDrivetrain(const DrivetrainConfig<double> &dt_config)
-    : dt_config_(dt_config) {}
+    : dt_config_(dt_config),
+      current_state_(::Eigen::Matrix<double, 2, 1>::Zero()) {}
+
+void SplineDrivetrain::ScaleCapU(Eigen::Matrix<double, 2, 1> *U) {
+  bool output_was_capped = ::std::abs((*U)(0, 0)) > 12.0 ||
+                       ::std::abs((*U)(1, 0)) > 12.0;
+
+  if (output_was_capped) {
+    *U *= 12.0 / U->lpNorm<Eigen::Infinity>();
+  }
+}
 
 // TODO(alex): put in another thread to avoid malloc in RT.
 void SplineDrivetrain::SetGoal(
@@ -65,17 +77,36 @@
     current_trajectory_->Plan();
     current_xva_ = current_trajectory_->FFAcceleration(0);
     current_xva_(1) = 0.0;
+    current_state_ = ::Eigen::Matrix<double, 2, 1>::Zero();
   }
 }
 
-void SplineDrivetrain::Update(bool enable) {
-  if (enable && current_trajectory_ &&
-      !current_trajectory_->is_at_end(current_state_)) {
+// TODO(alex): Hold position when done following the spline.
+// TODO(Austin): Compensate for voltage error.
+void SplineDrivetrain::Update(bool enable,
+                              const ::Eigen::Matrix<double, 5, 1> &state) {
+  enable_ = enable;
+  if (enable && current_trajectory_) {
+    ::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
+    if (!current_trajectory_->is_at_end(current_state_)) {
+      // TODO(alex): It takes about a cycle for the outputs to propagate to the
+      // motors. Consider delaying the output by a cycle.
+      U_ff = current_trajectory_->FFVoltage(current_xva_(0));
+    }
+    ::Eigen::Matrix<double, 2, 5> K =
+        current_trajectory_->KForState(state, dt_config_.dt, Q, R);
+    ::Eigen::Matrix<double, 5, 1> goal_state =
+        current_trajectory_->GoalState(current_xva_(0), current_xva_(1));
+    ::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
+    ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
+    next_U_ = U_ff + U_fb;
+    uncapped_U_ = next_U_;
+    ScaleCapU(&next_U_);
+
     next_xva_ = current_trajectory_->GetNextXVA(dt_config_.dt, &current_state_);
   }
 }
 
-// TODO(alex): Handle drift.
 void SplineDrivetrain::SetOutput(
     ::frc971::control_loops::DrivetrainQueue::Output *output) {
   if (!output) {
@@ -86,16 +117,22 @@
   }
   if (current_spline_handle_ == current_spline_idx_) {
     if (!current_trajectory_->is_at_end(current_state_)) {
-      double current_distance = current_xva_(0);
-      ::Eigen::Matrix<double, 2, 1> FFVoltage =
-          current_trajectory_->FFVoltage(current_distance);
-      output->left_voltage = FFVoltage(0);
-      output->right_voltage = FFVoltage(1);
+      output->left_voltage = next_U_(0);
+      output->right_voltage = next_U_(1);
       current_xva_ = next_xva_;
     }
   }
 }
 
+void SplineDrivetrain::PopulateStatus(
+    ::frc971::control_loops::DrivetrainQueue::Status *status) const {
+  if (status && enable_) {
+    status->uncapped_left_voltage = uncapped_U_(0);
+    status->uncapped_right_voltage = uncapped_U_(1);
+    status->robot_speed = current_xva_(1);
+  }
+}
+
 }  // namespace drivetrain
 }  // namespace control_loops
 }  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 4bb7ac9..a065167 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -19,7 +19,7 @@
 
   void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
 
-  void Update(bool enabled);
+  void Update(bool enabled, const ::Eigen::Matrix<double, 5, 1> &state);
 
   void SetOutput(
       ::frc971::control_loops::DrivetrainQueue::Output *output);
@@ -27,6 +27,8 @@
   void PopulateStatus(
       ::frc971::control_loops::DrivetrainQueue::Status *status) const;
  private:
+  void ScaleCapU(Eigen::Matrix<double, 2, 1> *U);
+
   const DrivetrainConfig<double> dt_config_;
 
   uint32_t current_spline_handle_; // Current spline told to excecute.
@@ -35,6 +37,25 @@
   ::std::unique_ptr<Trajectory> current_trajectory_;
   ::Eigen::Matrix<double, 3, 1> current_xva_, next_xva_;
   ::Eigen::Matrix<double, 2, 1> current_state_;
+  ::Eigen::Matrix<double, 2, 1> next_U_;
+  ::Eigen::Matrix<double, 2, 1> uncapped_U_;
+
+  bool enable_;
+
+  // TODO(alex): pull this out of dt_config.
+  const ::Eigen::DiagonalMatrix<double, 5> Q =
+      (::Eigen::DiagonalMatrix<double, 5>().diagonal()
+           << 1.0 / ::std::pow(0.05, 2),
+       1.0 / ::std::pow(0.05, 2), 1.0 / ::std::pow(0.2, 2),
+       1.0 / ::std::pow(0.5, 2), 1.0 / ::std::pow(0.5, 2))
+          .finished()
+          .asDiagonal();
+  const ::Eigen::DiagonalMatrix<double, 2> R =
+      (::Eigen::DiagonalMatrix<double, 2>().diagonal()
+           << 1.0 / ::std::pow(12.0, 2),
+       1.0 / ::std::pow(12.0, 2))
+          .finished()
+          .asDiagonal();
 };
 
 }  // namespace drivetrain