mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[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.
This commit is contained in:
Tyler Veness
@@ -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 <States> A Num representing the states of the system to integrate.
|
||||
* @param <Inputs> 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 <States extends Num, Inputs extends Num> Matrix<States, N1> rkf45(
|
||||
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
|
||||
Matrix<States, N1> x,
|
||||
Matrix<Inputs, N1> 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 <States> A Num representing the states of the system to integrate.
|
||||
* @param <Inputs> 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 <States extends Num, Inputs extends Num> Matrix<States, N1> rkf45(
|
||||
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
|
||||
Matrix<States, N1> x,
|
||||
Matrix<Inputs, N1> 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<States, N1> 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.
|
||||
|
||||
Reference in New Issue
Block a user