diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index ade396e..fc01edc 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -28,6 +28,7 @@
 void BaseAutonomousActor::ResetDrivetrain() {
   LOG(INFO, "resetting the drivetrain\n");
   max_drivetrain_voltage_ = 12.0;
+  goal_spline_handle_ = 0;
   drivetrain_queue.goal.MakeWithBuilder()
       .controller_type(0)
       .highgear(true)
@@ -350,7 +351,7 @@
 }
 
 BaseAutonomousActor::SplineHandle BaseAutonomousActor::PlanSpline(
-    const ::frc971::MultiSpline &spline) {
+    const ::frc971::MultiSpline &spline, SplineDirection direction) {
   LOG(INFO, "Planning spline\n");
 
   int32_t spline_handle = (++spline_handle_) | ((getpid() & 0xFFFF) << 15);
@@ -363,6 +364,8 @@
   drivetrain_message->spline = spline;
   drivetrain_message->spline.spline_idx = spline_handle;
   drivetrain_message->spline_handle = goal_spline_handle_;
+  drivetrain_message->drive_spline_backwards =
+      direction == SplineDirection::kBackward;
 
   LOG_STRUCT(DEBUG, "dtg", *drivetrain_message);
 
@@ -417,9 +420,17 @@
   drivetrain_queue.status.FetchLatest();
   LOG_STRUCT(INFO, "dts", *drivetrain_queue.status.get());
 
+  // We check that the spline we are waiting on is neither currently planning
+  // nor executing; note that we do *not* check the is_executing bit because
+  // immediately after calling Start we may still receive an old Status message
+  // that has not been updated. We check for planning so that the user can go
+  // straight from starting the planner to executing without a WaitForPlan in
+  // between.
   if (drivetrain_queue.status.get() &&
-      drivetrain_queue.status->trajectory_logging.current_spline_idx ==
-          spline_handle_) {
+      (drivetrain_queue.status->trajectory_logging.current_spline_idx ==
+           spline_handle_ ||
+       drivetrain_queue.status->trajectory_logging.planning_spline_idx ==
+           spline_handle_)) {
     return false;
   }
   return true;
diff --git a/frc971/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
index 7e3ef35..aeef38c 100644
--- a/frc971/autonomous/base_autonomous_actor.h
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -42,9 +42,16 @@
     BaseAutonomousActor *base_autonomous_actor_;
   };
 
+  // Represents the direction that we will drive along a spline.
+  enum class SplineDirection {
+    kForward,
+    kBackward,
+  };
+
   // Starts planning the spline, and returns a handle to be used to manipulate
   // it.
-  SplineHandle PlanSpline(const ::frc971::MultiSpline &spline);
+  SplineHandle PlanSpline(const ::frc971::MultiSpline &spline,
+                          SplineDirection direction);
 
   void ResetDrivetrain();
   void InitializeEncoders();
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 1480e4e..a1b96be 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -106,6 +106,7 @@
         ":spline",
         ":trajectory",
         "//aos:init",
+        "//aos/util:math",
     ],
 )
 
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 73ebe10..09ecb4f 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -130,6 +130,8 @@
 
     // Which spline to follow.
     int32_t spline_handle;
+    // Whether to follow the spline driving forwards or backwards.
+    bool drive_spline_backwards;
   };
 
   message Position {
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 05889e4..9d031b7 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -118,8 +118,8 @@
     double expected_x = my_drivetrain_queue_.status->trajectory_logging.x;
     double expected_y = my_drivetrain_queue_.status->trajectory_logging.y;
     auto actual = drivetrain_motor_plant_.GetPosition();
-    EXPECT_NEAR(actual(0), expected_x, 1e-2);
-    EXPECT_NEAR(actual(1), expected_y, 1e-2);
+    EXPECT_NEAR(actual(0), expected_x, 2e-2);
+    EXPECT_NEAR(actual(1), expected_y, 2e-2);
   }
 
   void WaitForTrajectoryPlan() {
@@ -435,6 +435,30 @@
   VerifyNearSplineGoal();
 }
 
+// Tests that we can drive a spline backwards.
+TEST_F(DrivetrainTest, SplineSimpleBackwards) {
+  ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+      my_drivetrain_queue_.goal.MakeMessage();
+  goal->drive_spline_backwards = true;
+  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();
+  WaitForTrajectoryPlan();
+
+  RunForTime(chrono::milliseconds(2000));
+  VerifyNearSplineGoal();
+}
+
 // Tests that simple spline with a single goal message.
 TEST_F(DrivetrainTest, SplineSingleGoal) {
   ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
@@ -528,8 +552,8 @@
   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->spline.spline_x = {{0.2, 0.25, 0.5, 0.5, 0.75, 1.0}};
+  goal->spline.spline_y = {{0.2, 0.0, 0.25 ,0.75, 1.0, 1.0}};
   goal.Send();
   RunIteration();
 
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index a37ca0e..18c8381 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -3,6 +3,7 @@
 #include "Eigen/Dense"
 
 #include "aos/init.h"
+#include "aos/util/math.h"
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
 
@@ -99,6 +100,7 @@
 
   ::aos::Mutex::State mutex_state = mutex_.TryLock();
   if (mutex_state == ::aos::Mutex::State::kLocked) {
+    drive_spline_backwards_ = goal_.drive_spline_backwards;
     if (goal.spline.spline_idx && future_spline_idx_ != goal.spline.spline_idx) {
       goal_ = goal;
       new_goal_.Broadcast();
@@ -143,7 +145,17 @@
     ::Eigen::Matrix<double, 2, 5> K =
         current_trajectory_->KForState(state, dt_config_.dt, Q, R);
     ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
+    if (drive_spline_backwards_) {
+      ::Eigen::Matrix<double, 2, 1> swapU(U_ff(1, 0), U_ff(0, 0));
+      U_ff = -swapU;
+      goal_state(2, 0) += M_PI;
+      double left_goal = goal_state(3, 0);
+      double right_goal = goal_state(4, 0);
+      goal_state(3, 0) = -right_goal;
+      goal_state(4, 0) = -left_goal;
+    }
     ::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
+    state_error(2, 0) = ::aos::math::NormalizeAngle(state_error(2, 0));
     ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
 
     ::Eigen::Matrix<double, 2, 1> xv_state = current_xva_.block<2,1>(0,0);
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index d70cb6d..59175b6 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -71,6 +71,7 @@
   int32_t current_spline_handle_ = 0;  // Current spline told to excecute.
   int32_t current_spline_idx_ = 0;     // Current executing spline.
   bool has_started_execution_ = false;
+  bool drive_spline_backwards_ = false;
 
   ::std::unique_ptr<DistanceSpline> current_distance_spline_;
   ::std::unique_ptr<Trajectory> current_trajectory_;
@@ -105,8 +106,8 @@
   // TODO(alex): pull this out of dt_config.
   const ::Eigen::DiagonalMatrix<double, 5> Q =
       (::Eigen::DiagonalMatrix<double, 5>().diagonal()
-           << 1.0 / ::std::pow(0.07, 2),
-       1.0 / ::std::pow(0.07, 2), 1.0 / ::std::pow(0.2, 2),
+           << 1.0 / ::std::pow(0.12, 2),
+       1.0 / ::std::pow(0.12, 2), 1.0 / ::std::pow(0.1, 2),
        1.0 / ::std::pow(1.5, 2), 1.0 / ::std::pow(1.5, 2))
           .finished()
           .asDiagonal();
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index b53e9d7..d30ee4b 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -410,7 +410,7 @@
     // interest here are that we (a) stop adding disturbances at the very end of
     // the trajectory, to allow us to actually converge to the goal, and (b)
     // scale disturbances by the corruent velocity.
-    if (GetParam().disturb && i % 50 == 0) {
+    if (GetParam().disturb && i % 75 == 0) {
       // Scale the disturbance so that when we have near-zero velocity we don't
       // have much disturbance.
       double disturbance_scale = ::std::min(
@@ -582,7 +582,7 @@
                 .finished(),
             /*noisify=*/false,
             /*disturb=*/true,
-            /*estimate_tolerance=*/2e-2,
+            /*estimate_tolerance=*/2.5e-2,
             /*goal_tolerance=*/0.15,
         }),
         // Add noise and some initial error in addition: