Add a time varying RK4 integration
Splines need a RK4 integration which is a function of time.
Change-Id: Iddd3dffe50beaf3b9d5b70e58ee88df5d727b853
diff --git a/frc971/control_loops/runge_kutta.h b/frc971/control_loops/runge_kutta.h
index c3d47ed..61d674e 100644
--- a/frc971/control_loops/runge_kutta.h
+++ b/frc971/control_loops/runge_kutta.h
@@ -19,6 +19,20 @@
return X + dt / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
+// Implements Runge Kutta integration (4th order). This integrates dy/dt = fn(t,
+// y). It must have the call signature of fn(double t, T y). The
+// integration starts at an initial value y, and integrates for dt.
+template <typename F, typename T>
+T RungeKutta(const F &fn, T y, double t, double dt) {
+ const double half_dt = dt * 0.5;
+ T k1 = dt * fn(t, y);
+ T k2 = dt * fn(t + half_dt, y + k1 / 2.0);
+ T k3 = dt * fn(t + half_dt, y + k2 / 2.0);
+ T k4 = dt * fn(t + dt, y + k3);
+
+ return y + (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0;
+}
+
// 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.
diff --git a/frc971/control_loops/runge_kutta_test.cc b/frc971/control_loops/runge_kutta_test.cc
index abd9466..615f82c 100644
--- a/frc971/control_loops/runge_kutta_test.cc
+++ b/frc971/control_loops/runge_kutta_test.cc
@@ -36,6 +36,33 @@
EXPECT_NEAR(y1(0, 0), ::std::exp(0.1) - ::std::exp(0), 1e-3);
}
+::Eigen::Matrix<double, 1, 1> RungeKuttaTimeVaryingSolution(double t) {
+ return (::Eigen::Matrix<double, 1, 1>()
+ << 12.0 * ::std::exp(t) / (::std::pow(::std::exp(t) + 1.0, 2.0)))
+ .finished();
+}
+
+// Tests RungeKutta with a time varying solution.
+// Now, lets test RK4 with a time varying solution. From
+// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
+// x' = x (2 / (e^t + 1) - 1)
+//
+// The true (analytical) solution is:
+//
+// x(t) = 12 * e^t / ((e^t + 1)^2)
+TEST(RungeKuttaTest, RungeKuttaTimeVarying) {
+ ::Eigen::Matrix<double, 1, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
+
+ ::Eigen::Matrix<double, 1, 1> y1 = RungeKutta(
+ [](double t, ::Eigen::Matrix<double, 1, 1> x) {
+ return (::Eigen::Matrix<double, 1, 1>()
+ << x(0, 0) * (2.0 / (::std::exp(t) + 1.0) - 1.0))
+ .finished();
+ },
+ y0, 5.0, 1.0);
+ EXPECT_NEAR(y1(0, 0), RungeKuttaTimeVaryingSolution(6.0)(0, 0), 1e-3);
+}
+
} // namespace testing
} // namespace control_loops
} // namespace frc971