Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Implement Dormand-Prince integration method (#3476)

Browse Source 
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
2021-07-11 10:42:33 -04:00
committed by GitHub
parent 9c2723391b
commit 1daadb812f
9 changed files with 392 additions and 187 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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