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.