WPILibC++ 2024.3.2
NumericalIntegration.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <algorithm>
8#include <array>
9#include <cmath>
10
11#include "units/time.h"
12
13namespace frc {
14
15/**
16 * Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
17 *
18 * @param f The function to integrate. It must take one argument x.
19 * @param x The initial value of x.
20 * @param dt The time over which to integrate.
21 */
22template <typename F, typename T>
23T RK4(F&& f, T x, units::second_t dt) {
24 const auto h = dt.value();
25
26 T k1 = f(x);
27 T k2 = f(x + h * 0.5 * k1);
28 T k3 = f(x + h * 0.5 * k2);
29 T k4 = f(x + h * k3);
30
31 return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
32}
33
34/**
35 * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
36 *
37 * @param f The function to integrate. It must take two arguments x and u.
38 * @param x The initial value of x.
39 * @param u The value u held constant over the integration period.
40 * @param dt The time over which to integrate.
41 */
42template <typename F, typename T, typename U>
43T RK4(F&& f, T x, U u, units::second_t dt) {
44 const auto h = dt.value();
45
46 T k1 = f(x, u);
47 T k2 = f(x + h * 0.5 * k1, u);
48 T k3 = f(x + h * 0.5 * k2, u);
49 T k4 = f(x + h * k3, u);
50
51 return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
52}
53
54/**
55 * Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt.
56 *
57 * @param f The function to integrate. It must take two arguments x and
58 * u.
59 * @param x The initial value of x.
60 * @param u The value u held constant over the integration period.
61 * @param dt The time over which to integrate.
62 * @param maxError The maximum acceptable truncation error. Usually a small
63 * number like 1e-6.
64 */
65template <typename F, typename T, typename U>
66T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
67 // See https://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method for the
68 // Butcher tableau the following arrays came from.
69
70 constexpr int kDim = 7;
71
72 // clang-format off
73 constexpr double A[kDim - 1][kDim - 1]{
74 { 1.0 / 5.0},
75 { 3.0 / 40.0, 9.0 / 40.0},
76 { 44.0 / 45.0, -56.0 / 15.0, 32.0 / 9.0},
77 {19372.0 / 6561.0, -25360.0 / 2187.0, 64448.0 / 6561.0, -212.0 / 729.0},
78 { 9017.0 / 3168.0, -355.0 / 33.0, 46732.0 / 5247.0, 49.0 / 176.0, -5103.0 / 18656.0},
79 { 35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, 11.0 / 84.0}};
80 // clang-format on
81
82 constexpr std::array<double, kDim> b1{
83 35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0,
84 11.0 / 84.0, 0.0};
85 constexpr std::array<double, kDim> b2{5179.0 / 57600.0, 0.0,
86 7571.0 / 16695.0, 393.0 / 640.0,
87 -92097.0 / 339200.0, 187.0 / 2100.0,
88 1.0 / 40.0};
89
90 T newX;
91 double truncationError;
92
93 double dtElapsed = 0.0;
94 double h = dt.value();
95
96 // Loop until we've gotten to our desired dt
97 while (dtElapsed < dt.value()) {
98 do {
99 // Only allow us to advance up to the dt remaining
100 h = (std::min)(h, dt.value() - dtElapsed);
101
102 // clang-format off
103 T k1 = f(x, u);
104 T k2 = f(x + h * (A[0][0] * k1), u);
105 T k3 = f(x + h * (A[1][0] * k1 + A[1][1] * k2), u);
106 T k4 = f(x + h * (A[2][0] * k1 + A[2][1] * k2 + A[2][2] * k3), u);
107 T k5 = f(x + h * (A[3][0] * k1 + A[3][1] * k2 + A[3][2] * k3 + A[3][3] * k4), u);
108 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);
109 // clang-format on
110
111 // Since the final row of A and the array b1 have the same coefficients
112 // and k7 has no effect on newX, we can reuse the calculation.
113 newX = x + h * (A[5][0] * k1 + A[5][1] * k2 + A[5][2] * k3 +
114 A[5][3] * k4 + A[5][4] * k5 + A[5][5] * k6);
115 T k7 = f(newX, u);
116
117 truncationError = (h * ((b1[0] - b2[0]) * k1 + (b1[1] - b2[1]) * k2 +
118 (b1[2] - b2[2]) * k3 + (b1[3] - b2[3]) * k4 +
119 (b1[4] - b2[4]) * k5 + (b1[5] - b2[5]) * k6 +
120 (b1[6] - b2[6]) * k7))
121 .norm();
122
123 if (truncationError == 0.0) {
124 h = dt.value() - dtElapsed;
125 } else {
126 h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
127 }
128 } while (truncationError > maxError);
129
130 dtElapsed += h;
131 x = newX;
132 }
133
134 return x;
135}
136
137} // namespace frc
Definition: AprilTagPoseEstimator.h:15
T RK4(F &&f, T x, units::second_t dt)
Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
Definition: NumericalIntegration.h:23
T RKDP(F &&f, T x, U u, units::second_t dt, double maxError=1e-6)
Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt.
Definition: NumericalIntegration.h:66
static constexpr const unit_t< compound_unit< energy::joule, time::seconds > > h(6.626070040e-34)
Planck constant.
static constexpr const unit_t< compound_unit< charge::coulomb, inverse< substance::mol > > > F(N_A *e)