mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-24 01:31:46 +00:00
[wpimath] Implement Dormand-Prince integration method (#3476)
Also refactored RKF45 implementation to match the new style, which is easier to read. The tests were switched from RKF45 to RKDP since it's more accurate.
This commit is contained in:
Tyler Veness
@@ -85,7 +85,7 @@ void ElevatorSim::SetInputVoltage(units::volt_t voltage) {
|
||||
Eigen::Matrix<double, 2, 1> ElevatorSim::UpdateX(
|
||||
const Eigen::Matrix<double, 2, 1>& currentXhat,
|
||||
const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) {
|
||||
auto updatedXhat = RKF45(
|
||||
auto updatedXhat = RKDP(
|
||||
[&](const Eigen::Matrix<double, 2, 1>& x,
|
||||
const Eigen::Matrix<double, 1, 1>& u_)
|
||||
-> Eigen::Matrix<double, 2, 1> {
|
||||
|
||||
@@ -88,7 +88,7 @@ Eigen::Matrix<double, 2, 1> SingleJointedArmSim::UpdateX(
|
||||
// We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I *
|
||||
// std::cos(theta)]]
|
||||
|
||||
Eigen::Matrix<double, 2, 1> updatedXhat = RKF45(
|
||||
Eigen::Matrix<double, 2, 1> updatedXhat = RKDP(
|
||||
[&](const auto& x, const auto& u) -> Eigen::Matrix<double, 2, 1> {
|
||||
Eigen::Matrix<double, 2, 1> xdot = m_plant.A() * x + m_plant.B() * u;
|
||||
|
||||
|
||||
@@ -80,7 +80,7 @@ TEST(ElevatorSim, Stability) {
|
||||
|
||||
Eigen::Matrix<double, 2, 1> x1 = frc::MakeMatrix<2, 1>(0.0, 0.0);
|
||||
for (size_t i = 0; i < 50; i++) {
|
||||
x1 = frc::RKF45(
|
||||
x1 = frc::RKDP(
|
||||
[&](Eigen::Matrix<double, 2, 1> x,
|
||||
Eigen::Matrix<double, 1, 1> u) -> Eigen::Matrix<double, 2, 1> {
|
||||
return system.A() * x + system.B() * u;
|
||||
|
||||
@@ -221,7 +221,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
|
||||
protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
|
||||
// Calculate updated x-hat from Runge-Kutta.
|
||||
var updatedXhat =
|
||||
NumericalIntegration.rkf45(
|
||||
NumericalIntegration.rkdp(
|
||||
(x, u_) ->
|
||||
(m_plant.getA().times(x))
|
||||
.plus(m_plant.getB().times(u_))
|
||||
|
||||
@@ -282,7 +282,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
|
||||
// We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I *
|
||||
// cos(theta)]]
|
||||
Matrix<N2, N1> updatedXhat =
|
||||
NumericalIntegration.rkf45(
|
||||
NumericalIntegration.rkdp(
|
||||
(Matrix<N2, N1> x, Matrix<N1, N1> u_) -> {
|
||||
Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(u_));
|
||||
if (m_simulateGravity) {
|
||||
|
||||
@@ -5,12 +5,8 @@
|
||||
package edu.wpi.first.math.system;
|
||||
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.Nat;
|
||||
import edu.wpi.first.math.Num;
|
||||
import edu.wpi.first.math.Pair;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N5;
|
||||
import edu.wpi.first.math.numbers.N6;
|
||||
import java.util.function.BiFunction;
|
||||
import java.util.function.DoubleFunction;
|
||||
import java.util.function.Function;
|
||||
@@ -145,142 +141,234 @@ public final class NumericalIntegration {
|
||||
Matrix<Inputs, N1> u,
|
||||
double dtSeconds,
|
||||
double maxError) {
|
||||
double dtElapsed = 0;
|
||||
double previousH = dtSeconds;
|
||||
// See
|
||||
// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
|
||||
// for the Butcher tableau the following arrays came from.
|
||||
|
||||
// This is used for time-varying integration
|
||||
// // final double[5]
|
||||
// final double[] A = {
|
||||
// 1.0 / 4.0, 3.0 / 8.0, 12.0 / 13.0, 1.0, 1.0 / 2.0};
|
||||
|
||||
// final double[5][5]
|
||||
final double[][] B = {
|
||||
{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[] C1 = {
|
||||
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[] C2 = {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) {
|
||||
// RKF45 will give us an updated x and a dt back.
|
||||
// We use the new dt (h) as the initial dt for our next loop
|
||||
var ret = rkf45Impl(f, x, u, previousH, maxError, dtSeconds - dtElapsed);
|
||||
dtElapsed += ret.getSecond();
|
||||
previousH = ret.getSecond();
|
||||
x = ret.getFirst();
|
||||
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).times(h);
|
||||
var k2 = f.apply(x.plus(k1.times(B[0][0])), u).times(h);
|
||||
var k3 = f.apply(x.plus(k1.times(B[1][0])).plus(k2.times(B[1][1])), u).times(h);
|
||||
var k4 =
|
||||
f.apply(x.plus(k1.times(B[2][0])).plus(k2.times(B[2][1])).plus(k3.times(B[2][2])), u)
|
||||
.times(h);
|
||||
var k5 =
|
||||
f.apply(
|
||||
x.plus(k1.times(B[3][0]))
|
||||
.plus(k2.times(B[3][1]))
|
||||
.plus(k3.times(B[3][2]))
|
||||
.plus(k4.times(B[3][3])),
|
||||
u)
|
||||
.times(h);
|
||||
var k6 =
|
||||
f.apply(
|
||||
x.plus(k1.times(B[4][0]))
|
||||
.plus(k2.times(B[4][1]))
|
||||
.plus(k3.times(B[4][2]))
|
||||
.plus(k4.times(B[4][3]))
|
||||
.plus(k5.times(B[4][4])),
|
||||
u)
|
||||
.times(h);
|
||||
|
||||
newX =
|
||||
x.plus(k1.times(C1[0]))
|
||||
.plus(k2.times(C1[1]))
|
||||
.plus(k3.times(C1[2]))
|
||||
.plus(k4.times(C1[3]))
|
||||
.plus(k5.times(C1[4]))
|
||||
.plus(k6.times(C1[5]));
|
||||
truncationError =
|
||||
(k1.times(C1[0] - C2[0])
|
||||
.plus(k2.times(C1[1] - C2[1]))
|
||||
.plus(k3.times(C1[2] - C2[2]))
|
||||
.plus(k4.times(C1[3] - C2[3]))
|
||||
.plus(k5.times(C1[4] - C2[4]))
|
||||
.plus(k6.times(C1[5] - C2[5])))
|
||||
.normF();
|
||||
|
||||
h = 0.9 * h * Math.pow(maxError / truncationError, 1.0 / 5.0);
|
||||
} while (truncationError > maxError);
|
||||
|
||||
dtElapsed += h;
|
||||
x = newX;
|
||||
}
|
||||
|
||||
return x;
|
||||
}
|
||||
|
||||
static final double[] ch = {47 / 450.0, 0, 12 / 25.0, 32 / 225.0, 1 / 30.0, 6 / 25.0};
|
||||
static final double[] ct = {-1 / 150.0, 0, 3 / 100.0, -16 / 75.0, -1 / 20.0, 6 / 25.0};
|
||||
static final Matrix<N6, N5> Bs =
|
||||
Matrix.mat(Nat.N6(), Nat.N5())
|
||||
.fill(
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
2 / 9.0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
1 / 12.0,
|
||||
1 / 4.0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
69 / 128.0,
|
||||
-243 / 128.0,
|
||||
135 / 64.0,
|
||||
0,
|
||||
0,
|
||||
-17 / 12.0,
|
||||
27 / 4.0,
|
||||
-27 / 5.0,
|
||||
16 / 15.0,
|
||||
0,
|
||||
65 / 432.0,
|
||||
-5 / 16.0,
|
||||
13 / 16.0,
|
||||
4 / 27.0,
|
||||
5 / 144.0);
|
||||
|
||||
/**
|
||||
* Implements one loop of RKF45. This takes an initial state, dt guess, and max truncation error,
|
||||
* and returns a new x and the dt over which that x was updated. This should be called until there
|
||||
* is no dt remaining.
|
||||
* Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt. By default, the max
|
||||
* error is 1e-6.
|
||||
*
|
||||
* @param <States> Num representing the states of the system to integrate.
|
||||
* @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 initialH The initial dt guess. This is refined to clamp truncation error to the
|
||||
* specified max.
|
||||
* @param maxTruncationError The max truncation error acceptable. Usually a small number like
|
||||
* 1e-6.
|
||||
* @param dtRemaining How much time is left to integrate over. Used to clamp h.
|
||||
* @param dtSeconds The time over which to integrate.
|
||||
* @return the integration of dx/dt = f(x, u) for dt.
|
||||
*/
|
||||
@SuppressWarnings("MethodTypeParameterName")
|
||||
private static <States extends Num, Inputs extends Num>
|
||||
Pair<Matrix<States, N1>, Double> rkf45Impl(
|
||||
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
|
||||
Matrix<States, N1> x,
|
||||
Matrix<Inputs, N1> u,
|
||||
double initialH,
|
||||
double maxTruncationError,
|
||||
double dtRemaining) {
|
||||
double truncationErr;
|
||||
double h = initialH;
|
||||
public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkdp(
|
||||
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
|
||||
Matrix<States, N1> x,
|
||||
Matrix<Inputs, N1> u,
|
||||
double dtSeconds) {
|
||||
return rkdp(f, x, u, dtSeconds, 1e-6);
|
||||
}
|
||||
|
||||
/**
|
||||
* Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt.
|
||||
*
|
||||
* @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("MethodTypeParameterName")
|
||||
public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkdp(
|
||||
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/Dormand%E2%80%93Prince_method for the
|
||||
// Butcher tableau the following arrays came from.
|
||||
|
||||
// This is used for time-varying integration
|
||||
// // final double[6]
|
||||
// final double[] A = {
|
||||
// 1.0 / 5.0, 3.0 / 10.0, 4.0 / 5.0, 8.0 / 9.0, 1.0, 1.0};
|
||||
|
||||
// final double[6][6]
|
||||
final double[][] B = {
|
||||
{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[] C1 = {
|
||||
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[] C2 = {
|
||||
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
|
||||
};
|
||||
|
||||
Matrix<States, N1> newX;
|
||||
double truncationError;
|
||||
|
||||
do {
|
||||
// only allow us to advance up to the dt remaining
|
||||
h = Math.min(h, dtRemaining);
|
||||
double dtElapsed = 0.0;
|
||||
double h = dtSeconds;
|
||||
|
||||
// Notice how the derivative in the Wikipedia notation is dy/dx.
|
||||
// That means their y is our x and their x is our t
|
||||
Matrix<States, N1> k1 = f.apply(x, u).times(h);
|
||||
Matrix<States, N1> k2 = f.apply(x.plus(k1.times(Bs.get(1, 0))), u).times(h);
|
||||
Matrix<States, N1> k3 =
|
||||
f.apply(x.plus(k1.times(Bs.get(2, 0))).plus(k2.times(Bs.get(2, 1))), u).times(h);
|
||||
Matrix<States, N1> k4 =
|
||||
f.apply(
|
||||
x.plus(k1.times(Bs.get(3, 0)))
|
||||
.plus(k2.times(Bs.get(3, 1)))
|
||||
.plus(k3.times(Bs.get(3, 2))),
|
||||
u)
|
||||
.times(h);
|
||||
Matrix<States, N1> k5 =
|
||||
f.apply(
|
||||
x.plus(k1.times(Bs.get(4, 0)))
|
||||
.plus(k2.times(Bs.get(4, 1)))
|
||||
.plus(k3.times(Bs.get(4, 2)))
|
||||
.plus(k4.times(Bs.get(4, 3))),
|
||||
u)
|
||||
.times(h);
|
||||
Matrix<States, N1> k6 =
|
||||
f.apply(
|
||||
x.plus(k1.times(Bs.get(5, 0)))
|
||||
.plus(k2.times(Bs.get(5, 1)))
|
||||
.plus(k3.times(Bs.get(5, 2)))
|
||||
.plus(k4.times(Bs.get(5, 3)))
|
||||
.plus(k5.times(Bs.get(5, 4))),
|
||||
u)
|
||||
.times(h);
|
||||
// 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);
|
||||
|
||||
newX =
|
||||
x.plus(k1.times(ch[0]))
|
||||
.plus(k2.times(ch[1]))
|
||||
.plus(k3.times(ch[2]))
|
||||
.plus(k4.times(ch[3]))
|
||||
.plus(k5.times(ch[4]))
|
||||
.plus(k6.times(ch[5]));
|
||||
var k1 = f.apply(x, u).times(h);
|
||||
var k2 = f.apply(x.plus(k1.times(B[0][0])), u).times(h);
|
||||
var k3 = f.apply(x.plus(k1.times(B[1][0])).plus(k2.times(B[1][1])), u).times(h);
|
||||
var k4 =
|
||||
f.apply(x.plus(k1.times(B[2][0])).plus(k2.times(B[2][1])).plus(k3.times(B[2][2])), u)
|
||||
.times(h);
|
||||
var k5 =
|
||||
f.apply(
|
||||
x.plus(k1.times(B[3][0]))
|
||||
.plus(k2.times(B[3][1]))
|
||||
.plus(k3.times(B[3][2]))
|
||||
.plus(k4.times(B[3][3])),
|
||||
u)
|
||||
.times(h);
|
||||
var k6 =
|
||||
f.apply(
|
||||
x.plus(k1.times(B[4][0]))
|
||||
.plus(k2.times(B[4][1]))
|
||||
.plus(k3.times(B[4][2]))
|
||||
.plus(k4.times(B[4][3]))
|
||||
.plus(k5.times(B[4][4])),
|
||||
u)
|
||||
.times(h);
|
||||
|
||||
truncationErr =
|
||||
k1.times(ct[0])
|
||||
.plus(k2.times(ct[1]))
|
||||
.plus(k3.times(ct[2]))
|
||||
.plus(k4.times(ct[3]))
|
||||
.plus(k5.times(ct[4]))
|
||||
.plus(k6.times(ct[5]))
|
||||
.normF();
|
||||
// Since the final row of B and the array C1 have the same coefficients
|
||||
// and k7 has no effect on newX, we can reuse the calculation.
|
||||
newX =
|
||||
x.plus(k1.times(B[5][0]))
|
||||
.plus(k2.times(B[5][1]))
|
||||
.plus(k3.times(B[5][2]))
|
||||
.plus(k4.times(B[5][3]))
|
||||
.plus(k5.times(B[5][4]))
|
||||
.plus(k6.times(B[5][5]));
|
||||
var k7 = f.apply(newX, u).times(h);
|
||||
|
||||
h = 0.9 * h * Math.pow(maxTruncationError / truncationErr, 1 / 5.0);
|
||||
} while (truncationErr > maxTruncationError);
|
||||
truncationError =
|
||||
(k1.times(C1[0] - C2[0])
|
||||
.plus(k2.times(C1[1] - C2[1]))
|
||||
.plus(k3.times(C1[2] - C2[2]))
|
||||
.plus(k4.times(C1[3] - C2[3]))
|
||||
.plus(k5.times(C1[4] - C2[4]))
|
||||
.plus(k6.times(C1[5] - C2[5]))
|
||||
.plus(k7.times(C1[6] - C2[6])))
|
||||
.normF();
|
||||
|
||||
// Return the new x, and the timestep
|
||||
return Pair.of(newX, h);
|
||||
h = 0.9 * h * Math.pow(maxError / truncationError, 1.0 / 5.0);
|
||||
} while (truncationError > maxError);
|
||||
|
||||
dtElapsed += h;
|
||||
x = newX;
|
||||
}
|
||||
|
||||
return x;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -7,6 +7,7 @@
|
||||
#include <frc/StateSpaceUtil.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "units/time.h"
|
||||
@@ -67,56 +68,6 @@ T RungeKuttaTimeVarying(F&& f, T x, units::second_t t, units::second_t dt) {
|
||||
return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
|
||||
}
|
||||
|
||||
template <typename F, typename T, typename U>
|
||||
T RKF45Impl(F&& f, T x, U u, double& h, double maxTruncationError,
|
||||
double dtRemaining) {
|
||||
double truncationErr;
|
||||
T newX;
|
||||
|
||||
static double ch[] = {47 / 450.0, 0, 12 / 25.0,
|
||||
32 / 225.0, 1 / 30.0, 6 / 25.0};
|
||||
static double ct[] = {-1 / 150.0, 0, 3 / 100.0,
|
||||
-16 / 75.0, -1 / 20.0, 6 / 25.0};
|
||||
static Eigen::Matrix<double, 6, 5> Bs = frc::MakeMatrix<6, 5>(
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 2 / 9.0, 0.0, 0.0, 0.0, 0.0, 1 / 12.0, 1 / 4.0,
|
||||
0.0, 0.0, 0.0, 69 / 128.0, -243 / 128.0, 135 / 64.0, 0.0, 0.0, -17 / 12.0,
|
||||
27 / 4.0, -27 / 5.0, 16 / 15.0, 0.0, 65 / 432.0, -5 / 16.0, 13 / 16.0,
|
||||
4 / 27.0, 5 / 144.0);
|
||||
|
||||
do {
|
||||
// only allow us to advance up to the dt remaining
|
||||
h = std::min(h, dtRemaining);
|
||||
|
||||
// Notice how the derivative in the Wikipedia notation is dy/dx.
|
||||
// That means their y is our x and their x is our t
|
||||
T k1 = f(x, u) * h;
|
||||
T k2 = f(x + (k1 * Bs(1, 0)), u) * h;
|
||||
T k3 = f(x + (k1 * (Bs(2, 0))) + (k2 * (Bs(2, 1))), u) * h;
|
||||
T k4 =
|
||||
f(x + (k1 * (Bs(3, 0))) + (k2 * (Bs(3, 1))) + (k3 * (Bs(3, 2))), u) * h;
|
||||
T k5 = f(x + (k1 * (Bs(4, 0))) + (k2 * (Bs(4, 1))) + (k3 * (Bs(4, 2))) +
|
||||
(k4 * (Bs(4, 3))),
|
||||
u) *
|
||||
h;
|
||||
T k6 = f(x + (k1 * (Bs(5, 0))) + (k2 * (Bs(5, 1))) + (k3 * (Bs(5, 2))) +
|
||||
(k4 * (Bs(5, 3))) + (k5 * (Bs(5, 4))),
|
||||
u) *
|
||||
h;
|
||||
|
||||
newX = x + (k1 * (ch[0])) + (k2 * (ch[1])) + (k3 * (ch[2])) +
|
||||
(k4 * (ch[3])) + (k5 * (ch[4])) + (k6 * (ch[5]));
|
||||
|
||||
truncationErr = (k1 * (ct[0]) + (k2 * (ct[1])) + (k3 * (ct[2])) +
|
||||
(k4 * (ct[3])) + (k5 * (ct[4])) + (k6 * (ct[5])))
|
||||
.norm();
|
||||
|
||||
h = 0.9 * h * std::pow(maxTruncationError / truncationErr, 1 / 5.);
|
||||
} while (truncationErr > maxTruncationError);
|
||||
|
||||
// Return the new x. Dt is changed by reference
|
||||
return newX;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
@@ -131,19 +82,150 @@ T RKF45Impl(F&& f, T x, U u, double& h, double maxTruncationError,
|
||||
*/
|
||||
template <typename F, typename T, typename U>
|
||||
T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
|
||||
double dtElapsed = 0;
|
||||
double dtSeconds = dt.to<double>();
|
||||
double previousH = dt.to<double>();
|
||||
// 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;
|
||||
|
||||
// This is used for time-varying integration
|
||||
// constexpr std::array<double, kDim - 1> A{
|
||||
// 1.0 / 4.0, 3.0 / 8.0, 12.0 / 13.0, 1.0, 1.0 / 2.0};
|
||||
|
||||
// clang-format off
|
||||
constexpr double B[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<double, kDim> C1{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<double, kDim> C2{
|
||||
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.to<double>();
|
||||
|
||||
// Loop until we've gotten to our desired dt
|
||||
while (dtElapsed < dtSeconds) {
|
||||
// RKF45 will give us an updated x and a dt back.
|
||||
// We use the new dt (h) as the initial dt for our next loop
|
||||
// previousH is changed by-reference
|
||||
T ret = RKF45Impl(f, x, u, previousH, maxError, dtSeconds - dtElapsed);
|
||||
dtElapsed += previousH;
|
||||
x = ret;
|
||||
while (dtElapsed < dt.to<double>()) {
|
||||
do {
|
||||
// Only allow us to advance up to the dt remaining
|
||||
h = std::min(h, dt.to<double>() - 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) * h;
|
||||
T k2 = f(x + k1 * B[0][0], u) * h;
|
||||
T k3 = f(x + k1 * B[1][0] + k2 * B[1][1], u) * h;
|
||||
T k4 = f(x + k1 * B[2][0] + k2 * B[2][1] + k3 * B[2][2], u) * h;
|
||||
T k5 = f(x + k1 * B[3][0] + k2 * B[3][1] + k3 * B[3][2] + k4 * B[3][3], u) * h;
|
||||
T k6 = f(x + k1 * B[4][0] + k2 * B[4][1] + k3 * B[4][2] + k4 * B[4][3] + k5 * B[4][4], u) * h;
|
||||
// clang-format on
|
||||
|
||||
newX = x + k1 * C1[0] + k2 * C1[1] + k3 * C1[2] + k4 * C1[3] +
|
||||
k5 * C1[4] + k6 * C1[5];
|
||||
truncationError =
|
||||
(k1 * (C1[0] - C2[0]) + k2 * (C1[1] - C2[1]) + k3 * (C1[2] - C2[2]) +
|
||||
k4 * (C1[3] - C2[3]) + k5 * (C1[4] - C2[4]) + k6 * (C1[5] - C2[5]))
|
||||
.norm();
|
||||
|
||||
h = 0.9 * h * 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.
|
||||
*
|
||||
* @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 <typename F, typename T, typename U>
|
||||
T RKDP(F&& f, T x, U u, 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;
|
||||
|
||||
// This is used for time-varying integration
|
||||
// constexpr std::array<double, kDim - 1> A{
|
||||
// 1.0 / 5.0, 3.0 / 10.0, 4.0 / 5.0, 8.0 / 9.0, 1.0, 1.0};
|
||||
|
||||
// clang-format off
|
||||
constexpr double B[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> C1{
|
||||
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> C2{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};
|
||||
|
||||
T newX;
|
||||
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(x, u) * h;
|
||||
T k2 = f(x + k1 * B[0][0], u) * h;
|
||||
T k3 = f(x + k1 * B[1][0] + k2 * B[1][1], u) * h;
|
||||
T k4 = f(x + k1 * B[2][0] + k2 * B[2][1] + k3 * B[2][2], u) * h;
|
||||
T k5 = f(x + k1 * B[3][0] + k2 * B[3][1] + k3 * B[3][2] + k4 * B[3][3], u) * h;
|
||||
T k6 = f(x + k1 * B[4][0] + k2 * B[4][1] + k3 * B[4][2] + k4 * B[4][3] + k5 * B[4][4], u) * h;
|
||||
// clang-format on
|
||||
|
||||
// Since the final row of B and the array C1 have the same coefficients
|
||||
// and k7 has no effect on newX, we can reuse the calculation.
|
||||
newX = x + k1 * B[5][0] + k2 * B[5][1] + k3 * B[5][2] + k4 * B[5][3] +
|
||||
k5 * B[5][4] + k6 * B[5][5];
|
||||
T k7 = f(newX, u) * h;
|
||||
|
||||
truncationError =
|
||||
(k1 * (C1[0] - C2[0]) + k2 * (C1[1] - C2[1]) + k3 * (C1[2] - C2[2]) +
|
||||
k4 * (C1[3] - C2[3]) + k5 * (C1[4] - C2[4]) + k6 * (C1[5] - C2[5]) +
|
||||
k7 * (C1[6] - C2[6]))
|
||||
.norm();
|
||||
|
||||
h = 0.9 * h * std::pow(maxError / truncationError, 1.0 / 5.0);
|
||||
} while (truncationError > maxError);
|
||||
|
||||
dtElapsed += h;
|
||||
x = newX;
|
||||
}
|
||||
|
||||
return x;
|
||||
}
|
||||
|
||||
|
||||
@@ -34,7 +34,7 @@ public class NumericalIntegrationTest {
|
||||
|
||||
@Test
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName"})
|
||||
public void testExponentialAdaptive() {
|
||||
public void testExponentialRKF45() {
|
||||
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
|
||||
|
||||
//noinspection SuspiciousNameCombination
|
||||
@@ -51,4 +51,24 @@ public class NumericalIntegrationTest {
|
||||
|
||||
assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
|
||||
}
|
||||
|
||||
@Test
|
||||
@SuppressWarnings({"ParameterName", "LocalVariableName"})
|
||||
public void testExponentialRKDP() {
|
||||
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
|
||||
|
||||
//noinspection SuspiciousNameCombination
|
||||
var y1 =
|
||||
NumericalIntegration.rkdp(
|
||||
(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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -38,8 +38,8 @@ TEST(NumericalIntegrationTest, ExponentialWithU) {
|
||||
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(NumericalIntegrationTest, ExponentialWithUAdaptive) {
|
||||
// Tests that integrating dx/dt = e^x works with RKF45.
|
||||
TEST(NumericalIntegrationTest, ExponentialRKF45) {
|
||||
Eigen::Matrix<double, 1, 1> y0;
|
||||
y0(0) = 0.0;
|
||||
|
||||
@@ -53,6 +53,21 @@ TEST(NumericalIntegrationTest, ExponentialWithUAdaptive) {
|
||||
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) {
|
||||
Eigen::Matrix<double, 1, 1> y0;
|
||||
y0(0) = 0.0;
|
||||
|
||||
Eigen::Matrix<double, 1, 1> y1 = frc::RKDP(
|
||||
[](Eigen::Matrix<double, 1, 1> x, Eigen::Matrix<double, 1, 1> u) {
|
||||
Eigen::Matrix<double, 1, 1> y;
|
||||
y(0) = std::exp(x(0));
|
||||
return y;
|
||||
},
|
||||
y0, (Eigen::Matrix<double, 1, 1>() << 0.0).finished(), 0.1_s);
|
||||
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
|
||||
}
|
||||
|
||||
namespace {
|
||||
Eigen::Matrix<double, 1, 1> RungeKuttaTimeVaryingSolution(double t) {
|
||||
return (Eigen::Matrix<double, 1, 1>()
|
||||
|
||||
Reference in New Issue
Block a user