diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 0d100ed..f448760 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -67,9 +67,10 @@
 
   // Returns the current localizer state.
   Eigen::Matrix<double, 5, 1> trajectory_state() {
+    // Use the regular kalman filter's left/right velocity because they are
+    // generally smoother.
     return (Eigen::Matrix<double, 5, 1>() << localizer_->x(), localizer_->y(),
-            localizer_->theta(), localizer_->left_velocity(),
-            localizer_->right_velocity())
+            localizer_->theta(), DrivetrainXHat()(1), DrivetrainXHat()(3))
         .finished();
   }
 
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 824f0a8..edda034 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -657,8 +657,8 @@
       CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->theta();
   // As a sanity check, compare both against absolute angle and the spline's
   // goal angle.
-  EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), 0.0), 2e-2);
-  EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), expected_theta), 2e-2);
+  EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), 0.0), 5e-2);
+  EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), expected_theta), 5e-2);
 }
 
 // Tests that simple spline with a single goal message.
@@ -1005,7 +1005,7 @@
   const ::Eigen::Vector2d actual = drivetrain_plant_.GetPosition();
   // Expect the x position comparison to fail; everything else to succeed.
   spline_estimate_tolerance_ = 0.11;
-  spline_control_tolerance_ = 0.11;
+  spline_control_tolerance_ = 0.09;
   EXPECT_GT(std::abs(estimated_x - expected_x), spline_control_tolerance_);
   EXPECT_NEAR(estimated_y, expected_y, spline_control_tolerance_);
   EXPECT_NEAR(actual(0), estimated_x, spline_estimate_tolerance_);
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 27b31a0..33a8577 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -765,7 +765,7 @@
   // TODO(james): Pull these out into a config.
   Eigen::Matrix<double, 5, 5> Q;
   Q.setIdentity();
-  Q.diagonal() << 20.0, 20.0, 20.0, 10.0, 10.0;
+  Q.diagonal() << 30.0, 30.0, 20.0, 15.0, 15.0;
   Q *= 2.0;
   Q = (Q * Q).eval();