diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 245e772..01a49a9 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -9,6 +9,14 @@
 #include "Eigen/Dense"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
 
+namespace y2019 {
+namespace control_loops {
+namespace testing {
+class ParameterizedLocalizerTest;
+}  // namespace testing
+}  // namespace control_loops
+}  // namespace y2019
+
 namespace frc971 {
 namespace control_loops {
 namespace drivetrain {
@@ -95,9 +103,10 @@
   // TODO(james): We may want to actually re-initialize and reset things on
   // the field. Create some sort of Reset() function.
   void ResetInitialState(::aos::monotonic_clock::time_point t,
-                       const State &state) {
+                         const State &state, const StateSquare &P) {
     observations_.clear();
     X_hat_ = state;
+    P_ = P;
     observations_.PushFromBottom(
         {t,
          t,
@@ -299,6 +308,7 @@
       observations_;
 
   friend class testing::HybridEkfTest;
+  friend class ::y2019::control_loops::testing::ParameterizedLocalizerTest;
 };  // class HybridEkf
 
 template <typename Scalar>
@@ -419,8 +429,9 @@
 
   Q_continuous_.setZero();
   // TODO(james): Improve estimates of process noise--e.g., X/Y noise can
-  // probably be reduced because we rarely randomly jump in space, but maybe
-  // wheel speed noise should increase due to likelihood of slippage.
+  // probably be reduced when we are stopped because you rarely jump randomly.
+  // Or maybe it's more appropriate to scale wheelspeed noise with wheelspeed,
+  // since the wheels aren't likely to slip much stopped.
   Q_continuous_(kX, kX) = 0.005;
   Q_continuous_(kY, kY) = 0.005;
   Q_continuous_(kTheta, kTheta) = 0.001;
@@ -428,7 +439,7 @@
       dt_config_.make_kf_drivetrain_loop().observer().coefficients().Q;
 
   P_.setZero();
-  P_.diagonal() << 1, 1, 1, 0.5, 0.1, 0.5, 0.1, 1, 1, 1;
+  P_.diagonal() << 0.1, 0.1, 0.01, 0.02, 0.01, 0.02, 0.01, 1, 1, 0.03;
 
   H_encoders_and_gyro_.setZero();
   // Encoders are stored directly in the state matrix, so are a minor
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
index 9bdb30b..27119b1 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
+++ b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
@@ -28,7 +28,8 @@
         velocity_plant_coefs_(dt_config_.make_hybrid_drivetrain_velocity_loop()
                                   .plant()
                                   .coefficients()) {
-    ekf_.ResetInitialState(t0_, HybridEkf<>::State::Zero());
+    ekf_.ResetInitialState(t0_, State::Zero(),
+                           HybridEkf<>::StateSquare::Identity());
   }
 
  protected:
@@ -107,25 +108,26 @@
               {5.0, 6.0});
 }
 
-// Tests that if we provide a bunch of observations of the angular position
+// Tests that if we provide a bunch of observations of the position
 // with zero change in time, the state should approach the estimation.
 TEST_F(HybridEkfTest, ZeroTimeCorrect) {
-  HybridEkf<>::Output Z(1, 0, 0);
+  HybridEkf<>::Output Z(0.5, 0.5, 1);
   Eigen::Matrix<double, 3, 10> H;
-  H.setZero();
-  H(0, 2) = 1;
+  H.setIdentity();
   auto h = [H](const State &X, const Input &) { return H * X; };
   auto dhdx = [H](const State &) { return H; };
   Eigen::Matrix<double, 3, 3> R;
-  R.setZero();
-  R.diagonal() << 0.01, 0.01, 0.01;
+  R.setIdentity();
+  R *= 1e-3;
   Input U(0, 0);
   EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kTheta));
   const double starting_p_norm = ekf_.P().norm();
   for (int ii = 0; ii < 100; ++ii) {
     ekf_.Correct(Z, &U, {}, h, dhdx, R, t0_);
   }
-  EXPECT_NEAR(Z(0, 0), ekf_.X_hat(StateIdx::kTheta), 1e-3);
+  EXPECT_NEAR(Z(0, 0), ekf_.X_hat(StateIdx::kX), 1e-3);
+  EXPECT_NEAR(Z(1, 0), ekf_.X_hat(StateIdx::kY), 1e-3);
+  EXPECT_NEAR(Z(2, 0), ekf_.X_hat(StateIdx::kTheta), 1e-3);
   const double ending_p_norm = ekf_.P().norm();
   // Due to corrections, noise should've decreased.
   EXPECT_LT(ending_p_norm, starting_p_norm * 0.95);
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index b45f218..9aa1708 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -88,15 +88,14 @@
   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_)) {
+    if (!IsAtEnd()) {
       // 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> goal_state = CurrentGoalState();
     ::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;
@@ -116,7 +115,7 @@
     return;
   }
   if (current_spline_handle_ == current_spline_idx_) {
-    if (!current_trajectory_->is_at_end(current_state_)) {
+    if (!IsAtEnd()) {
       output->left_voltage = next_U_(0);
       output->right_voltage = next_U_(1);
       current_xva_ = next_xva_;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index a065167..2a79df6 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -26,6 +26,16 @@
   // TODO(alex): What status do we need?
   void PopulateStatus(
       ::frc971::control_loops::DrivetrainQueue::Status *status) const;
+
+  // Accessor for the current goal state, pretty much only present for debugging
+  // purposes.
+  Eigen::Matrix<double, 5, 1> CurrentGoalState() const {
+    return current_trajectory_->GoalState(current_xva_(0), current_xva_(1));
+  }
+
+  bool IsAtEnd() const {
+    return current_trajectory_->is_at_end(current_state_);
+  }
  private:
   void ScaleCapU(Eigen::Matrix<double, 2, 1> *U);