From 661bae568fdf33249c48ca0e52635478a6789d0d Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Thu, 7 Nov 2024 23:46:52 -0800 Subject: [PATCH] [wpimath] Add time-varying RKDP (#7362) This makes the ground truth for the Taylor series AQ discretization more accurate. --- .../math/system/NumericalIntegration.java | 154 ++++++++++++++++++ .../include/frc/system/NumericalIntegration.h | 103 ++++++++++++ .../first/math/system/DiscretizationTest.java | 4 +- .../math/system/NumericalIntegrationTest.java | 47 ++++++ .../math/system/RungeKuttaTimeVarying.java | 40 ----- .../system/RungeKuttaTimeVaryingTest.java | 42 ----- .../native/cpp/system/DiscretizationTest.cpp | 37 ++--- .../cpp/system/NumericalIntegrationTest.cpp | 48 +++++- .../cpp/system/RungeKuttaTimeVaryingTest.cpp | 35 ---- .../frc/system/RungeKuttaTimeVarying.h | 33 ---- 10 files changed, 369 insertions(+), 174 deletions(-) delete mode 100644 wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java delete mode 100644 wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java delete mode 100644 wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp delete mode 100644 wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h 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 feddf66e0a..1dcddea840 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 @@ -107,6 +107,32 @@ 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 4th order Runge-Kutta integration of dx/dt = f(t, y) for dt. + * + * @param Rows in y. + * @param Columns in y. + * @param f The function to integrate. It must take two arguments t and y. + * @param t The initial value of t. + * @param y The initial value of y. + * @param dtSeconds The time over which to integrate. + * @return the integration of dx/dt = f(x) for dt. + */ + public static Matrix rk4( + BiFunction, Matrix> f, + double t, + Matrix y, + double dtSeconds) { + final var h = dtSeconds; + + Matrix k1 = f.apply(t, y); + Matrix k2 = f.apply(t + dtSeconds * 0.5, y.plus(k1.times(h * 0.5))); + Matrix k3 = f.apply(t + dtSeconds * 0.5, y.plus(k2.times(h * 0.5))); + Matrix k4 = f.apply(t + dtSeconds, y.plus(k3.times(h))); + + return y.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(h / 6.0)); + } + /** * Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt. By default, the max * error is 1e-6. @@ -252,4 +278,132 @@ public final class NumericalIntegration { return x; } + + /** + * Performs adaptive Dormand-Prince integration of dx/dt = f(t, y) for dt. + * + * @param Rows in y. + * @param Columns in y. + * @param f The function to integrate. It must take two arguments t and y. + * @param t The initial value of t. + * @param y The initial value of y. + * @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 rkdp( + BiFunction, Matrix> f, + double t, + Matrix y, + double dtSeconds, + double maxError) { + // See https://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method for the + // Butcher tableau the following arrays came from. + + // final double[6][6] + final double[][] A = { + {1.0 / 5.0}, + {3.0 / 40.0, 9.0 / 40.0}, + {44.0 / 45.0, -56.0 / 15.0, 32.0 / 9.0}, + {19372.0 / 6561.0, -25360.0 / 2187.0, 64448.0 / 6561.0, -212.0 / 729.0}, + {9017.0 / 3168.0, -355.0 / 33.0, 46732.0 / 5247.0, 49.0 / 176.0, -5103.0 / 18656.0}, + {35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, 11.0 / 84.0} + }; + + // final double[7] + final double[] b1 = { + 35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, 11.0 / 84.0, 0.0 + }; + + // final double[7] + final double[] b2 = { + 5179.0 / 57600.0, + 0.0, + 7571.0 / 16695.0, + 393.0 / 640.0, + -92097.0 / 339200.0, + 187.0 / 2100.0, + 1.0 / 40.0 + }; + + // final double[6] + final double[] c = {1.0 / 5.0, 3.0 / 10.0, 4.0 / 5.0, 8.0 / 9.0, 1.0, 1.0}; + + Matrix newY; + 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); + + var k1 = f.apply(t, y); + var k2 = f.apply(t + h * c[0], y.plus(k1.times(A[0][0]).times(h))); + var k3 = f.apply(t + h * c[1], y.plus(k1.times(A[1][0]).plus(k2.times(A[1][1])).times(h))); + var k4 = + f.apply( + t + h * c[2], + y.plus(k1.times(A[2][0]).plus(k2.times(A[2][1])).plus(k3.times(A[2][2])).times(h))); + var k5 = + f.apply( + t + h * c[3], + y.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))); + var k6 = + f.apply( + t + h * c[4], + y.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))); + + // Since the final row of A and the array b1 have the same coefficients + // and k7 has no effect on newY, we can reuse the calculation. + newY = + y.plus( + k1.times(A[5][0]) + .plus(k2.times(A[5][1])) + .plus(k3.times(A[5][2])) + .plus(k4.times(A[5][3])) + .plus(k5.times(A[5][4])) + .plus(k6.times(A[5][5])) + .times(h)); + var k7 = f.apply(t + h * c[5], newY); + + 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])) + .plus(k7.times(b1[6] - b2[6])) + .times(h)) + .normF(); + + if (truncationError == 0.0) { + h = dtSeconds - dtElapsed; + } else { + h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0); + } + } while (truncationError > maxError); + + dtElapsed += h; + y = newY; + } + + return y; + } } diff --git a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h index 98b6cc3044..2fe1946f0d 100644 --- a/wpimath/src/main/native/include/frc/system/NumericalIntegration.h +++ b/wpimath/src/main/native/include/frc/system/NumericalIntegration.h @@ -51,6 +51,26 @@ 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 4th order Runge-Kutta integration of dy/dt = f(t, y) for dt. + * + * @param f The function to integrate. It must take two arguments t and y. + * @param t The initial value of t. + * @param y The initial value of y. + * @param dt The time over which to integrate. + */ +template +T RK4(F&& f, units::second_t t, T y, units::second_t dt) { + const auto h = dt.to(); + + T k1 = f(t, y); + T k2 = f(t + dt * 0.5, y + h * k1 * 0.5); + T k3 = f(t + dt * 0.5, y + h * k2 * 0.5); + T k4 = f(t + dt, y + h * k3); + + return y + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4); +} + /** * Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt. * @@ -134,4 +154,87 @@ T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) { return x; } +/** + * Performs adaptive Dormand-Prince integration of dy/dt = f(t, y) for dt. + * + * @param f The function to integrate. It must take two arguments t and + * y. + * @param t The initial value of t. + * @param y The initial value of y. + * @param dt The time over which to integrate. + * @param maxError The maximum acceptable truncation error. Usually a small + * number like 1e-6. + */ +template +T RKDP(F&& f, units::second_t t, T y, units::second_t dt, + double maxError = 1e-6) { + // See https://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method for the + // Butcher tableau the following arrays came from. + + constexpr int kDim = 7; + + // clang-format off + constexpr double A[kDim - 1][kDim - 1]{ + { 1.0 / 5.0}, + { 3.0 / 40.0, 9.0 / 40.0}, + { 44.0 / 45.0, -56.0 / 15.0, 32.0 / 9.0}, + {19372.0 / 6561.0, -25360.0 / 2187.0, 64448.0 / 6561.0, -212.0 / 729.0}, + { 9017.0 / 3168.0, -355.0 / 33.0, 46732.0 / 5247.0, 49.0 / 176.0, -5103.0 / 18656.0}, + { 35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, 11.0 / 84.0}}; + // clang-format on + + constexpr std::array b1{ + 35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, + 11.0 / 84.0, 0.0}; + constexpr std::array b2{5179.0 / 57600.0, 0.0, + 7571.0 / 16695.0, 393.0 / 640.0, + -92097.0 / 339200.0, 187.0 / 2100.0, + 1.0 / 40.0}; + + constexpr std::array c{1.0 / 5.0, 3.0 / 10.0, 4.0 / 5.0, + 8.0 / 9.0, 1.0, 1.0}; + + T newY; + double truncationError; + + double dtElapsed = 0.0; + double h = dt.to(); + + // Loop until we've gotten to our desired dt + while (dtElapsed < dt.to()) { + do { + // Only allow us to advance up to the dt remaining + h = std::min(h, dt.to() - dtElapsed); + + // clang-format off + T k1 = f(t, y); + T k2 = f(t + units::second_t{h} * c[0], y + h * (A[0][0] * k1)); + T k3 = f(t + units::second_t{h} * c[1], y + h * (A[1][0] * k1 + A[1][1] * k2)); + T k4 = f(t + units::second_t{h} * c[2], y + h * (A[2][0] * k1 + A[2][1] * k2 + A[2][2] * k3)); + T k5 = f(t + units::second_t{h} * c[3], y + h * (A[3][0] * k1 + A[3][1] * k2 + A[3][2] * k3 + A[3][3] * k4)); + T k6 = f(t + units::second_t{h} * c[4], y + h * (A[4][0] * k1 + A[4][1] * k2 + A[4][2] * k3 + A[4][3] * k4 + A[4][4] * k5)); + // clang-format on + + // Since the final row of A and the array b1 have the same coefficients + // and k7 has no effect on newY, we can reuse the calculation. + newY = y + h * (A[5][0] * k1 + A[5][1] * k2 + A[5][2] * k3 + + A[5][3] * k4 + A[5][4] * k5 + A[5][5] * k6); + T k7 = f(t + units::second_t{h} * c[5], newY); + + 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 + + (b1[6] - b2[6]) * k7)) + .norm(); + + h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0); + } while (truncationError > maxError); + + dtElapsed += h; + y = newY; + } + + return y; +} + } // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java index 830135d2b3..696a549c7e 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java @@ -72,7 +72,7 @@ class DiscretizationTest { // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ // 0 final var discQIntegrated = - RungeKuttaTimeVarying.rungeKuttaTimeVarying( + NumericalIntegration.rk4( (Double t, Matrix x) -> contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()), 0.0, @@ -104,7 +104,7 @@ class DiscretizationTest { // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ // 0 final var discQIntegrated = - RungeKuttaTimeVarying.rungeKuttaTimeVarying( + NumericalIntegration.rk4( (Double t, Matrix x) -> contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()), 0.0, 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 a0c7689cb0..db7cf5c6d1 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 @@ -6,6 +6,7 @@ package edu.wpi.first.math.system; import static org.junit.jupiter.api.Assertions.assertEquals; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -30,6 +31,28 @@ class NumericalIntegrationTest { assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3); } + // Tests RK4 with a time varying solution. From + // http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html: + // x' = x (2/(eᵗ + 1) - 1) + // + // The true (analytical) solution is: + // + // x(t) = 12eᵗ/(eᵗ + 1)² + @Test + void testRK4TimeVarying() { + final var y0 = VecBuilder.fill(12.0 * Math.exp(5.0) / Math.pow(Math.exp(5.0) + 1.0, 2.0)); + + final var y1 = + NumericalIntegration.rk4( + (Double t, Matrix y) -> + MatBuilder.fill( + Nat.N1(), Nat.N1(), y.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0)), + 5.0, + y0, + 1.0); + assertEquals(12.0 * Math.exp(6.0) / Math.pow(Math.exp(6.0) + 1.0, 2.0), y1.get(0, 0), 1e-3); + } + @Test void testZeroRKDP() { var y1 = @@ -56,4 +79,28 @@ class NumericalIntegrationTest { assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3); } + + // Tests RKDP with a time varying solution. From + // http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html: + // + // dx/dt = x(2/(eᵗ + 1) - 1) + // + // The true (analytical) solution is: + // + // x(t) = 12eᵗ/(eᵗ + 1)² + @Test + void testRKDPTimeVarying() { + final var y0 = VecBuilder.fill(12.0 * Math.exp(5.0) / Math.pow(Math.exp(5.0) + 1.0, 2.0)); + + final var y1 = + NumericalIntegration.rkdp( + (Double t, Matrix y) -> + MatBuilder.fill( + Nat.N1(), Nat.N1(), y.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0)), + 5.0, + y0, + 1.0, + 1e-12); + assertEquals(12.0 * Math.exp(6.0) / Math.pow(Math.exp(6.0) + 1.0, 2.0), y1.get(0, 0), 1e-3); + } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java deleted file mode 100644 index d0e6c126c7..0000000000 --- a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package edu.wpi.first.math.system; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Num; -import java.util.function.BiFunction; - -public final class RungeKuttaTimeVarying { - private RungeKuttaTimeVarying() { - // Utility class - } - - /** - * Performs 4th order Runge-Kutta integration of dx/dt = f(t, y) for dt. - * - * @param Rows in y. - * @param Columns in y. - * @param f The function to integrate. It must take two arguments t and y. - * @param t The initial value of t. - * @param y The initial value of y. - * @param dtSeconds The time over which to integrate. - */ - public static Matrix rungeKuttaTimeVarying( - BiFunction, Matrix> f, - double t, - Matrix y, - double dtSeconds) { - final var h = dtSeconds; - - Matrix k1 = f.apply(t, y); - Matrix k2 = f.apply(t + dtSeconds * 0.5, y.plus(k1.times(h * 0.5))); - Matrix k3 = f.apply(t + dtSeconds * 0.5, y.plus(k2.times(h * 0.5))); - Matrix k4 = f.apply(t + dtSeconds, y.plus(k3.times(h))); - - return y.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(h / 6.0)); - } -} diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java deleted file mode 100644 index bb28b73a9a..0000000000 --- a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package edu.wpi.first.math.system; - -import static org.junit.jupiter.api.Assertions.assertEquals; - -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.numbers.N1; -import org.junit.jupiter.api.Test; - -class RungeKuttaTimeVaryingTest { - private static Matrix rungeKuttaTimeVaryingSolution(double t) { - return MatBuilder.fill( - Nat.N1(), Nat.N1(), 12.0 * Math.exp(t) / Math.pow(Math.exp(t) + 1.0, 2.0)); - } - - // Tests RK4 with a time varying solution. From - // http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html: - // x' = x (2/(eᵗ + 1) - 1) - // - // The true (analytical) solution is: - // - // x(t) = 12eᵗ/(eᵗ + 1)² - @Test - void rungeKuttaTimeVaryingTest() { - final var y0 = rungeKuttaTimeVaryingSolution(5.0); - - final var y1 = - RungeKuttaTimeVarying.rungeKuttaTimeVarying( - (Double t, Matrix x) -> - MatBuilder.fill( - Nat.N1(), Nat.N1(), x.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0)), - 5.0, - y0, - 1.0); - assertEquals(rungeKuttaTimeVaryingSolution(6.0).get(0, 0), y1.get(0, 0), 1e-3); - } -} diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp index c42fcf0b9a..055ee49736 100644 --- a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp +++ b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp @@ -10,7 +10,6 @@ #include "frc/EigenCore.h" #include "frc/system/Discretization.h" #include "frc/system/NumericalIntegration.h" -#include "frc/system/RungeKuttaTimeVarying.h" // Check that for a simple second-order system that we can easily analyze // analytically, @@ -62,15 +61,15 @@ TEST(DiscretizationTest, DiscretizeSlowModelAQ) { // T // Q_d ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ // 0 - frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying< - std::function(units::second_t, - const frc::Matrixd<2, 2>&)>, - frc::Matrixd<2, 2>>( - [&](units::second_t t, const frc::Matrixd<2, 2>&) { - return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ * - (contA.transpose() * t.value()).exp()); - }, - 0_s, frc::Matrixd<2, 2>::Zero(), dt); + frc::Matrixd<2, 2> discQIntegrated = + frc::RKDP(units::second_t, + const frc::Matrixd<2, 2>&)>, + frc::Matrixd<2, 2>>( + [&](units::second_t t, const frc::Matrixd<2, 2>&) { + return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ * + (contA.transpose() * t.value()).exp()); + }, + 0_s, frc::Matrixd<2, 2>::Zero(), dt); frc::Matrixd<2, 2> discA; frc::Matrixd<2, 2> discQ; @@ -94,15 +93,15 @@ TEST(DiscretizationTest, DiscretizeFastModelAQ) { // T // Q_d = ∫ e^(Aτ) Q e^(Aᵀτ) dτ // 0 - frc::Matrixd<2, 2> discQIntegrated = frc::RungeKuttaTimeVarying< - std::function(units::second_t, - const frc::Matrixd<2, 2>&)>, - frc::Matrixd<2, 2>>( - [&](units::second_t t, const frc::Matrixd<2, 2>&) { - return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ * - (contA.transpose() * t.value()).exp()); - }, - 0_s, frc::Matrixd<2, 2>::Zero(), dt); + frc::Matrixd<2, 2> discQIntegrated = + frc::RKDP(units::second_t, + const frc::Matrixd<2, 2>&)>, + frc::Matrixd<2, 2>>( + [&](units::second_t t, const frc::Matrixd<2, 2>&) { + return frc::Matrixd<2, 2>((contA * t.value()).exp() * contQ * + (contA.transpose() * t.value()).exp()); + }, + 0_s, frc::Matrixd<2, 2>::Zero(), dt); frc::Matrixd<2, 2> discA; frc::Matrixd<2, 2> discQ; diff --git a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp index 1c73195ca7..05ec0a4826 100644 --- a/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp +++ b/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp @@ -9,7 +9,7 @@ #include "frc/EigenCore.h" #include "frc/system/NumericalIntegration.h" -// Tests that integrating dx/dt = e^x works. +// Test that integrating dx/dt = eˣ works TEST(NumericalIntegrationTest, Exponential) { frc::Vectord<1> y0{0.0}; @@ -19,7 +19,7 @@ TEST(NumericalIntegrationTest, Exponential) { EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3); } -// Tests that integrating dx/dt = e^x works when we provide a U. +// Test that integrating dx/dt = eˣ works when we provide a u TEST(NumericalIntegrationTest, ExponentialWithU) { frc::Vectord<1> y0{0.0}; @@ -31,6 +31,27 @@ TEST(NumericalIntegrationTest, ExponentialWithU) { EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3); } +// Tests RK4 with a time varying solution. From +// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html: +// +// dx/dt = x (2 / (eᵗ + 1) - 1) +// +// The true (analytical) solution is: +// +// x(t) = 12eᵗ/(eᵗ + 1)² +TEST(NumericalIntegrationTest, RK4TimeVarying) { + frc::Vectord<1> y0{12.0 * std::exp(5.0) / std::pow(std::exp(5.0) + 1.0, 2.0)}; + + frc::Vectord<1> y1 = frc::RK4( + [](units::second_t t, const frc::Vectord<1>& x) { + return frc::Vectord<1>{x(0) * + (2.0 / (std::exp(t.value()) + 1.0) - 1.0)}; + }, + 5_s, y0, 1_s); + EXPECT_NEAR(y1(0), 12.0 * std::exp(6.0) / std::pow(std::exp(6.0) + 1.0, 2.0), + 1e-3); +} + // Tests that integrating dx/dt = 0 works with RKDP TEST(NumericalIntegrationTest, ZeroRKDP) { frc::Vectord<1> y1 = frc::RKDP( @@ -41,7 +62,7 @@ TEST(NumericalIntegrationTest, ZeroRKDP) { EXPECT_NEAR(y1(0), 0.0, 1e-3); } -// Tests that integrating dx/dt = e^x works with RKDP +// Tests that integrating dx/dt = eˣ works with RKDP TEST(NumericalIntegrationTest, ExponentialRKDP) { frc::Vectord<1> y0{0.0}; @@ -52,3 +73,24 @@ TEST(NumericalIntegrationTest, ExponentialRKDP) { y0, frc::Vectord<1>{0.0}, 0.1_s); EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3); } + +// Tests RKDP with a time varying solution. From +// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html: +// +// dx/dt = x(2/(eᵗ + 1) - 1) +// +// The true (analytical) solution is: +// +// x(t) = 12eᵗ/(eᵗ + 1)² +TEST(NumericalIntegrationTest, RKDPTimeVarying) { + frc::Vectord<1> y0{12.0 * std::exp(5.0) / std::pow(std::exp(5.0) + 1.0, 2.0)}; + + frc::Vectord<1> y1 = frc::RKDP( + [](units::second_t t, const frc::Vectord<1>& x) { + return frc::Vectord<1>{x(0) * + (2.0 / (std::exp(t.value()) + 1.0) - 1.0)}; + }, + 5_s, y0, 1_s, 1e-12); + EXPECT_NEAR(y1(0), 12.0 * std::exp(6.0) / std::pow(std::exp(6.0) + 1.0, 2.0), + 1e-3); +} diff --git a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp deleted file mode 100644 index e502e95f28..0000000000 --- a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp +++ /dev/null @@ -1,35 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include - -#include - -#include "frc/EigenCore.h" -#include "frc/system/RungeKuttaTimeVarying.h" - -namespace { -frc::Vectord<1> RungeKuttaTimeVaryingSolution(double t) { - return frc::Vectord<1>{12.0 * std::exp(t) / std::pow(std::exp(t) + 1.0, 2.0)}; -} -} // namespace - -// Tests RK4 with a time varying solution. From -// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html: -// x' = x (2/(eᵗ + 1) - 1) -// -// The true (analytical) solution is: -// -// x(t) = 12eᵗ/((eᵗ + 1)²) -TEST(RungeKuttaTimeVaryingTest, RungeKuttaTimeVarying) { - frc::Vectord<1> y0 = RungeKuttaTimeVaryingSolution(5.0); - - frc::Vectord<1> y1 = frc::RungeKuttaTimeVarying( - [](units::second_t t, const frc::Vectord<1>& x) { - return frc::Vectord<1>{x(0) * - (2.0 / (std::exp(t.value()) + 1.0) - 1.0)}; - }, - 5_s, y0, 1_s); - EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3); -} diff --git a/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h b/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h deleted file mode 100644 index 6273532b65..0000000000 --- a/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#pragma once - -#include - -#include "units/time.h" - -namespace frc { - -/** - * Performs 4th order Runge-Kutta integration of dy/dt = f(t, y) for dt. - * - * @param f The function to integrate. It must take two arguments t and y. - * @param t The initial value of t. - * @param y The initial value of y. - * @param dt The time over which to integrate. - */ -template -T RungeKuttaTimeVarying(F&& f, units::second_t t, T y, units::second_t dt) { - const auto h = dt.value(); - - T k1 = f(t, y); - T k2 = f(t + dt * 0.5, y + h * k1 * 0.5); - T k3 = f(t + dt * 0.5, y + h * k2 * 0.5); - T k4 = f(t + dt, y + h * k3); - - return y + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4); -} - -} // namespace frc