Add a steps version of a time varying runge kutta
Aaand turns out the test was wrong... We had the wrong solution
function. I thought I was integating dx/dt = -x, and instead was
integrating dx/dt = exp(x). Integrate that for real.
Change-Id: Iea7674fbe7757bd5fc44861bbd03e6b73475f142
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/runge_kutta.h b/frc971/control_loops/runge_kutta.h
index 963f463..7fa3f3e 100644
--- a/frc971/control_loops/runge_kutta.h
+++ b/frc971/control_loops/runge_kutta.h
@@ -31,8 +31,8 @@
return X;
}
-// 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
+// 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) {
@@ -45,6 +45,15 @@
return y + (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0;
}
+template <typename F, typename T>
+T RungeKuttaSteps(const F &fn, T X, double t, double dt, int steps) {
+ dt = dt / steps;
+ for (int i = 0; i < steps; ++i) {
+ X = RungeKutta(fn, X, t + dt * i, dt);
+ }
+ return X;
+}
+
// 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 615f82c..e07c469 100644
--- a/frc971/control_loops/runge_kutta_test.cc
+++ b/frc971/control_loops/runge_kutta_test.cc
@@ -9,7 +9,7 @@
// Tests that integrating dx/dt = e^x works.
TEST(RungeKuttaTest, Exponential) {
::Eigen::Matrix<double, 1, 1> y0;
- y0(0, 0) = 0.0;
+ y0(0, 0) = 1.0;
::Eigen::Matrix<double, 1, 1> y1 = RungeKutta(
[](::Eigen::Matrix<double, 1, 1> x) {
@@ -18,7 +18,22 @@
return y;
},
y0, 0.1);
- EXPECT_NEAR(y1(0, 0), ::std::exp(0.1) - ::std::exp(0), 1e-3);
+ EXPECT_NEAR(y1(0, 0), -std::log(std::exp(-1.0) - 0.1), 1e-5);
+}
+
+// Now do it with sub steps.
+TEST(RungeKuttaTest, ExponentialSteps) {
+ ::Eigen::Matrix<double, 1, 1> y0;
+ y0(0, 0) = 1.0;
+
+ ::Eigen::Matrix<double, 1, 1> y1 = RungeKuttaSteps(
+ [](::Eigen::Matrix<double, 1, 1> x) {
+ ::Eigen::Matrix<double, 1, 1> y;
+ y(0, 0) = ::std::exp(x(0, 0));
+ return y;
+ },
+ y0, 0.1, 10);
+ EXPECT_NEAR(y1(0, 0), -std::log(std::exp(-1.0) - 0.1), 1e-8);
}
// Tests that integrating dx/dt = e^x works when we provide a U.
@@ -63,6 +78,20 @@
EXPECT_NEAR(y1(0, 0), RungeKuttaTimeVaryingSolution(6.0)(0, 0), 1e-3);
}
+// Now do it with a ton of sub steps.
+TEST(RungeKuttaTest, RungeKuttaTimeVaryingSteps) {
+ ::Eigen::Matrix<double, 1, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
+
+ ::Eigen::Matrix<double, 1, 1> y1 = RungeKuttaSteps(
+ [](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, 10);
+ EXPECT_NEAR(y1(0, 0), RungeKuttaTimeVaryingSolution(6.0)(0, 0), 1e-7);
+}
+
} // namespace testing
} // namespace control_loops
} // namespace frc971