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] Clean up NumericalIntegration and add Discretization tests (#3489)

Browse Source 
* Rename Butcher tableau sections in NumericalIntegration such that
  top-left is c, top-right is A, and bottom-right is b
* Move edu.wpi.first.math.Discretization to
  edu.wpi.first.math.system.Discretization
* Sort Java Discretization to match C++ function order
* Add tests for Java Discretization
  * Required adding Runge-Kutta time-varying impl to tests
* Move C++ Runge-Kutta time-varying impl to tests only
  * Users don't need it
This commit is contained in:
Tyler Veness
2021-07-25 07:42:59 -07:00
committed by GitHub
parent bfc209b120
commit 50af74c38f
20 changed files with 641 additions and 310 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -23,12 +23,14 @@ namespace frc {
*/
template <typename F, typename T>
T RK4(F&& f, T x, units::second_t dt) {
const auto halfDt = 0.5 * dt;
const auto h = dt.to<double>();
T k1 = f(x);
T k2 = f(x + k1 * halfDt.to<double>());
T k3 = f(x + k2 * halfDt.to<double>());
T k4 = f(x + k3 * dt.to<double>());
return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
T k2 = f(x + h * 0.5 * k1);
T k3 = f(x + h * 0.5 * k2);
T k4 = f(x + h * k3);
return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
/**
@@ -41,31 +43,14 @@ T RK4(F&& f, T x, units::second_t dt) {
*/
template <typename F, typename T, typename U>
T RK4(F&& f, T x, U u, units::second_t dt) {
const auto halfDt = 0.5 * dt;
const auto h = dt.to<double>();
T k1 = f(x, u);
T k2 = f(x + k1 * halfDt.to<double>(), u);
T k3 = f(x + k2 * halfDt.to<double>(), u);
T k4 = f(x + k3 * dt.to<double>(), u);
return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
T k2 = f(x + h * 0.5 * k1, u);
T k3 = f(x + h * 0.5 * k2, u);
T k4 = f(x + h * k3, u);
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(t, x) for dt.
*
* @param f The function to integrate. It must take two arguments x and t.
* @param x The initial value of x.
* @param t The initial value of t.
* @param dt The time over which to integrate.
*/
template <typename F, typename T>
T RungeKuttaTimeVarying(F&& f, T x, units::second_t t, units::second_t dt) {
const auto halfDt = 0.5 * dt;
T k1 = f(t, x);
T k2 = f(t + halfDt, x + k1 / 2.0);
T k3 = f(t + halfDt, x + k2 / 2.0);
T k4 = f(t + dt, x + k3);
return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
/**
@@ -87,12 +72,8 @@ T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
// 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]{
constexpr double A[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},
@@ -100,10 +81,10 @@ T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
{ -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,
constexpr std::array<double, kDim> b1{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{
constexpr std::array<double, kDim> b2{
25.0 / 216.0, 0.0, 1408.0 / 2565.0, 2197.0 / 4104.0, -1.0 / 5.0, 0.0};
T newX;
@@ -121,22 +102,22 @@ T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
// 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;
T k1 = f(x, u);
T k2 = f(x + h * (A[0][0] * k1), u);
T k3 = f(x + h * (A[1][0] * k1 + A[1][1] * k2), u);
T k4 = f(x + h * (A[2][0] * k1 + A[2][1] * k2 + A[2][2] * k3), u);
T k5 = f(x + h * (A[3][0] * k1 + A[3][1] * k2 + A[3][2] * k3 + A[3][3] * k4), u);
T k6 = f(x + h * (A[4][0] * k1 + A[4][1] * k2 + A[4][2] * k3 + A[4][3] * k4 + A[4][4] * k5), u);
// 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();
newX = x + h * (b1[0] * k1 + b1[1] * k2 + b1[2] * k3 + b1[3] * k4 +
b1[4] * k5 + b1[5] * k6);
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))
.norm();
h = 0.9 * h * std::pow(maxError / truncationError, 1.0 / 5.0);
h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
} while (truncationError > maxError);
dtElapsed += h;
@@ -164,12 +145,8 @@ T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
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]{
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},
@@ -178,10 +155,10 @@ T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
{ 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{
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> C2{5179.0 / 57600.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};
@@ -199,27 +176,27 @@ T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
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;
T k1 = f(x, u);
T k2 = f(x + h * (A[0][0] * k1), u);
T k3 = f(x + h * (A[1][0] * k1 + A[1][1] * k2), u);
T k4 = f(x + h * (A[2][0] * k1 + A[2][1] * k2 + A[2][2] * k3), u);
T k5 = f(x + h * (A[3][0] * k1 + A[3][1] * k2 + A[3][2] * k3 + A[3][3] * k4), u);
T k6 = f(x + h * (A[4][0] * k1 + A[4][1] * k2 + A[4][2] * k3 + A[4][3] * k4 + A[4][4] * k5), u);
// clang-format on
// Since the final row of B and the array C1 have the same coefficients
// Since the final row of A and the array b1 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;
newX = x + 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(newX, u);
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();
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 * h * std::pow(maxError / truncationError, 1.0 / 5.0);
h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
} while (truncationError > maxError);
dtElapsed += h;