diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 64b4612..c9582e6 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -362,31 +362,32 @@
   return result;
 }
 
-::std::vector<::Eigen::Matrix<double, 3, 1>> Trajectory::PlanXVA(
-    ::std::chrono::nanoseconds dt) {
+::Eigen::Matrix<double, 3, 1> Trajectory::GetNextXVA(
+    ::std::chrono::nanoseconds dt, ::Eigen::Matrix<double, 2, 1> *state) {
   double dt_float =
       ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt).count();
-  double t = 0.0;
-  ::Eigen::Matrix<double, 2, 1> state = ::Eigen::Matrix<double, 2, 1>::Zero();
 
+  // TODO(austin): This feels like something that should be pulled out into
+  // a library for re-use.
+  *state = RungeKutta([this](const ::Eigen::Matrix<double, 2, 1> x) {
+    ::Eigen::Matrix<double, 3, 1> xva = FFAcceleration(x(0));
+    return (::Eigen::Matrix<double, 2, 1>() << x(1), xva(2)).finished();
+  }, *state, dt_float);
+
+  ::Eigen::Matrix<double, 3, 1> result = FFAcceleration((*state)(0));
+  (*state)(1) = result(1);
+  return result;
+}
+
+::std::vector<::Eigen::Matrix<double, 3, 1>> Trajectory::PlanXVA(
+    ::std::chrono::nanoseconds dt) {
+  ::Eigen::Matrix<double, 2, 1> state = ::Eigen::Matrix<double, 2, 1>::Zero();
   ::std::vector<::Eigen::Matrix<double, 3, 1>> result;
   result.emplace_back(FFAcceleration(0));
   result.back()(1) = 0.0;
 
-  while (state(0) < length() - 1e-4) {
-    t += dt_float;
-
-    // TODO(austin): This feels like something that should be pulled out into
-    // a library for re-use.
-    state = RungeKutta(
-        [this](const ::Eigen::Matrix<double, 2, 1> x) {
-          ::Eigen::Matrix<double, 3, 1> xva = FFAcceleration(x(0));
-          return (::Eigen::Matrix<double, 2, 1>() << x(1), xva(2)).finished();
-        },
-        state, dt_float);
-
-    result.emplace_back(FFAcceleration(state(0)));
-    state(1) = result.back()(1);
+  while (!is_at_end(state)) {
+    result.emplace_back(GetNextXVA(dt, &state));
   }
   return result;
 }
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index d3468b3..66dcba1 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -98,6 +98,11 @@
   // Returns the feed forwards voltage for an explicit distance.
   ::Eigen::Matrix<double, 2, 1> FFVoltage(double distance);
 
+  // Returns whether a state represents a state at the end of the spline.
+  bool is_at_end(::Eigen::Matrix<double, 2, 1> state) const {
+    return state(0) > length() - 1e-4;
+  }
+
   // Returns the length of the path in meters.
   double length() const { return spline_->length(); }
 
@@ -151,6 +156,10 @@
       const ::Eigen::DiagonalMatrix<double, 5> &Q,
       const ::Eigen::DiagonalMatrix<double, 2> &R) const;
 
+  // Return the next position, velocity, acceleration based on the current
+  // state. Updates the passed in state for the next iteration.
+  ::Eigen::Matrix<double, 3, 1> GetNextXVA(
+      ::std::chrono::nanoseconds dt, ::Eigen::Matrix<double, 2, 1> *state);
   ::std::vector<::Eigen::Matrix<double, 3, 1>> PlanXVA(
       ::std::chrono::nanoseconds dt);
 
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index eb1664a..f65adf1 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -265,6 +265,15 @@
             1.2e-2);
 }
 
+// Tests that Iteratively computing the XVA plan is the same as precomputing it.
+TEST_P(ParameterizedSplineTest, IterativeXVA) {
+  ::Eigen::Matrix<double, 2, 1> state = ::Eigen::Matrix<double, 2, 1>::Zero();
+  for (size_t i = 1; i < length_plan_xva_.size(); ++i) {
+    ::Eigen::Matrix<double, 3, 1> xva = trajectory_->GetNextXVA(kDt, &state);
+    EXPECT_LT((length_plan_xva_[i] - xva).norm(), 1e-2);
+  }
+}
+
 SplineTestParams MakeSplineTestParams(struct SplineTestParams params) {
   return params;
 }