Add a RK4 method which takes a U.
Now we can integrate dynamics too!
Change-Id: I3c3b1a23746310eac66a0b65906f6a69865dcab2
diff --git a/frc971/control_loops/runge_kutta.h b/frc971/control_loops/runge_kutta.h
index 4804054..020a25d 100644
--- a/frc971/control_loops/runge_kutta.h
+++ b/frc971/control_loops/runge_kutta.h
@@ -19,6 +19,19 @@
return X + dt / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
+// 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, typename Tu>
+T RungeKutta(const F &fn, T X, Tu U, double dt) {
+ const double half_dt = dt * 0.5;
+ T k1 = fn(X, U);
+ T k2 = fn(X + half_dt * k1, U);
+ T k3 = fn(X + half_dt * k2, U);
+ T k4 = fn(X + dt * k3, U);
+ return X + dt / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+}
+
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/runge_kutta_test.cc b/frc971/control_loops/runge_kutta_test.cc
index 680715a..aa64638 100644
--- a/frc971/control_loops/runge_kutta_test.cc
+++ b/frc971/control_loops/runge_kutta_test.cc
@@ -21,6 +21,21 @@
EXPECT_NEAR(y1(0, 0), ::std::exp(0.1) - ::std::exp(0), 1e-3);
}
+// Tests that integrating dx/dt = e^x works when we provide a U.
+TEST(RungeKuttaTest, ExponentialWithU) {
+ ::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> u) {
+ ::Eigen::Matrix<double, 1, 1> y;
+ y(0, 0) = ::std::exp(u(0, 0) * x(0, 0));
+ return y;
+ },
+ y0, (::Eigen::Matrix<double, 1, 1>() << 1.0).finished(), 0.1);
+ EXPECT_NEAR(y1(0, 0), ::std::exp(0.1) - ::std::exp(0), 1e-3);
+}
+
} // namespace testing
} // namespace control_loops
} // namespace frc971