Created RungeKutta integrator and tests
Change-Id: I6f8587d11ad4872821456e2d1eabfe576bcaf971
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 9dee424..a1b0e68 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -124,3 +124,25 @@
':state_feedback_loop',
],
)
+
+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',
+ ],
+)
diff --git a/frc971/control_loops/runge_kutta.h b/frc971/control_loops/runge_kutta.h
new file mode 100644
index 0000000..fe88da0
--- /dev/null
+++ b/frc971/control_loops/runge_kutta.h
@@ -0,0 +1,25 @@
+#ifndef FRC971_CONTROL_LOOPS_RUNGE_KUTTA_H_
+#define FRC971_CONTROL_LOOPS_RUNGE_KUTTA_H_
+
+#include <Eigen/Dense>
+
+namespace frc971 {
+namespace control_loops {
+
+// Implements Runge Kutta integration (4th order). fn is the function to
+// integrate. It must take 1 argument of type T. The integration starts at an
+// initial value X, and integrates for dt.
+template <typename F, typename T>
+T RungeKutta(const F &fn, T X, double dt) {
+ const double half_dt = dt * 0.5;
+ auto k1 = fn(X);
+ auto k2 = fn(X + half_dt * k1);
+ auto k3 = fn(X + half_dt * k2);
+ auto k4 = fn(X + dt * k3);
+ return X + dt / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+}
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_RUNGE_KUTTA_H_
diff --git a/frc971/control_loops/runge_kutta_test.cc b/frc971/control_loops/runge_kutta_test.cc
new file mode 100644
index 0000000..680715a
--- /dev/null
+++ b/frc971/control_loops/runge_kutta_test.cc
@@ -0,0 +1,26 @@
+#include "frc971/control_loops/runge_kutta.h"
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// Tests that integrating dx/dt = e^x works.
+TEST(RungeKuttaTest, Exponential) {
+ ::Eigen::Matrix<double, 1, 1> y0;
+ y0(0, 0) = 0.0;
+
+ ::Eigen::Matrix<double, 1, 1> y1 = RungeKutta(
+ [](::Eigen::Matrix<double, 1, 1> x) {
+ ::Eigen::Matrix<double, 1, 1> y;
+ y(0, 0) = ::std::exp(x(0, 0));
+ return y;
+ },
+ y0, 0.1);
+ EXPECT_NEAR(y1(0, 0), ::std::exp(0.1) - ::std::exp(0), 1e-3);
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971