diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 6d895de..d46bca6 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -265,6 +265,19 @@
     ],
 )
 
+cc_test(
+    name = "c2d_test",
+    srcs = [
+        "c2d_test.cc",
+    ],
+    visibility = ["//visibility:public"],
+    deps = [
+        ":c2d",
+        ":runge_kutta",
+        "//aos/testing:googletest",
+    ],
+)
+
 cc_library(
     name = "c2d",
     hdrs = [
diff --git a/frc971/control_loops/c2d.h b/frc971/control_loops/c2d.h
index 453a6a5..1bf99a2 100644
--- a/frc971/control_loops/c2d.h
+++ b/frc971/control_loops/c2d.h
@@ -4,6 +4,8 @@
 #include <chrono>
 
 #include <Eigen/Dense>
+// We need to include MatrixFunctions for the matrix exponential.
+#include "unsupported/Eigen/MatrixFunctions"
 
 namespace frc971 {
 namespace controls {
@@ -33,6 +35,39 @@
   *B = M_state.template block<num_states, num_inputs>(0, num_states);
 }
 
+template <typename Scalar, int num_states>
+void DiscretizeQ(
+    const Eigen::Matrix<Scalar, num_states, num_states> &Q_continuous,
+    const Eigen::Matrix<Scalar, num_states, num_states> &A_continuous,
+    ::std::chrono::nanoseconds dt,
+    Eigen::Matrix<Scalar, num_states, num_states> *Q_d) {
+  Eigen::Matrix<Scalar, num_states, num_states> Qtemp =
+      (Q_continuous + Q_continuous.transpose()) / static_cast<Scalar>(2.0);
+  Eigen::Matrix<Scalar, 2 * num_states, 2 * num_states> M_gain;
+  M_gain.setZero();
+  // Set up the matrix M = [[-A, Q], [0, A.T]]
+  M_gain.template block<num_states, num_states>(0, 0) = -A_continuous;
+  M_gain.template block<num_states, num_states>(0, num_states) = Qtemp;
+  M_gain.template block<num_states, num_states>(num_states, num_states) =
+      A_continuous.transpose();
+
+  Eigen::Matrix<Scalar, 2 * num_states, 2 *num_states> phi =
+      (M_gain *
+       ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
+           .count()).exp();
+
+  // Phi12 = phi[0:num_states, num_states:2*num_states]
+  // Phi22 = phi[num_states:2*num_states,
+  // num_states:2*num_states]
+  Eigen::Matrix<Scalar, num_states, num_states> phi12 =
+      phi.block(0, num_states, num_states, num_states);
+  Eigen::Matrix<Scalar, num_states, num_states> phi22 =
+      phi.block(num_states, num_states, num_states, num_states);
+
+  *Q_d = phi22.transpose() * phi12;
+  *Q_d = (*Q_d + Q_d->transpose()) / static_cast<Scalar>(2.0);
+}
+
 }  // namespace controls
 }  // namespace frc971
 
diff --git a/frc971/control_loops/c2d_test.cc b/frc971/control_loops/c2d_test.cc
new file mode 100644
index 0000000..dcdb048
--- /dev/null
+++ b/frc971/control_loops/c2d_test.cc
@@ -0,0 +1,74 @@
+#include "frc971/control_loops/c2d.h"
+
+#include <functional>
+
+#include "frc971/control_loops/runge_kutta.h"
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace controls {
+namespace testing {
+
+class C2DTest : public ::testing::Test {
+ public:
+  C2DTest() {
+    // Create a trivial second-order system.
+    A_continuous << 0, 1, 0, 0;
+    B_continuous << 0, 1;
+    Q_continuous << 1, 0, 0, 1;
+  }
+
+ protected:
+  Eigen::Matrix<double, 2, 2> A_continuous;
+  Eigen::Matrix<double, 2, 1> B_continuous;
+  Eigen::Matrix<double, 2, 2> Q_continuous;
+};
+
+// Check that for a simple second-order system that we can easily analyze
+// analytically, C2D creates valid A/B matrices.
+TEST_F(C2DTest, DiscretizeAB) {
+  Eigen::Matrix<double, 2, 1> X0;
+  X0 << 1, 1;
+  Eigen::Matrix<double, 1, 1> U;
+  U << 1;
+  Eigen::Matrix<double, 2, 2> A_d;
+  Eigen::Matrix<double, 2, 1> B_d;
+
+  C2D(A_continuous, B_continuous, ::std::chrono::seconds(1), &A_d, &B_d);
+  Eigen::Matrix<double, 2, 1> X1_discrete = A_d * X0 + B_d * U;
+  // We now have pos = vel = accel = 1, which should give us:
+  Eigen::Matrix<double, 2, 1> X1_truth;
+  X1_truth(1, 0) = X0(1, 0) + 1.0 * U(0, 0);
+  X1_truth(0, 0) = X0(0, 0) + 1.0 * X0(1, 0) + 0.5 * U(0, 0);
+  EXPECT_EQ(X1_truth, X1_discrete);
+}
+
+// Test that the discrete approximation of Q is roughly equal to
+// integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
+TEST_F(C2DTest, DiscretizeQ) {
+  Eigen::Matrix<double, 2, 2> Q_d;
+  const auto dt = ::std::chrono::seconds(1);
+  DiscretizeQ(Q_continuous, A_continuous, dt, &Q_d);
+  // TODO(james): Using Runge Kutta for this is a bit silly as f is just a
+  // function of t, not Q, but I don't want to rewrite any of our math
+  // utilities.
+  // Note that we are being very explicit about the types of everything in this
+  // integration because otherwise it doesn't compile very well.
+  Eigen::Matrix<double, 2, 2> Q_d_integrated = control_loops::RungeKutta<
+      ::std::function<Eigen::Matrix<double, 2, 2>(
+          const double, const Eigen::Matrix<double, 2, 2> &)>,
+      Eigen::Matrix<double, 2, 2>>(
+      [this](const double t, const Eigen::Matrix<double, 2, 2> &) {
+        return Eigen::Matrix<double, 2, 2>(
+            (A_continuous * t).exp() * Q_continuous *
+            (A_continuous.transpose() * t).exp());
+      },
+      Eigen::Matrix<double, 2, 2>::Zero(), 0, 1.0);
+  EXPECT_LT((Q_d_integrated - Q_d).norm(), 1e-10)
+      << "Expected these to be nearly equal:\nQ_d:\n" << Q_d
+      << "\nQ_d_integrated:\n" << Q_d_integrated;
+}
+
+}  // namespace testing
+}  // namespace controls
+}  // namespace frc971
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index 0af0144..ad7551f 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -336,43 +336,15 @@
   void UpdateQR(StateFeedbackHybridPlant<number_of_states, number_of_inputs,
                                          number_of_outputs> *plant,
                 ::std::chrono::nanoseconds dt) {
-    // Now, compute the discrete time Q and R coefficients.
-    Eigen::Matrix<Scalar, number_of_states, number_of_states> Qtemp =
-        (coefficients().Q_continuous +
-         coefficients().Q_continuous.transpose()) /
-        static_cast<Scalar>(2.0);
+    ::frc971::controls::DiscretizeQ(coefficients().Q_continuous,
+                                    plant->coefficients().A_continuous, dt,
+                                    &Q_);
+
     Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> Rtemp =
         (coefficients().R_continuous +
          coefficients().R_continuous.transpose()) /
         static_cast<Scalar>(2.0);
 
-    Eigen::Matrix<Scalar, 2 * number_of_states, 2 * number_of_states> M_gain;
-    M_gain.setZero();
-    // Set up the matrix M = [[-A, Q], [0, A.T]]
-    M_gain.template block<number_of_states, number_of_states>(0, 0) =
-        -plant->coefficients().A_continuous;
-    M_gain.template block<number_of_states, number_of_states>(
-        0, number_of_states) = Qtemp;
-    M_gain.template block<number_of_states, number_of_states>(
-        number_of_states, number_of_states) =
-        plant->coefficients().A_continuous.transpose();
-
-    Eigen::Matrix<Scalar, 2 * number_of_states, 2 *number_of_states> phi =
-        (M_gain *
-         ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
-             .count())
-            .exp();
-
-    // Phi12 = phi[0:number_of_states, number_of_states:2*number_of_states]
-    // Phi22 = phi[number_of_states:2*number_of_states,
-    // number_of_states:2*number_of_states]
-    Eigen::Matrix<Scalar, number_of_states, number_of_states> phi12 =
-        phi.block(0, number_of_states, number_of_states, number_of_states);
-    Eigen::Matrix<Scalar, number_of_states, number_of_states> phi22 = phi.block(
-        number_of_states, number_of_states, number_of_states, number_of_states);
-
-    Q_ = phi22.transpose() * phi12;
-    Q_ = (Q_ + Q_.transpose()) / static_cast<Scalar>(2.0);
     R_ = Rtemp /
          ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
              .count();