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