diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 7988e51..b6b0362 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -155,6 +155,7 @@
         "hybrid_state_feedback_loop.h",
     ],
     deps = [
+        ":c2d",
         ":state_feedback_loop",
         "//aos:macros",
         "//aos/controls:control_loop",
@@ -265,6 +266,17 @@
 )
 
 cc_library(
+    name = "c2d",
+    hdrs = [
+        "c2d.h",
+    ],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//third_party/eigen",
+    ],
+)
+
+cc_library(
     name = "dlqr",
     hdrs = [
         "dlqr.h",
diff --git a/frc971/control_loops/c2d.h b/frc971/control_loops/c2d.h
new file mode 100644
index 0000000..453a6a5
--- /dev/null
+++ b/frc971/control_loops/c2d.h
@@ -0,0 +1,39 @@
+#ifndef FRC971_CONTROL_LOOPS_C2D_H_
+#define FRC971_CONTROL_LOOPS_C2D_H_
+
+#include <chrono>
+
+#include <Eigen/Dense>
+
+namespace frc971 {
+namespace controls {
+
+template <typename Scalar, int num_states, int num_inputs>
+void C2D(const ::Eigen::Matrix<Scalar, num_states, num_states> &A_continuous,
+         const ::Eigen::Matrix<Scalar, num_states, num_inputs> &B_continuous,
+         ::std::chrono::nanoseconds dt,
+         ::Eigen::Matrix<Scalar, num_states, num_states> *A,
+         ::Eigen::Matrix<Scalar, num_states, num_inputs> *B) {
+  // Trick from
+  // https://en.wikipedia.org/wiki/Discretization#Discretization_of_linear_state_space_models
+  // to solve for A and B more efficiently.
+  Eigen::Matrix<Scalar, num_states + num_inputs, num_states + num_inputs>
+      M_state_continuous;
+  M_state_continuous.setZero();
+  M_state_continuous.template block<num_states, num_states>(0, 0) =
+      A_continuous *
+      ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt).count();
+  M_state_continuous.template block<num_states, num_inputs>(0, num_states) =
+      B_continuous *
+      ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt).count();
+
+  Eigen::Matrix<Scalar, num_states + num_inputs, num_states + num_inputs>
+      M_state = M_state_continuous.exp();
+  *A = M_state.template block<num_states, num_states>(0, 0);
+  *B = M_state.template block<num_states, num_inputs>(0, num_states);
+}
+
+}  // namespace controls
+}  // namespace frc971
+
+#endif  // FRC971_CONTROL_LOOPS_C2D_H_
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index 0fb803f..05d5aab 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -15,6 +15,7 @@
 #include "aos/controls/control_loop.h"
 #include "aos/logging/logging.h"
 #include "aos/macros.h"
+#include "frc971/control_loops/c2d.h"
 #include "frc971/control_loops/state_feedback_loop.h"
 
 template <int number_of_states, int number_of_inputs, int number_of_outputs,
@@ -188,27 +189,8 @@
 
  private:
   void UpdateAB(::std::chrono::nanoseconds dt) {
-    Eigen::Matrix<Scalar, number_of_states + number_of_inputs,
-                  number_of_states + number_of_inputs>
-        M_state_continuous;
-    M_state_continuous.setZero();
-    M_state_continuous.template block<number_of_states, number_of_states>(0,
-                                                                          0) =
-        coefficients().A_continuous *
-        ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
-            .count();
-    M_state_continuous.template block<number_of_states, number_of_inputs>(
-        0, number_of_states) =
-        coefficients().B_continuous *
-        ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt)
-            .count();
-
-    Eigen::Matrix<Scalar, number_of_states + number_of_inputs,
-                  number_of_states + number_of_inputs>
-        M_state = M_state_continuous.exp();
-    A_ = M_state.template block<number_of_states, number_of_states>(0, 0);
-    B_ = M_state.template block<number_of_states, number_of_inputs>(
-        0, number_of_states);
+    ::frc971::controls::C2D(coefficients().A_continuous,
+                            coefficients().B_continuous, dt, &A_, &B_);
   }
 
   Eigen::Matrix<Scalar, number_of_states, 1> X_;