Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add time-varying RKDP (#7362)

Browse Source 
This makes the ground truth for the Taylor series AQ discretization more
accurate.
This commit is contained in:
Tyler Veness
2024-11-07 23:46:52 -08:00
committed by GitHub
parent 01f85abcfe
commit 661bae568f
10 changed files with 369 additions and 174 deletions
Download Patch File Download Diff File Expand all files Collapse all files

154
wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
View File

@@ -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> Rows in y.
* @param <Cols> 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 <Rows extends Num, Cols extends Num> Matrix<Rows, Cols> rk4(
BiFunction<Double, Matrix<Rows, Cols>, Matrix<Rows, Cols>> f,
double t,
Matrix<Rows, Cols> y,
double dtSeconds) {
final var h = dtSeconds;
Matrix<Rows, Cols> k1 = f.apply(t, y);
Matrix<Rows, Cols> k2 = f.apply(t + dtSeconds * 0.5, y.plus(k1.times(h * 0.5)));
Matrix<Rows, Cols> k3 = f.apply(t + dtSeconds * 0.5, y.plus(k2.times(h * 0.5)));
Matrix<Rows, Cols> 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> Rows in y.
* @param <Cols> 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 <Rows extends Num, Cols extends Num> Matrix<Rows, Cols> rkdp(
BiFunction<Double, Matrix<Rows, Cols>, Matrix<Rows, Cols>> f,
double t,
Matrix<Rows, Cols> 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<Rows, Cols> 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;
}
}

103
wpimath/src/main/native/include/frc/system/NumericalIntegration.h
View File

@@ -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 <typename F, typename T>
T RK4(F&& f, units::second_t t, T y, units::second_t dt) {
const auto h = dt.to<double>();
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 <typename F, typename T>
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<double, kDim> 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<double, kDim> 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<double, kDim - 1> 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<double>();
// Loop until we've gotten to our desired dt
while (dtElapsed < dt.to<double>()) {
do {
// Only allow us to advance up to the dt remaining
h = std::min(h, dt.to<double>() - 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

4
wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
View File

@@ -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<N2, N2> 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<N2, N2> x) ->
contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
0.0,

47
wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
View File

@@ -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<N1, N1> 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<N1, N1> 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);
}
}

40
wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java
View File

@@ -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> Rows in y.
* @param <Cols> 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 <Rows extends Num, Cols extends Num> Matrix<Rows, Cols> rungeKuttaTimeVarying(
BiFunction<Double, Matrix<Rows, Cols>, Matrix<Rows, Cols>> f,
double t,
Matrix<Rows, Cols> y,
double dtSeconds) {
final var h = dtSeconds;
Matrix<Rows, Cols> k1 = f.apply(t, y);
Matrix<Rows, Cols> k2 = f.apply(t + dtSeconds * 0.5, y.plus(k1.times(h * 0.5)));
Matrix<Rows, Cols> k3 = f.apply(t + dtSeconds * 0.5, y.plus(k2.times(h * 0.5)));
Matrix<Rows, Cols> 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));
}
}

42
wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
View File

@@ -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<N1, N1> 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<N1, N1> 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);
}
}

37
wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
View File

@@ -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<frc::Matrixd<2, 2>(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<std::function<frc::Matrixd<2, 2>(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<frc::Matrixd<2, 2>(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<std::function<frc::Matrixd<2, 2>(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;

48
wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
View File

@@ -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);
}

35
wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
View File

@@ -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 <cmath>
#include <gtest/gtest.h>
#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);
}

33
wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
View File

@@ -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 <array>
#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 <typename F, typename T>
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