Add ode45 to runge_kutta.h
This gives us a way to integrate with an adaptive step size for when we
don't know the time constants super well.
Change-Id: Ie6073c208ae9988957f0c4cd79f9519a4a978efe
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/runge_kutta_test.cc b/frc971/control_loops/runge_kutta_test.cc
index 1022a47..357b7f5 100644
--- a/frc971/control_loops/runge_kutta_test.cc
+++ b/frc971/control_loops/runge_kutta_test.cc
@@ -90,4 +90,28 @@
EXPECT_NEAR(y1(0, 0), RungeKuttaTimeVaryingSolution(6.0)(0, 0), 1e-7);
}
+// Tests that integrating dx/dt = e^x works with RK45
+TEST(RungeKuttaTest, RungeKuttaTimeVaryingAdaptive) {
+ ::Eigen::Matrix<double, 1, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
+
+ size_t count = 0;
+
+ ::Eigen::Matrix<double, 1, 1> y1 = AdaptiveRungeKutta(
+ [&](double t, ::Eigen::Matrix<double, 1, 1> x) {
+ ++count;
+ return (::Eigen::Matrix<double, 1, 1>()
+ << x(0, 0) * (2.0 / (::std::exp(t) + 1.0) - 1.0))
+ .finished();
+ },
+ y0, 5.0, 1.0, 1e-6, 1e-9);
+ EXPECT_NEAR(y1(0, 0), RungeKuttaTimeVaryingSolution(6.0)(0, 0), 1e-7);
+
+ // Using sympy as a benchmark, we should expect to see the function called 38
+ // times.
+ EXPECT_EQ(count, 38);
+
+ LOG(INFO) << "Got " << y1(0, 0) << " vs expected "
+ << RungeKuttaTimeVaryingSolution(6.0)(0, 0);
+}
+
} // namespace frc971::control_loops::testing