From 2121bd5fb8b7916f1475b74ef105fbf223f5f875 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 27 Dec 2022 17:29:59 -0800 Subject: [PATCH] [wpimath] Remove RKF45 (#4870) RKDP is strictly better in terms of accuracy per unit of work. We used RKF45 for sim physics in the 2021 season, but we transitioned to RKDP before the 2022 season. --- .../math/system/NumericalIntegration.java | 133 ------------------ .../include/frc/system/NumericalIntegration.h | 74 ---------- .../math/system/NumericalIntegrationTest.java | 18 --- .../cpp/system/NumericalIntegrationTest.cpp | 12 -- 4 files changed, 237 deletions(-) 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};