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] 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

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