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_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