Add numerical jacobian library.
Change-Id: I1f54172a561f00c9cca2b218f16a1f1dba776d5c
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_