diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index ac2a2e8..ffa2c11 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -702,6 +702,7 @@
     deps = [
         ":drivetrain_config",
         ":drivetrain_status_fbs",
+        "//aos/controls:quaternion_utils",
         "//aos/events:event_loop",
         "//frc971/control_loops:control_loops_fbs",
         "//frc971/control_loops:runge_kutta",
@@ -718,6 +719,7 @@
     ],
     deps = [
         ":drivetrain_test_lib",
+        "//aos/controls:quaternion_utils",
         "//aos/testing:googletest",
         "//aos/testing:random_seed",
         "//frc971/control_loops/drivetrain:improved_down_estimator",
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.cc b/frc971/control_loops/drivetrain/improved_down_estimator.cc
index cdb1587..665bf32 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.cc
@@ -3,139 +3,12 @@
 #include "Eigen/Dense"
 #include "Eigen/Geometry"
 
+#include "aos/controls/quaternion_utils.h"
+
 namespace frc971 {
 namespace control_loops {
 namespace drivetrain {
 
-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;
-
-  // 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;
-
-  // To plot the residual in matplotlib, run:
-  // import numpy
-  // import scipy
-  // from matplotlib import pyplot
-  // x = numpy.arange(-numpy.pi, numpy.pi, 0.01)
-  // pyplot.plot(x, 1 - x**2 / scipy.misc.factorial(3) +
-  //                   x**4 / scipy.misc.factorial(5) -
-  //                   x**6 / scipy.misc.factorial(7) +
-  //                   x**8 / scipy.misc.factorial(9) -
-  //                   x ** 10 / scipy.misc.factorial(11) +
-  //                   x ** 12 / scipy.misc.factorial(13) -
-  //                   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;
-}
-
-inline Eigen::Matrix<double, 4, 1> MaybeFlipX(
-    const Eigen::Matrix<double, 4, 1> &X) {
-  if (X(3, 0) < 0.0) {
-    return -X;
-  } else {
-    return X;
-  }
-}
-
-Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
-    const Eigen::Matrix<double, 4, 1> &X) {
-  // TODO(austin): Verify we still need it.
-  const Eigen::Matrix<double, 4, 1> corrected_X = MaybeFlipX(X);
-  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;
-
-  return corrected_X.block<3, 1>(0, 0) * scalar;
-}
-
 void QuaternionUkf::Predict(const Eigen::Matrix<double, 3, 1> &U,
                             const Eigen::Matrix<double, 3, 1> &measurement,
                             const aos::monotonic_clock::duration dt) {
@@ -183,7 +56,7 @@
 
   // We now have the sigma points after the model update.
   // Compute the mean of the transformed sigma point
-  X_hat_ = Eigen::Quaternion<double>(QuaternionMean(Y));
+  X_hat_ = Eigen::Quaternion<double>(aos::controls::QuaternionMean(Y));
 
   // And the covariance.
   Eigen::Matrix<double, 3, 2 * 3 + 1> Wprime;
@@ -262,8 +135,9 @@
       P_xz * P_vv.inverse();
 
   // Update X_hat and the covariance P
-  X_hat_ = X_hat_ * Eigen::Quaternion<double>(ToQuaternionFromRotationVector(
-                        K * (measurement - Z_hat_)));
+  X_hat_ = X_hat_ * Eigen::Quaternion<double>(
+                        aos::controls::ToQuaternionFromRotationVector(
+                            K * (measurement - Z_hat_)));
   P_ = P_prior - K * P_vv * K.transpose();
 }
 
@@ -317,9 +191,10 @@
 
   for (int i = 0; i < 7; ++i) {
     // Compute the error vector for each sigma point.
-    Eigen::Matrix<double, 3, 1> Wprimei = ToRotationVectorFromQuaternion(
-        Eigen::Quaternion<double>(mean).conjugate() *
-        Eigen::Quaternion<double>(points.col(i)));
+    Eigen::Matrix<double, 3, 1> Wprimei =
+        aos::controls::ToRotationVectorFromQuaternion(
+            Eigen::Quaternion<double>(mean).conjugate() *
+            Eigen::Quaternion<double>(points.col(i)));
     // Now, compute the contribution of this sigma point to P_prior.
     P_prior += 1.0 / 6.0 * (Wprimei * Wprimei.transpose());
     // Save the error for the cross-correlation matrix.
@@ -343,7 +218,7 @@
   Eigen::Matrix<double, 4, 3 * 2 + 1> X;
   for (int i = 0; i < 3; ++i) {
     Eigen::Quaternion<double> perturbation(
-        ToQuaternionFromRotationVector(S.col(i), M_PI_2));
+        aos::controls::ToQuaternionFromRotationVector(S.col(i), M_PI_2));
 
     X.col(i * 2) = (mean * perturbation).coeffs();
     X.col(i * 2 + 1) = (mean * perturbation.conjugate()).coeffs();
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.h b/frc971/control_loops/drivetrain/improved_down_estimator.h
index c1c8e1b..457bbef 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.h
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.h
@@ -15,63 +15,6 @@
 namespace control_loops {
 namespace drivetrain {
 
-// Function to compute the quaternion average of a bunch of quaternions. Each
-// column in the input matrix is a quaternion (optionally scaled by it's
-// weight).
-template <int SM>
-Eigen::Matrix<double, 4, 1> QuaternionMean(
-    Eigen::Matrix<double, 4, SM> input) {
-  // Algorithm to compute the average of a bunch of quaternions:
-  // http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf
-
-  Eigen::Matrix<double, 4, 4> m = input * input.transpose();
-
-  Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>> solver;
-  solver.compute(m);
-
-  Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvectorsType
-      eigenvectors = solver.eigenvectors();
-  Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvalueType eigenvalues =
-      solver.eigenvalues();
-
-  int max_index = 0;
-  double max_eigenvalue = 0.0;
-  for (int i = 0; i < 4; ++i) {
-    const double eigenvalue = std::abs(eigenvalues(i, 0));
-    if (eigenvalue > max_eigenvalue) {
-      max_eigenvalue = eigenvalue;
-      max_index = i;
-    }
-  }
-
-  // Assume that there shouldn't be any imaginary components to the eigenvector.
-  // I can't prove this is true, but everyone else seems to assume it...
-  // TODO(james): Handle this more rigorously.
-  for (int i = 0; i < 4; ++i) {
-    CHECK_LT(eigenvectors(i, max_index).imag(), 1e-4)
-        << eigenvectors(i, max_index);
-  }
-  return eigenvectors.col(max_index).real().normalized();
-}
-
-// Converts from a quaternion to a rotation vector, where the rotation vector's
-// direction represents the axis to rotate around and its magnitude represents
-// the number of radians to rotate.
-Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
-    const Eigen::Matrix<double, 4, 1> &X);
-
-inline Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
-    const Eigen::Quaternion<double> &X) {
-  return ToRotationVectorFromQuaternion(X.coeffs());
-};
-
-// Converts from a rotation vector to a quaternion. If you supply max_angle_cap,
-// then the rotation vector's magnitude will be clipped to be no more than
-// max_angle_cap before being converted to a quaternion.
-Eigen::Matrix<double, 4, 1> ToQuaternionFromRotationVector(
-    const Eigen::Matrix<double, 3, 1> &X,
-    const double max_angle_cap = std::numeric_limits<double>::infinity());
-
 // Generates the sigma points to use in the UKF given the current estimate and
 // covariance.
 Eigen::Matrix<double, 4, 3 * 2 + 1> GenerateSigmaPoints(
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
index 2edce5a..c4643d9 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
@@ -1,11 +1,12 @@
-#include "frc971/control_loops/runge_kutta.h"
+#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
 
 #include <Eigen/Geometry>
 #include <random>
 
+#include "aos/controls/quaternion_utils.h"
 #include "aos/testing/random_seed.h"
 #include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
-#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
+#include "frc971/control_loops/runge_kutta.h"
 #include "glog/logging.h"
 #include "gtest/gtest.h"
 
@@ -220,31 +221,6 @@
       << dtukf.H(dtukf.X_hat().coeffs());
 }
 
-// Tests that small perturbations around a couple quaternions averaged out
-// return the original quaternion.
-TEST(DownEstimatorTest, QuaternionMean) {
-  Eigen::Matrix<double, 4, 7> vectors;
-  vectors.col(0) << 0, 0, 0, 1;
-  for (int i = 0; i < 3; ++i) {
-    Eigen::Matrix<double, 4, 1> perturbation;
-    perturbation.setZero();
-    perturbation(i, 0) = 0.1;
-
-    vectors.col(i * 2 + 1) = vectors.col(0) + perturbation;
-    vectors.col(i * 2 + 2) = vectors.col(0) - perturbation;
-  }
-
-  for (int i = 0; i < 7; ++i) {
-    vectors.col(i).normalize();
-  }
-
-  Eigen::Matrix<double, 4, 1> mean = drivetrain::QuaternionMean(vectors);
-
-  for (int i = 0; i < 4; ++i) {
-    EXPECT_NEAR(mean(i, 0), vectors(i, 0), 0.001) << ": Failed on index " << i;
-  }
-}
-
 // Tests that computing sigma points, and then computing the mean and covariance
 // returns the original answer.
 TEST(DownEstimatorTest, SigmaPoints) {
@@ -259,7 +235,7 @@
       drivetrain::GenerateSigmaPoints(mean, covariance);
 
   const Eigen::Matrix<double, 4, 1> calculated_mean =
-      drivetrain::QuaternionMean(vectors);
+      aos::controls::QuaternionMean(vectors);
 
   VLOG(1) << "actual mean: " << mean.coeffs();
   VLOG(1) << "calculated mean: " << calculated_mean;
@@ -290,7 +266,7 @@
       drivetrain::GenerateSigmaPoints(mean, covariance);
 
   const Eigen::Matrix<double, 4, 1> calculated_mean =
-      drivetrain::QuaternionMean(vectors);
+      aos::controls::QuaternionMean(vectors);
 
   Eigen::Matrix<double, 3, 3 * 2 + 1> Wprime;
   Eigen::Matrix<double, 3, 3> calculated_covariance =
@@ -308,120 +284,6 @@
          "covariance.";
 }
 
-// Tests that ToRotationVectorFromQuaternion works for a 0 rotation.
-TEST(DownEstimatorTest, ToRotationVectorFromQuaternionAtZero) {
-  Eigen::Matrix<double, 3, 1> vector =
-      drivetrain::ToRotationVectorFromQuaternion(
-          Eigen::Quaternion<double>(
-              Eigen::AngleAxis<double>(0.0, Eigen::Vector3d::UnitX()))
-              .coeffs());
-
-  EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::Zero()).norm(), 1e-4);
-}
-
-// Tests that ToRotationVectorFromQuaternion works for a real rotation.
-TEST(DownEstimatorTest, ToRotationVectorFromQuaternion) {
-  Eigen::Matrix<double, 3, 1> vector =
-      drivetrain::ToRotationVectorFromQuaternion(
-          Eigen::Quaternion<double>(
-              Eigen::AngleAxis<double>(M_PI * 0.5, Eigen::Vector3d::UnitX()))
-              .coeffs());
-
-  EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::UnitX() * M_PI * 0.5).norm(),
-              1e-4);
-}
-
-// Tests that ToRotationVectorFromQuaternion works for a solution with negative
-// coefficients.
-TEST(DownEstimatorTest, ToRotationVectorFromQuaternionNegative) {
-  Eigen::Matrix<double, 3, 1> vector =
-      drivetrain::ToRotationVectorFromQuaternion(
-          Eigen::Quaternion<double>(
-              -Eigen::Quaternion<double>(
-                   Eigen::AngleAxis<double>(M_PI * 0.5,
-                                              Eigen::Vector3d::UnitX()))
-                   .coeffs())
-              .coeffs());
-
-  EXPECT_NEAR(0.0, (vector - Eigen::Vector3d::UnitX() * M_PI * 0.5).norm(),
-              1e-4);
-}
-
-// Tests that ToQuaternionFromRotationVector works for a 0 rotation.
-TEST(DownEstimatorTest, ToQuaternionFromRotationVectorAtZero) {
-  Eigen::Matrix<double, 4, 1> quaternion =
-      drivetrain::ToQuaternionFromRotationVector(Eigen::Vector3d::Zero());
-
-  EXPECT_NEAR(0.0, (quaternion -
-                    Eigen::Quaternion<double>(
-                        Eigen::AngleAxis<double>(0.0, Eigen::Vector3d::UnitX()))
-                        .coeffs()).norm(),
-              1e-4);
-}
-
-// Tests that ToQuaternionFromRotationVector works for a real rotation.
-TEST(DownEstimatorTest, ToQuaternionFromRotationVector) {
-  Eigen::Matrix<double, 4, 1> quaternion =
-      drivetrain::ToQuaternionFromRotationVector(Eigen::Vector3d::UnitX() *
-                                                 M_PI * 0.5);
-
-  EXPECT_NEAR(0.0, (quaternion -
-                    Eigen::Quaternion<double>(
-                        Eigen::AngleAxis<double>(
-                            M_PI * 0.5, Eigen::Vector3d::UnitX())).coeffs())
-
-                       .norm(),
-              1e-4);
-}
-
-// Tests that ToQuaternionFromRotationVector correctly clips a rotation vector
-// that is too large in magnitude.
-TEST(DownEstimatorTest, ToQuaternionFromLargeRotationVector) {
-  const double kMaxAngle = 2.0;
-  const Eigen::Vector3d rotation_vector =
-      Eigen::Vector3d::UnitX() * kMaxAngle * 2.0;
-  const Eigen::Matrix<double, 3, 1> clipped_vector =
-      drivetrain::ToRotationVectorFromQuaternion(
-          drivetrain::ToQuaternionFromRotationVector(rotation_vector,
-                                                     kMaxAngle));
-
-  EXPECT_NEAR(0.0, (rotation_vector / 2.0 - clipped_vector).norm(), 1e-4);
-}
-
-// Tests that ToQuaternionFromRotationVector and ToRotationVectorFromQuaternion
-// works for random rotations.
-TEST(DownEstimatorTest, RandomQuaternions) {
-  std::mt19937 generator(aos::testing::RandomSeed());
-  std::uniform_real_distribution<double> random_scalar(-1.0, 1.0);
-
-  for (int i = 0; i < 1000; ++i) {
-    Eigen::Matrix<double, 3, 1> axis;
-    axis << random_scalar(generator), random_scalar(generator),
-        random_scalar(generator);
-    EXPECT_GE(axis.norm(), 1e-6);
-    axis.normalize();
-
-    const double angle = random_scalar(generator) * M_PI;
-
-    Eigen::Matrix<double, 4, 1> quaternion =
-        drivetrain::ToQuaternionFromRotationVector(axis * angle);
-
-    Eigen::Quaternion<double> answer(Eigen::AngleAxis<double>(angle, axis));
-
-    EXPECT_NEAR(quaternion(3, 0), std::cos(angle / 2.0), 1e-8);
-    EXPECT_NEAR(answer.w(), std::cos(angle / 2.0), 1e-8);
-
-    EXPECT_NEAR(1.0, (answer.coeffs() * quaternion.transpose()).norm(), 1e-6);
-
-    const Eigen::Matrix<double, 3, 1> recalculated_axis =
-        drivetrain::ToRotationVectorFromQuaternion(quaternion);
-
-    EXPECT_NEAR(std::abs(angle), recalculated_axis.norm(), 1e-8);
-
-    EXPECT_NEAR(0.0, (axis * angle - recalculated_axis).norm(), 1e-8);
-  }
-}
-
 }  // namespace testing
 }  // namespace control_loops
 }  // namespace frc971