diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 644346a..c7a5dd7 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -99,7 +99,9 @@
     ],
     target_compatible_with = ["@platforms//os:linux"],
     deps = [
+        ":jacobian",
         ":quaternion_utils",
+        ":runge_kutta",
         "//aos/testing:googletest",
         "//aos/testing:random_seed",
         "@com_github_google_glog//:glog",
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.h b/frc971/control_loops/drivetrain/improved_down_estimator.h
index 548dba8..ddbf0bb 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.h
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.h
@@ -8,6 +8,7 @@
 #include "aos/time/time.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
 #include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/control_loops/quaternion_utils.h"
 #include "frc971/control_loops/runge_kutta.h"
 #include "glog/logging.h"
 
@@ -216,22 +217,9 @@
   // A good reference for angular velocity vectors with quaternions is at
   // http://www.euclideanspace.com/physics/kinematics/angularvelocity/
 
-  // Creates a rotational velocity vector to be integrated.
-  //
-  // omega is the rotational velocity vector in body coordinates.
-  // q is a matrix with the compononents of the quaternion in it.
-  //
-  // Returns dq / dt
   static Eigen::Vector4d QuaternionDerivative(Eigen::Vector3d omega,
                                               const Eigen::Vector4d &q_matrix) {
-    Eigen::Quaternion<double> q(q_matrix);
-
-    Eigen::Quaternion<double> omega_q;
-    omega_q.w() = 0.0;
-    omega_q.vec() = 0.5 * (q * omega);
-
-    Eigen::Quaternion<double> deriv = omega_q * q;
-    return deriv.coeffs();
+    return frc971::controls::QuaternionDerivative(omega, q_matrix);
   }
 
   // Moves the robot by the provided rotation vector (U).
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
index 4dcfe60..3312030 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
@@ -27,71 +27,6 @@
 }
 }  // namespace
 
-// Do a known transformation to see if quaternion integration is working
-// correctly.
-TEST(DownEstimatorTest, QuaternionIntegral) {
-  Eigen::Vector3d ux = Eigen::Vector3d::UnitX();
-  Eigen::Vector3d uy = Eigen::Vector3d::UnitY();
-  Eigen::Vector3d uz = Eigen::Vector3d::UnitZ();
-
-  Eigen::Quaternion<double> q(
-      Eigen::AngleAxis<double>(0.5 * M_PI, Eigen::Vector3d::UnitY()));
-
-  Eigen::Quaternion<double> q0(
-      Eigen::AngleAxis<double>(0, Eigen::Vector3d::UnitY()));
-
-  auto qux = q * ux;
-
-  VLOG(1) << "Q is w: " << q.w() << " vec: " << q.vec();
-  VLOG(1) << "ux is " << ux;
-  VLOG(1) << "qux is " << qux;
-
-  // Start by rotating around the X body vector for pi/2
-  Eigen::Quaternion<double> integral1(
-      RungeKutta(std::bind(&drivetrain::DrivetrainUkf::QuaternionDerivative, ux,
-                           std::placeholders::_1),
-                 q0.coeffs(), 0.5 * M_PI));
-
-  VLOG(1) << "integral1 * uz => " << integral1 * uz;
-
-  // Then rotate around the Y body vector for pi/2
-  Eigen::Quaternion<double> integral2(
-      RungeKutta(std::bind(&drivetrain::DrivetrainUkf::QuaternionDerivative, uy,
-                           std::placeholders::_1),
-                 integral1.normalized().coeffs(), 0.5 * M_PI));
-
-  VLOG(1) << "integral2 * uz => " << integral2 * uz;
-
-  // Then rotate around the X body vector for -pi/2
-  Eigen::Quaternion<double> integral3(
-      RungeKutta(std::bind(&drivetrain::DrivetrainUkf::QuaternionDerivative,
-                           -ux, std::placeholders::_1),
-                 integral2.normalized().coeffs(), 0.5 * M_PI));
-
-  integral1.normalize();
-  integral2.normalize();
-  integral3.normalize();
-
-  VLOG(1) << "Integral is w: " << integral1.w() << " vec: " << integral1.vec()
-          << " norm " << integral1.norm();
-
-  VLOG(1) << "Integral is w: " << integral3.w() << " vec: " << integral3.vec()
-          << " norm " << integral3.norm();
-
-  VLOG(1) << "ux => " << integral3 * ux;
-  EXPECT_NEAR(0.0, (ux - integral1 * ux).norm(), 5e-2);
-  EXPECT_NEAR(0.0, (uz - integral1 * uy).norm(), 5e-2);
-  EXPECT_NEAR(0.0, (-uy - integral1 * uz).norm(), 5e-2);
-
-  EXPECT_NEAR(0.0, (uy - integral2 * ux).norm(), 5e-2);
-  EXPECT_NEAR(0.0, (uz - integral2 * uy).norm(), 5e-2);
-  EXPECT_NEAR(0.0, (ux - integral2 * uz).norm(), 5e-2);
-
-  EXPECT_NEAR(0.0, (uy - integral3 * ux).norm(), 5e-2);
-  EXPECT_NEAR(0.0, (-ux - integral3 * uy).norm(), 5e-2);
-  EXPECT_NEAR(0.0, (uz - integral3 * uz).norm(), 5e-2);
-}
-
 TEST(DownEstimatorTest, UkfConstantRotation) {
   drivetrain::DrivetrainUkf dtukf(
       drivetrain::testing::GetTestDrivetrainConfig());
diff --git a/frc971/control_loops/quaternion_utils.cc b/frc971/control_loops/quaternion_utils.cc
index 0226e6d..bce78d0 100644
--- a/frc971/control_loops/quaternion_utils.cc
+++ b/frc971/control_loops/quaternion_utils.cc
@@ -5,52 +5,48 @@
 
 namespace frc971 {
 namespace controls {
+namespace {
 
-Eigen::Matrix<double, 4, 1> ToQuaternionFromRotationVector(
-    const Eigen::Matrix<double, 3, 1> &X, const double max_angle_cap) {
-  const double unclipped_angle = X.norm();
-  const double angle_scalar =
-      (unclipped_angle > max_angle_cap) ? max_angle_cap / unclipped_angle : 1.0;
-  const double angle = unclipped_angle * angle_scalar;
-  const double half_angle = angle * 0.5;
-
-  const double half_angle_squared = half_angle * half_angle;
+double SinXoverX(double x) {
+  const double xsquared = x * x;
 
   // sin(x)/x = 1
   double sinx_x = 1.0;
 
   // - x^2/3!
-  double value = half_angle_squared / 6.0;
+  double value = xsquared / 6.0;
   sinx_x -= value;
 
   // + x^4/5!
-  value = value * half_angle_squared / 20.0;
+  value = value * xsquared / 20.0;
   sinx_x += value;
 
   // - x^6/7!
-  value = value * half_angle_squared / (6.0 * 7.0);
+  value = value * xsquared / (6.0 * 7.0);
   sinx_x -= value;
 
   // + x^8/9!
-  value = value * half_angle_squared / (8.0 * 9.0);
+  value = value * xsquared / (8.0 * 9.0);
   sinx_x += value;
 
   // - x^10/11!
-  value = value * half_angle_squared / (10.0 * 11.0);
+  value = value * xsquared / (10.0 * 11.0);
   sinx_x -= value;
 
   // + x^12/13!
-  value = value * half_angle_squared / (12.0 * 13.0);
+  value = value * xsquared / (12.0 * 13.0);
   sinx_x += value;
 
   // - x^14/15!
-  value = value * half_angle_squared / (14.0 * 15.0);
+  value = value * xsquared / (14.0 * 15.0);
   sinx_x -= value;
 
   // + x^16/17!
-  value = value * half_angle_squared / (16.0 * 17.0);
+  value = value * xsquared / (16.0 * 17.0);
   sinx_x += value;
 
+  return sinx_x;
+
   // To plot the residual in matplotlib, run:
   // import numpy
   // import scipy
@@ -65,15 +61,10 @@
   //                   x ** 14 / scipy.misc.factorial(15) +
   //                   x ** 16 / scipy.misc.factorial(17) -
   //                   numpy.sin(x) / x)
-
-  const double scalar = sinx_x * 0.5;
-
-  Eigen::Matrix<double, 4, 1> result;
-  result.block<3, 1>(0, 0) = X * scalar * angle_scalar;
-  result(3, 0) = std::cos(half_angle);
-  return result;
 }
 
+}  // namespace
+
 inline Eigen::Matrix<double, 4, 1> MaybeFlipX(
     const Eigen::Matrix<double, 4, 1> &X) {
   if (X(3, 0) < 0.0) {
@@ -83,6 +74,24 @@
   }
 }
 
+Eigen::Matrix<double, 4, 1> ToQuaternionFromRotationVector(
+    const Eigen::Matrix<double, 3, 1> &X, const double max_angle_cap) {
+  const double unclipped_angle = X.norm();
+  const double angle_scalar =
+      (unclipped_angle > max_angle_cap) ? max_angle_cap / unclipped_angle : 1.0;
+  const double angle = unclipped_angle * angle_scalar;
+  const double half_angle = angle * 0.5;
+
+  const double scalar = SinXoverX(half_angle) * 0.5;
+
+  Eigen::Matrix<double, 4, 1> result;
+  result.block<3, 1>(0, 0) = X * scalar * angle_scalar;
+  result(3, 0) = std::cos(half_angle);
+  return result;
+}
+
+// q = cos(a/2) + i ( x * sin(a/2)) + j (y * sin(a/2)) + k ( z * sin(a/2))
+
 Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
     const Eigen::Matrix<double, 4, 1> &X) {
   // TODO(austin): Verify we still need it.
@@ -90,50 +99,38 @@
   const double half_angle =
       std::atan2(corrected_X.block<3, 1>(0, 0).norm(), corrected_X(3, 0));
 
-  const double half_angle_squared = half_angle * half_angle;
-
-  // TODO(austin): We are doing a division at the end of this. Do the taylor
-  // series expansion of x/sin(x) instead to avoid this.
-
-  // sin(x)/x = 1
-  double sinx_x = 1.0;
-
-  // - x^2/3!
-  double value = half_angle_squared / 6.0;
-  sinx_x -= value;
-
-  // + x^4/5!
-  value = value * half_angle_squared / 20.0;
-  sinx_x += value;
-
-  // - x^6/7!
-  value = value * half_angle_squared / (6.0 * 7.0);
-  sinx_x -= value;
-
-  // + x^8/9!
-  value = value * half_angle_squared / (8.0 * 9.0);
-  sinx_x += value;
-
-  // - x^10/11!
-  value = value * half_angle_squared / (10.0 * 11.0);
-  sinx_x -= value;
-
-  // + x^12/13!
-  value = value * half_angle_squared / (12.0 * 13.0);
-  sinx_x += value;
-
-  // - x^14/15!
-  value = value * half_angle_squared / (14.0 * 15.0);
-  sinx_x -= value;
-
-  // + x^16/17!
-  value = value * half_angle_squared / (16.0 * 17.0);
-  sinx_x += value;
-
-  const double scalar = 2.0 / sinx_x;
+  const double scalar = 2.0 / SinXoverX(half_angle);
 
   return corrected_X.block<3, 1>(0, 0) * scalar;
 }
 
+Eigen::Matrix<double, 4, 3> QuaternionDerivativeDerivitive(
+    const Eigen::Vector4d &q_matrix) {
+  // qa * qb = (a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
+  //            a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
+  //            a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x(),
+  //            a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z())
+  //
+  // We want q * omega_q = result * omega.
+  Eigen::Matrix<double, 4, 3> result;
+  result(3, 0) = -q_matrix.x() * 0.5;
+  result(3, 1) = -q_matrix.y() * 0.5;
+  result(3, 2) = -q_matrix.z() * 0.5;
+
+  result(0, 0) = q_matrix.w() * 0.5;
+  result(0, 1) = -q_matrix.z() * 0.5;
+  result(0, 2) = q_matrix.y() * 0.5;
+
+  result(1, 0) = q_matrix.z() * 0.5;
+  result(1, 1) = q_matrix.w() * 0.5;
+  result(1, 2) = -q_matrix.x() * 0.5;
+
+  result(2, 0) = -q_matrix.y() * 0.5;
+  result(2, 1) = q_matrix.x() * 0.5;
+  result(2, 2) = q_matrix.w() * 0.5;
+
+  return result;
+}
+
 }  // namespace controls
 }  // namespace frc971
diff --git a/frc971/control_loops/quaternion_utils.h b/frc971/control_loops/quaternion_utils.h
index 25ffa91..e97935c 100644
--- a/frc971/control_loops/quaternion_utils.h
+++ b/frc971/control_loops/quaternion_utils.h
@@ -65,6 +65,34 @@
     const Eigen::Matrix<double, 3, 1> &X,
     const double max_angle_cap = std::numeric_limits<double>::infinity());
 
+// Creates a rotational velocity vector to be integrated.
+//
+// omega is the rotational velocity vector in body coordinates.
+// q is a matrix with the compononents of the quaternion in it.
+//
+// Returns dq / dt
+inline Eigen::Vector4d QuaternionDerivative(Eigen::Vector3d omega,
+                                            const Eigen::Vector4d &q_matrix) {
+  // See https://www.ashwinnarayan.com/post/how-to-integrate-quaternions/ for
+  // another resource on quaternion integration and derivitives.
+  Eigen::Quaternion<double> q(q_matrix);
+
+  Eigen::Quaternion<double> omega_q;
+  omega_q.w() = 0.0;
+  omega_q.vec() = 0.5 * omega;
+
+  Eigen::Quaternion<double> deriv = q * omega_q;
+  return deriv.coeffs();
+}
+
+// d QuaternionDerivative / d omega
+Eigen::Matrix<double, 4, 3> QuaternionDerivativeDerivitive(
+    const Eigen::Vector4d &q_matrix);
+inline Eigen::Matrix<double, 4, 3> QuaternionDerivativeDerivitive(
+    const Eigen::Quaternion<double> &q) {
+  return QuaternionDerivativeDerivitive(q.coeffs());
+}
+
 }  // namespace controls
 }  // namespace frc971
 
diff --git a/frc971/control_loops/quaternion_utils_test.cc b/frc971/control_loops/quaternion_utils_test.cc
index f472a46..ada5e60 100644
--- a/frc971/control_loops/quaternion_utils_test.cc
+++ b/frc971/control_loops/quaternion_utils_test.cc
@@ -2,8 +2,10 @@
 
 #include <random>
 
-#include "frc971/control_loops/quaternion_utils.h"
 #include "aos/testing/random_seed.h"
+#include "frc971/control_loops/jacobian.h"
+#include "frc971/control_loops/quaternion_utils.h"
+#include "frc971/control_loops/runge_kutta.h"
 #include "glog/logging.h"
 #include "gtest/gtest.h"
 
@@ -147,6 +149,68 @@
   }
 }
 
+// Do a known transformation to see if quaternion integration is working
+// correctly.
+TEST(DownEstimatorTest, QuaternionIntegral) {
+  Eigen::Vector3d ux = Eigen::Vector3d::UnitX();
+  Eigen::Vector3d uy = Eigen::Vector3d::UnitY();
+  Eigen::Vector3d uz = Eigen::Vector3d::UnitZ();
+
+  Eigen::Quaternion<double> q(
+      Eigen::AngleAxis<double>(0.5 * M_PI, Eigen::Vector3d::UnitY()));
+
+  Eigen::Quaternion<double> q0(
+      Eigen::AngleAxis<double>(0, Eigen::Vector3d::UnitY()));
+
+  auto qux = q * ux;
+
+  VLOG(1) << "Q is w: " << q.w() << " vec: " << q.vec();
+  VLOG(1) << "ux is " << ux;
+  VLOG(1) << "qux is " << qux;
+
+  // Start by rotating around the X body vector for pi/2
+  Eigen::Quaternion<double> integral1(control_loops::RungeKutta(
+      std::bind(&QuaternionDerivative, ux, std::placeholders::_1), q0.coeffs(),
+      0.5 * M_PI));
+
+  VLOG(1) << "integral1 * uz => " << integral1 * uz;
+
+  // Then rotate around the Y body vector for pi/2
+  Eigen::Quaternion<double> integral2(control_loops::RungeKutta(
+      std::bind(&QuaternionDerivative, uy, std::placeholders::_1),
+      integral1.normalized().coeffs(), 0.5 * M_PI));
+
+  VLOG(1) << "integral2 * uz => " << integral2 * uz;
+
+  // Then rotate around the X body vector for -pi/2
+  Eigen::Quaternion<double> integral3(control_loops::RungeKutta(
+      std::bind(&QuaternionDerivative, -ux, std::placeholders::_1),
+      integral2.normalized().coeffs(), 0.5 * M_PI));
+
+  integral1.normalize();
+  integral2.normalize();
+  integral3.normalize();
+
+  VLOG(1) << "Integral is w: " << integral1.w() << " vec: " << integral1.vec()
+          << " norm " << integral1.norm();
+
+  VLOG(1) << "Integral is w: " << integral3.w() << " vec: " << integral3.vec()
+          << " norm " << integral3.norm();
+
+  VLOG(1) << "ux => " << integral3 * ux;
+  EXPECT_NEAR(0.0, (ux - integral1 * ux).norm(), 5e-2);
+  EXPECT_NEAR(0.0, (uz - integral1 * uy).norm(), 5e-2);
+  EXPECT_NEAR(0.0, (-uy - integral1 * uz).norm(), 5e-2);
+
+  EXPECT_NEAR(0.0, (uy - integral2 * ux).norm(), 5e-2);
+  EXPECT_NEAR(0.0, (uz - integral2 * uy).norm(), 5e-2);
+  EXPECT_NEAR(0.0, (ux - integral2 * uz).norm(), 5e-2);
+
+  EXPECT_NEAR(0.0, (uy - integral3 * ux).norm(), 5e-2);
+  EXPECT_NEAR(0.0, (-ux - integral3 * uy).norm(), 5e-2);
+  EXPECT_NEAR(0.0, (uz - integral3 * uz).norm(), 5e-2);
+}
+
 }  // namespace testing
 }  // namespace controls
 }  // namespace frc971
diff --git a/frc971/control_loops/runge_kutta.h b/frc971/control_loops/runge_kutta.h
index 61d674e..963f463 100644
--- a/frc971/control_loops/runge_kutta.h
+++ b/frc971/control_loops/runge_kutta.h
@@ -19,6 +19,18 @@
   return X + dt / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
 }
 
+// Implements Runge Kutta integration (4th order) split up into steps steps.  fn
+// is the function to integrate.  It must take 1 argument of type T.  The
+// integration starts at an initial value X, and integrates for dt.
+template <typename F, typename T>
+T RungeKuttaSteps(const F &fn, T X, double dt, int steps) {
+  dt = dt / steps;
+  for (int i = 0; i < steps; ++i) {
+    X = RungeKutta(fn, X, dt);
+  }
+  return X;
+}
+
 // Implements Runge Kutta integration (4th order).  This integrates dy/dt = fn(t,
 // y).  It must have the call signature of fn(double t, T y).  The
 // integration starts at an initial value y, and integrates for dt.