diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index eccbb14..01dc25e 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -1,204 +1,228 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
 
-load('//aos/build:queues.bzl', 'queue_library')
+load("//aos/build:queues.bzl", "queue_library")
 
 cc_library(
-  name = 'team_number_test_environment',
-  srcs = [
-    'team_number_test_environment.cc',
-  ],
-  hdrs = [
-    'team_number_test_environment.h',
-  ],
-  deps = [
-    '//aos/common/network:team_number',
-    '//aos/testing:googletest',
-  ],
-  testonly = True,
+    name = "team_number_test_environment",
+    testonly = True,
+    srcs = [
+        "team_number_test_environment.cc",
+    ],
+    hdrs = [
+        "team_number_test_environment.h",
+    ],
+    deps = [
+        "//aos/common/network:team_number",
+        "//aos/testing:googletest",
+    ],
 )
 
 cc_test(
-  name = 'hybrid_state_feedback_loop_test',
-  srcs = [
-    'hybrid_state_feedback_loop_test.cc',
-  ],
-  deps = [
-    ':hybrid_state_feedback_loop',
-    '//aos/testing:googletest',
-  ],
+    name = "hybrid_state_feedback_loop_test",
+    srcs = [
+        "hybrid_state_feedback_loop_test.cc",
+    ],
+    deps = [
+        ":hybrid_state_feedback_loop",
+        "//aos/testing:googletest",
+    ],
 )
 
 cc_library(
-  name = 'hall_effect_tracker',
-  hdrs = [
-    'hall_effect_tracker.h',
-  ],
-  deps = [
-    ':queues',
-  ],
+    name = "hall_effect_tracker",
+    hdrs = [
+        "hall_effect_tracker.h",
+    ],
+    deps = [
+        ":queues",
+    ],
 )
 
 queue_library(
-  name = 'queues',
-  srcs = [
-    'control_loops.q',
-  ],
+    name = "queues",
+    srcs = [
+        "control_loops.q",
+    ],
 )
 
 cc_test(
-  name = 'position_sensor_sim_test',
-  srcs = [
-    'position_sensor_sim_test.cc',
-  ],
-  deps = [
-    ':queues',
-    ':position_sensor_sim',
-    '//aos/testing:googletest',
-    '//aos/common/logging',
-  ],
+    name = "position_sensor_sim_test",
+    srcs = [
+        "position_sensor_sim_test.cc",
+    ],
+    deps = [
+        ":position_sensor_sim",
+        ":queues",
+        "//aos/common/logging",
+        "//aos/testing:googletest",
+    ],
 )
 
 cc_library(
-  name = 'position_sensor_sim',
-  testonly = True,
-  srcs = [
-    'position_sensor_sim.cc',
-  ],
-  hdrs = [
-    'position_sensor_sim.h',
-  ],
-  deps = [
-    ':queues',
-    ':gaussian_noise',
-    '//debian:libm',
-    '//aos/testing:random_seed',
-  ],
+    name = "position_sensor_sim",
+    testonly = True,
+    srcs = [
+        "position_sensor_sim.cc",
+    ],
+    hdrs = [
+        "position_sensor_sim.h",
+    ],
+    deps = [
+        ":gaussian_noise",
+        ":queues",
+        "//aos/testing:random_seed",
+        "//debian:libm",
+    ],
 )
 
 cc_library(
-  name = 'gaussian_noise',
-  srcs = [
-    'gaussian_noise.cc',
-  ],
-  hdrs = [
-    'gaussian_noise.h',
-  ],
-  deps = [
-    '//debian:libm',
-  ],
+    name = "gaussian_noise",
+    srcs = [
+        "gaussian_noise.cc",
+    ],
+    hdrs = [
+        "gaussian_noise.h",
+    ],
+    deps = [
+        "//debian:libm",
+    ],
 )
 
 cc_library(
-  name = 'coerce_goal',
-  srcs = [
-    'coerce_goal.cc',
-  ],
-  hdrs = [
-    'coerce_goal.h',
-  ],
-  deps = [
-    '//third_party/eigen',
-    '//aos/common/controls:polytope',
-    '//debian:libm',
-  ],
+    name = "coerce_goal",
+    srcs = [
+        "coerce_goal.cc",
+    ],
+    hdrs = [
+        "coerce_goal.h",
+    ],
+    deps = [
+        "//aos/common/controls:polytope",
+        "//debian:libm",
+        "//third_party/eigen",
+    ],
 )
 
 # TODO(austin): Select isn't working right.  We should be able to remove
 # logging conditionally with select and have CPU constraints work correctly.
 cc_library(
-  name = 'state_feedback_loop_uc',
-  hdrs = [
-    'state_feedback_loop.h',
-  ],
-  deps = [
-    '//aos/common:macros',
-    '//third_party/eigen',
-  ],
-  restricted_to = ['//tools:cortex-m4f'],
-)
-cc_library(
-  name = 'state_feedback_loop',
-  hdrs = [
-    'state_feedback_loop.h',
-  ],
-  deps = [
-    '//aos/common/logging',
-    '//aos/common:macros',
-    '//third_party/eigen',
-  ],
+    name = "state_feedback_loop_uc",
+    hdrs = [
+        "state_feedback_loop.h",
+    ],
+    restricted_to = ["//tools:cortex-m4f"],
+    deps = [
+        "//aos/common:macros",
+        "//third_party/eigen",
+    ],
 )
 
 cc_library(
-  name = 'hybrid_state_feedback_loop',
-  hdrs = [
-    'hybrid_state_feedback_loop.h',
-  ],
-  deps = [
-    ':state_feedback_loop',
-    '//aos/common/controls:control_loop',
-    '//aos/common/logging',
-    '//aos/common:macros',
-    '//third_party/eigen',
-  ],
+    name = "state_feedback_loop",
+    hdrs = [
+        "state_feedback_loop.h",
+    ],
+    deps = [
+        "//aos/common:macros",
+        "//aos/common/logging",
+        "//third_party/eigen",
+    ],
 )
 
 cc_library(
-  name = 'simple_capped_state_feedback_loop',
-  hdrs = [
-    'simple_capped_state_feedback_loop.h',
-  ],
-  deps = [
-    '//third_party/eigen',
-    ':state_feedback_loop',
-  ],
+    name = "hybrid_state_feedback_loop",
+    hdrs = [
+        "hybrid_state_feedback_loop.h",
+    ],
+    deps = [
+        ":state_feedback_loop",
+        "//aos/common:macros",
+        "//aos/common/controls:control_loop",
+        "//aos/common/logging",
+        "//third_party/eigen",
+    ],
 )
 
 cc_library(
-  name = 'runge_kutta',
-  hdrs = [
-    'runge_kutta.h',
-  ],
-  deps = [
-    '//third_party/eigen',
-  ],
+    name = "simple_capped_state_feedback_loop",
+    hdrs = [
+        "simple_capped_state_feedback_loop.h",
+    ],
+    deps = [
+        ":state_feedback_loop",
+        "//third_party/eigen",
+    ],
+)
+
+cc_library(
+    name = "runge_kutta",
+    hdrs = [
+        "runge_kutta.h",
+    ],
+    deps = [
+        "//third_party/eigen",
+    ],
 )
 
 cc_test(
-  name = 'runge_kutta_test',
-  srcs = [
-    'runge_kutta_test.cc',
-  ],
-  deps = [
-    ':runge_kutta',
-    '//aos/testing:googletest',
-    '//third_party/eigen',
-  ],
+    name = "runge_kutta_test",
+    srcs = [
+        "runge_kutta_test.cc",
+    ],
+    deps = [
+        ":runge_kutta",
+        "//aos/testing:googletest",
+        "//third_party/eigen",
+    ],
 )
 
 queue_library(
-  name = 'profiled_subsystem_queue',
-  srcs = [
-    'profiled_subsystem.q',
-  ],
-  deps = [
-    ':queues',
-  ],
+    name = "profiled_subsystem_queue",
+    srcs = [
+        "profiled_subsystem.q",
+    ],
+    deps = [
+        ":queues",
+    ],
 )
 
 cc_library(
-  name = 'profiled_subsystem',
-  srcs = [
-    'profiled_subsystem.cc',
-  ],
-  hdrs = [
-    'profiled_subsystem.h',
-  ],
-  deps = [
-    ':profiled_subsystem_queue',
-    ':simple_capped_state_feedback_loop',
-    ':state_feedback_loop',
-    '//aos/common/controls:control_loop',
-    '//aos/common/util:trapezoid_profile',
-    '//frc971/zeroing:zeroing',
-  ],
+    name = "profiled_subsystem",
+    srcs = [
+        "profiled_subsystem.cc",
+    ],
+    hdrs = [
+        "profiled_subsystem.h",
+    ],
+    deps = [
+        ":profiled_subsystem_queue",
+        ":simple_capped_state_feedback_loop",
+        ":state_feedback_loop",
+        "//aos/common/controls:control_loop",
+        "//aos/common/util:trapezoid_profile",
+        "//frc971/zeroing",
+    ],
 )
+
+cc_library(
+    name = "jacobian",
+    hdrs = [
+        "jacobian.h",
+    ],
+    deps = [
+        "//third_party/eigen",
+    ],
+)
+
+cc_test(
+    name = "jacobian_test",
+    srcs = [
+        "jacobian_test.cc",
+    ],
+    deps = [
+        ":jacobian",
+        "//aos/testing:googletest",
+        "//third_party/eigen",
+    ],
+)
+
diff --git a/frc971/control_loops/jacobian.h b/frc971/control_loops/jacobian.h
new file mode 100644
index 0000000..fe3c157
--- /dev/null
+++ b/frc971/control_loops/jacobian.h
@@ -0,0 +1,52 @@
+#ifndef FRC971_CONTROL_LOOPS_JACOBIAN_H_
+#define FRC971_CONTROL_LOOPS_JACOBIAN_H_
+
+#include <Eigen/Dense>
+
+namespace frc971 {
+namespace control_loops {
+
+template <int num_states, int num_inputs, typename F>
+::Eigen::Matrix<double, num_states, num_inputs> NumericalJacobian(
+    const F &fn, ::Eigen::Matrix<double, num_inputs, 1> input) {
+  constexpr double kEpsilon = 1e-4;
+  ::Eigen::Matrix<double, num_states, num_inputs> result =
+      ::Eigen::Matrix<double, num_states, num_inputs>::Zero();
+
+  // It's more expensive, but +- epsilon will be more accurate
+  for (int i = 0; i < num_inputs; ++i) {
+    ::Eigen::Matrix<double, num_inputs, 1> dX_plus = input;
+    dX_plus(i, 0) += kEpsilon;
+    ::Eigen::Matrix<double, num_inputs, 1> dX_minus = input;
+    dX_minus(i, 0) -= kEpsilon;
+    result.col(i) = (fn(dX_plus) - fn(dX_minus)) / (kEpsilon * 2.0);
+  }
+  return result;
+}
+
+// Implements a numerical jacobian with respect to X for f(X, U, ...).
+template <int num_states, int num_u, typename F, typename... Args>
+::Eigen::Matrix<double, num_states, num_states> NumericalJacobianX(
+    const F &fn, ::Eigen::Matrix<double, num_states, 1> X,
+    ::Eigen::Matrix<double, num_u, 1> U, Args &&... args) {
+  return NumericalJacobian<num_states, num_states>(
+      [&](::Eigen::Matrix<double, num_states, 1> X) {
+        return fn(X, U, args...);
+      },
+      X);
+}
+
+// Implements a numerical jacobian with respect to U for f(X, U, ...).
+template <int num_states, int num_u, typename F, typename... Args>
+::Eigen::Matrix<double, num_states, num_u> NumericalJacobianU(
+    const F &fn, ::Eigen::Matrix<double, num_states, 1> X,
+    ::Eigen::Matrix<double, num_u, 1> U, Args &&... args) {
+  return NumericalJacobian<num_states, num_u>(
+      [&](::Eigen::Matrix<double, num_u, 1> U) { return fn(X, U, args...); },
+      U);
+}
+
+}  // namespace control_loops
+}  // namespace frc971
+
+#endif  // FRC971_CONTROL_LOOPS_JACOBIAN_H_
diff --git a/frc971/control_loops/jacobian_test.cc b/frc971/control_loops/jacobian_test.cc
new file mode 100644
index 0000000..6046c37
--- /dev/null
+++ b/frc971/control_loops/jacobian_test.cc
@@ -0,0 +1,40 @@
+#include "frc971/control_loops/jacobian.h"
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+::Eigen::Matrix<double, 4, 4> A = (::Eigen::Matrix<double, 4, 4>() << 1, 2, 4,
+                                   1, 5, 2, 3, 4, 5, 1, 3, 2, 1, 1, 3,
+                                   7).finished();
+
+::Eigen::Matrix<double, 4, 2> B =
+    (::Eigen::Matrix<double, 4, 2>() << 1, 1, 2, 1, 3, 2, 3, 7).finished();
+
+// Function to recover A and B from.
+::Eigen::Matrix<double, 4, 1> AxBufn(const ::Eigen::Matrix<double, 4, 1> &X,
+                                     const ::Eigen::Matrix<double, 2, 1> &U) {
+  return A * X + B * U;
+}
+
+// Test that we can recover A from AxBufn pretty accurately.
+TEST(RungeKuttaTest, Ax) {
+  ::Eigen::Matrix<double, 4, 4> NewA =
+      NumericalJacobianX<4, 2>(AxBufn, ::Eigen::Matrix<double, 4, 1>::Zero(),
+                               ::Eigen::Matrix<double, 2, 1>::Zero());
+  EXPECT_TRUE(NewA.isApprox(A));
+}
+
+// Test that we can recover B from AxBufn pretty accurately.
+TEST(RungeKuttaTest, Bu) {
+  ::Eigen::Matrix<double, 4, 2> NewB =
+      NumericalJacobianU<4, 2>(AxBufn, ::Eigen::Matrix<double, 4, 1>::Zero(),
+                               ::Eigen::Matrix<double, 2, 1>::Zero());
+  EXPECT_TRUE(NewB.isApprox(B));
+}
+
+}  // namespace testing
+}  // namespace control_loops
+}  // namespace frc971