diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java index e56d3c4a77..94be369f14 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java @@ -106,139 +106,6 @@ public final class NumericalIntegration { return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(h / 6.0)); } - /** - * Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described in - * https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method. By default, the max - * error is 1e-6. - * - * @param A Num representing the states of the system to integrate. - * @param A Num representing the inputs of the system to integrate. - * @param f The function to integrate. It must take two arguments x and u. - * @param x The initial value of x. - * @param u The value u held constant over the integration period. - * @param dtSeconds The time over which to integrate. - * @return the integration of dx/dt = f(x, u) for dt. - */ - @SuppressWarnings("overloads") - public static Matrix rkf45( - BiFunction, Matrix, Matrix> f, - Matrix x, - Matrix u, - double dtSeconds) { - return rkf45(f, x, u, dtSeconds, 1e-6); - } - - /** - * Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described in - * https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method - * - * @param A Num representing the states of the system to integrate. - * @param A Num representing the inputs of the system to integrate. - * @param f The function to integrate. It must take two arguments x and u. - * @param x The initial value of x. - * @param u The value u held constant over the integration period. - * @param dtSeconds The time over which to integrate. - * @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6. - * @return the integration of dx/dt = f(x, u) for dt. - */ - @SuppressWarnings("overloads") - public static Matrix rkf45( - BiFunction, Matrix, Matrix> f, - Matrix x, - Matrix u, - double dtSeconds, - double maxError) { - // See - // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method - // for the Butcher tableau the following arrays came from. - - // final double[5][5] - final double[][] A = { - {1.0 / 4.0}, - {3.0 / 32.0, 9.0 / 32.0}, - {1932.0 / 2197.0, -7200.0 / 2197.0, 7296.0 / 2197.0}, - {439.0 / 216.0, -8.0, 3680.0 / 513.0, -845.0 / 4104.0}, - {-8.0 / 27.0, 2.0, -3544.0 / 2565.0, 1859.0 / 4104.0, -11.0 / 40.0} - }; - - // final double[6] - final double[] b1 = { - 16.0 / 135.0, 0.0, 6656.0 / 12825.0, 28561.0 / 56430.0, -9.0 / 50.0, 2.0 / 55.0 - }; - - // final double[6] - final double[] b2 = {25.0 / 216.0, 0.0, 1408.0 / 2565.0, 2197.0 / 4104.0, -1.0 / 5.0, 0.0}; - - Matrix newX; - double truncationError; - - double dtElapsed = 0.0; - double h = dtSeconds; - - // Loop until we've gotten to our desired dt - while (dtElapsed < dtSeconds) { - do { - // Only allow us to advance up to the dt remaining - h = Math.min(h, dtSeconds - dtElapsed); - - // Notice how the derivative in the Wikipedia notation is dy/dx. - // That means their y is our x and their x is our t - var k1 = f.apply(x, u); - var k2 = f.apply(x.plus(k1.times(A[0][0]).times(h)), u); - var k3 = f.apply(x.plus(k1.times(A[1][0]).plus(k2.times(A[1][1])).times(h)), u); - var k4 = - f.apply( - x.plus(k1.times(A[2][0]).plus(k2.times(A[2][1])).plus(k3.times(A[2][2])).times(h)), - u); - var k5 = - f.apply( - x.plus( - k1.times(A[3][0]) - .plus(k2.times(A[3][1])) - .plus(k3.times(A[3][2])) - .plus(k4.times(A[3][3])) - .times(h)), - u); - var k6 = - f.apply( - x.plus( - k1.times(A[4][0]) - .plus(k2.times(A[4][1])) - .plus(k3.times(A[4][2])) - .plus(k4.times(A[4][3])) - .plus(k5.times(A[4][4])) - .times(h)), - u); - - newX = - x.plus( - k1.times(b1[0]) - .plus(k2.times(b1[1])) - .plus(k3.times(b1[2])) - .plus(k4.times(b1[3])) - .plus(k5.times(b1[4])) - .plus(k6.times(b1[5])) - .times(h)); - truncationError = - (k1.times(b1[0] - b2[0]) - .plus(k2.times(b1[1] - b2[1])) - .plus(k3.times(b1[2] - b2[2])) - .plus(k4.times(b1[3] - b2[3])) - .plus(k5.times(b1[4] - b2[4])) - .plus(k6.times(b1[5] - b2[5])) - .times(h)) - .normF(); - - h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0); - } while (truncationError > maxError); - - dtElapsed += h; - x = newX; - } - - return x; - } - /** * Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt. By default, the max * error is 1e-6. diff --git a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h index 68d047fee9..48748e2472 100644 --- a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h +++ b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h @@ -53,80 +53,6 @@ T RK4(F&& f, T x, U u, units::second_t dt) { return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4); } -/** - * Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described - * in https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method - * - * @param f The function to integrate. It must take two arguments x and - * u. - * @param x The initial value of x. - * @param u The value u held constant over the integration period. - * @param dt The time over which to integrate. - * @param maxError The maximum acceptable truncation error. Usually a small - * number like 1e-6. - */ -template -T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) { - // See - // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method - // for the Butcher tableau the following arrays came from. - constexpr int kDim = 6; - - // clang-format off - constexpr double A[kDim - 1][kDim - 1]{ - { 1.0 / 4.0}, - { 3.0 / 32.0, 9.0 / 32.0}, - {1932.0 / 2197.0, -7200.0 / 2197.0, 7296.0 / 2197.0}, - { 439.0 / 216.0, -8.0, 3680.0 / 513.0, -845.0 / 4104.0}, - { -8.0 / 27.0, 2.0, -3544.0 / 2565.0, 1859.0 / 4104.0, -11.0 / 40.0}}; - // clang-format on - - constexpr std::array b1{16.0 / 135.0, 0.0, - 6656.0 / 12825.0, 28561.0 / 56430.0, - -9.0 / 50.0, 2.0 / 55.0}; - constexpr std::array b2{ - 25.0 / 216.0, 0.0, 1408.0 / 2565.0, 2197.0 / 4104.0, -1.0 / 5.0, 0.0}; - - T newX; - double truncationError; - - double dtElapsed = 0.0; - double h = dt.value(); - - // Loop until we've gotten to our desired dt - while (dtElapsed < dt.value()) { - do { - // Only allow us to advance up to the dt remaining - h = std::min(h, dt.value() - dtElapsed); - - // Notice how the derivative in the Wikipedia notation is dy/dx. - // That means their y is our x and their x is our t - // clang-format off - T k1 = f(x, u); - T k2 = f(x + h * (A[0][0] * k1), u); - T k3 = f(x + h * (A[1][0] * k1 + A[1][1] * k2), u); - T k4 = f(x + h * (A[2][0] * k1 + A[2][1] * k2 + A[2][2] * k3), u); - T k5 = f(x + h * (A[3][0] * k1 + A[3][1] * k2 + A[3][2] * k3 + A[3][3] * k4), u); - T k6 = f(x + h * (A[4][0] * k1 + A[4][1] * k2 + A[4][2] * k3 + A[4][3] * k4 + A[4][4] * k5), u); - // clang-format on - - newX = x + h * (b1[0] * k1 + b1[1] * k2 + b1[2] * k3 + b1[3] * k4 + - b1[4] * k5 + b1[5] * k6); - truncationError = (h * ((b1[0] - b2[0]) * k1 + (b1[1] - b2[1]) * k2 + - (b1[2] - b2[2]) * k3 + (b1[3] - b2[3]) * k4 + - (b1[4] - b2[4]) * k5 + (b1[5] - b2[5]) * k6)) - .norm(); - - h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0); - } while (truncationError > maxError); - - dtElapsed += h; - x = newX; - } - - return x; -} - /** * Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt. * diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java index 6671963077..72fb5eab50 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java @@ -30,24 +30,6 @@ class NumericalIntegrationTest { assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3); } - @Test - void testExponentialRKF45() { - Matrix y0 = VecBuilder.fill(0.0); - - var y1 = - NumericalIntegration.rkf45( - (x, u) -> { - var y = new Matrix<>(Nat.N1(), Nat.N1()); - y.set(0, 0, Math.exp(x.get(0, 0))); - return y; - }, - y0, - VecBuilder.fill(0), - 0.1); - - assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3); - } - @Test void testExponentialRKDP() { Matrix y0 = VecBuilder.fill(0.0); diff --git a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp index f8e475fa17..b1793adb9b 100644 --- a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp +++ b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp @@ -30,18 +30,6 @@ TEST(NumericalIntegrationTest, ExponentialWithU) { EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3); } -// Tests that integrating dx/dt = e^x works with RKF45. -TEST(NumericalIntegrationTest, ExponentialRKF45) { - frc::Vectord<1> y0{0.0}; - - frc::Vectord<1> y1 = frc::RKF45( - [](const frc::Vectord<1>& x, const frc::Vectord<1>& u) { - return frc::Vectord<1>{std::exp(x(0))}; - }, - y0, frc::Vectord<1>{0.0}, 0.1_s); - EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3); -} - // Tests that integrating dx/dt = e^x works with RKDP TEST(NumericalIntegrationTest, ExponentialRKDP) { frc::Vectord<1> y0{0.0};