|
| 1 | +# Runge-Kutta ODE Integrators (RK4 + Dormand-Prince) |
| 2 | + |
| 3 | +## Overview & Motivation |
| 4 | + |
| 5 | +Ordinary differential equations of the form $\dot{x} = f(x, u, t)$ arise throughout embedded control and dynamics — propagating plant models for prediction, running model-based observers, or performing hardware-in-the-loop simulation on the device itself. |
| 6 | + |
| 7 | +**RK4** (the classical fourth-order Runge-Kutta method) solves this problem with a fixed step size, producing deterministic, constant-work-per-tick execution. It is the natural choice for hard-real-time control loops. |
| 8 | + |
| 9 | +**Dormand-Prince RK45** is an *embedded* pair that computes both a 4th- and a 5th-order estimate from the same seven slope evaluations, then uses their difference as a cheap local error gauge. The step-size controller shrinks the step when the estimated error is too large and grows it when the solution is smooth — adapting accuracy to computational budget without user intervention. It is suited for offline simulation or hardware-in-the-loop testing where timing determinism is less critical than accuracy. |
| 10 | + |
| 11 | +## Mathematical Theory |
| 12 | + |
| 13 | +### Problem Statement |
| 14 | + |
| 15 | +Given $\dot{x} = f(x, u, t)$ with $x(t_0) = x_0$, advance the state by one step from $t$ to $t + h$. |
| 16 | + |
| 17 | +### RK4 — Classic Four-Stage Formula |
| 18 | + |
| 19 | +$$ |
| 20 | +k_1 = f(x_n, u, t_n) |
| 21 | +$$ |
| 22 | +$$ |
| 23 | +k_2 = f\!\left(x_n + \tfrac{h}{2}k_1,\ u,\ t_n + \tfrac{h}{2}\right) |
| 24 | +$$ |
| 25 | +$$ |
| 26 | +k_3 = f\!\left(x_n + \tfrac{h}{2}k_2,\ u,\ t_n + \tfrac{h}{2}\right) |
| 27 | +$$ |
| 28 | +$$ |
| 29 | +k_4 = f\!\left(x_n + h\,k_3,\ u,\ t_n + h\right) |
| 30 | +$$ |
| 31 | +$$ |
| 32 | +x_{n+1} = x_n + \frac{h}{6}\left(k_1 + 2k_2 + 2k_3 + k_4\right) |
| 33 | +$$ |
| 34 | + |
| 35 | +The weights $(1, 2, 2, 1)/6$ are derived by matching the Taylor series of the exact solution through fourth order. The local truncation error is $O(h^5)$; the global error is $O(h^4)$. |
| 36 | + |
| 37 | +### Dormand-Prince RK45 — Butcher Tableau |
| 38 | + |
| 39 | +Dormand and Prince (1980) selected a 7-stage Butcher tableau whose 5th-order propagator $y_5$ and 4th-order embedded propagator $y_4$ share stages $k_1, \ldots, k_6$, with $k_7 = f(y_5, u, t+h)$ added only for the 4th-order correction and for FSAL reuse. |
| 40 | + |
| 41 | +The **5th-order** solution used to advance the state: |
| 42 | + |
| 43 | +$$ |
| 44 | +y_5 = x_n + h\left(\frac{35}{384}k_1 + \frac{500}{1113}k_3 - \frac{125}{192}k_4 + \frac{2187}{6784}k_5 + \frac{11}{84}k_6\right) |
| 45 | +$$ |
| 46 | + |
| 47 | +The **4th-order** embedded solution used only for error estimation: |
| 48 | + |
| 49 | +$$ |
| 50 | +y_4 = x_n + h\left(\frac{5179}{57600}k_1 + \frac{7571}{16695}k_3 - \frac{393}{640}k_4 + \frac{92097}{339200}k_5 + \frac{187}{2100}k_6 + \frac{1}{40}k_7\right) |
| 51 | +$$ |
| 52 | + |
| 53 | +### Error Norm and Step-Size Control |
| 54 | + |
| 55 | +The mixed absolute/relative weighted RMS norm over all $n_s$ state components: |
| 56 | + |
| 57 | +$$ |
| 58 | +\text{err} = \sqrt{\frac{1}{n_s} \sum_{i=1}^{n_s} \left(\frac{(y_5 - y_4)_i}{\text{atol} + \text{rtol}\,|x_i|}\right)^2} |
| 59 | +$$ |
| 60 | + |
| 61 | +A step is *accepted* when $\text{err} \le 1$. The next step size is: |
| 62 | + |
| 63 | +$$ |
| 64 | +h_{\text{new}} = h \cdot \text{clamp}\!\left(0.9 \cdot \text{err}^{-1/5},\ 0.2,\ 5\right) |
| 65 | +$$ |
| 66 | + |
| 67 | +clamped further to $[h_{\min}, h_{\max}]$. |
| 68 | + |
| 69 | +### FSAL Property |
| 70 | + |
| 71 | +The 7th stage $k_7 = f(y_5, u, t+h)$ equals the first stage of the next accepted step. Caching it reduces each accepted step from 7 to 6 function evaluations. |
| 72 | + |
| 73 | +## Complexity Analysis |
| 74 | + |
| 75 | +| Integrator | RHS evaluations per accepted step | State memory | |
| 76 | +|----------------|-----------------------------------|--------------| |
| 77 | +| RK4 (fixed) | 4 (always) | $O(n_s)$ | |
| 78 | +| Dormand-Prince | 6 (with FSAL), 7 on first step | $O(n_s)$ | |
| 79 | + |
| 80 | +All intermediate stage vectors are stack-allocated. No heap is used. The cost of one step is $O(s \cdot n_s)$ where $s$ is the stage count plus the cost of evaluating $f$. |
| 81 | + |
| 82 | +## Step-by-Step Walkthrough |
| 83 | + |
| 84 | +**Scalar decay** $\dot{x} = -x$, $x(0) = 1$, exact solution $x(t) = e^{-t}$, $h = 0.1$: |
| 85 | + |
| 86 | +| Stage | Formula | Value | |
| 87 | +|-------|-------------------------------------------|-------------------| |
| 88 | +| $k_1$ | $f(1, 0) = -1$ | $-1$ | |
| 89 | +| $k_2$ | $f(1 - 0.05, 0.05) = -0.95$ | $-0.95$ | |
| 90 | +| $k_3$ | $f(1 - 0.0475, 0.05) = -0.9525$ | $-0.9525$ | |
| 91 | +| $k_4$ | $f(1 - 0.09525, 0.1) = -0.90475$ | $-0.90475$ | |
| 92 | +| $x_1$ | $1 + (0.1/6)(-1 - 1.9 - 1.905 - 0.90475)$ | $\approx 0.90484$ | |
| 93 | + |
| 94 | +Exact: $e^{-0.1} \approx 0.90484$. Agreement to six significant figures — consistent with $O(h^5)$ local error. |
| 95 | + |
| 96 | +## Pitfalls & Edge Cases |
| 97 | + |
| 98 | +- **Stiff systems.** Explicit RK methods are unstable for stiff problems when $h|\lambda| \gtrsim 2.8$ (RK4 stability boundary for a scalar complex eigenvalue). A stiff plant requires either an implicit integrator or a very small step size. |
| 99 | +- **Step-size floor.** When DP45 shrinks the step below $h_{\min}$, the step is clamped and forced accepted regardless of error — useful to avoid infinite rejection loops on a discontinuity, but the solution at that point is degraded. |
| 100 | +- **Zero or negative step.** Guard $h > 0$ before calling `Step`; a zero step produces an unchanged state but wastes evaluations. |
| 101 | +- **FSAL invalidation.** After a rejected step, the cached $k_7$ is discarded, and the next attempt recomputes $k_1$ from scratch (7 evaluations instead of 6). |
| 102 | +- **Float precision.** The Dormand-Prince Butcher coefficients have denominators up to 339200; in single precision the accumulated rounding can erode one to two digits of accuracy compared with double. Use tighter tolerances or shorter integration windows. |
| 103 | + |
| 104 | +## Variants & Generalizations |
| 105 | + |
| 106 | +| Variant | Key Difference | |
| 107 | +|---------------------------|-------------------------------------------------------------------------------------------------------------| |
| 108 | +| **Euler (1st order)** | One stage; $O(h)$ global error; useful only for rough prototyping | |
| 109 | +| **RK4 (this)** | Four stages; $O(h^4)$ global error; standard fixed-step workhorse | |
| 110 | +| **Dormand-Prince (this)** | Seven stages; $O(h^5)$ propagator with built-in $O(h^4)$ error estimate | |
| 111 | +| **Bogacki-Shampine RK23** | Three-stage embedded pair; lower overhead for mildly stiff or smooth problems | |
| 112 | +| **Adams-Bashforth** | Multi-step; reuses past evaluations; efficient but requires startup phase | |
| 113 | +| **Implicit RK / SDIRK** | Solves a nonlinear system at each stage; suitable for stiff problems at the cost of a linear solve per step | |
| 114 | + |
| 115 | +## Applications |
| 116 | + |
| 117 | +- **Dynamics propagation** — advance plant models (`dynamics/`) forward in time for prediction horizons in MPC or trajectory planning. |
| 118 | +- **Model-based state estimation** — propagate the process model in an extended Kalman filter between measurement updates. |
| 119 | +- **Hardware-in-the-loop simulation** — embed a physics model on the device for closed-loop testing without external simulation hardware. |
| 120 | +- **Trajectory generation** — integrate a kinematic model to produce smooth, time-parameterized reference trajectories. |
| 121 | + |
| 122 | +## Connections to Other Algorithms |
| 123 | + |
| 124 | +```mermaid |
| 125 | +graph LR |
| 126 | + RK["RK4 / Dormand-Prince"] |
| 127 | + DYN["dynamics/ (Euler-Lagrange, Newton-Euler)"] |
| 128 | + EKF["Extended Kalman Filter"] |
| 129 | + MPC["MPC Controller"] |
| 130 | + C2D["ContinuousToDiscrete"] |
| 131 | + DYN --> RK |
| 132 | + RK --> EKF |
| 133 | + RK --> MPC |
| 134 | + C2D -.->|"exact linear alternative"| RK |
| 135 | +``` |
| 136 | + |
| 137 | +| Algorithm | Relationship | |
| 138 | +|------------------------|---------------------------------------------------------------------------------------------| |
| 139 | +| `dynamics/` models | Provide the right-hand side $f(x, u, t)$ that RK integrates | |
| 140 | +| Extended Kalman Filter | Uses RK to propagate the state prediction step between measurements | |
| 141 | +| MPC Controller | Uses RK to simulate the plant over a prediction horizon | |
| 142 | +| ContinuousToDiscrete | Exact matrix-exponential discretization — an alternative for linear, time-invariant systems | |
| 143 | + |
| 144 | +## References & Further Reading |
| 145 | + |
| 146 | +- Dormand, J.R. and Prince, P.J., "A family of embedded Runge-Kutta formulae," *Journal of Computational and Applied Mathematics*, 6(1):19–26, 1980. |
| 147 | +- Hairer, E., Nørsett, S.P., and Wanner, G., *Solving Ordinary Differential Equations I: Nonstiff Problems*, 2nd ed., Springer, 1993 — Chapters II.4–II.6. |
| 148 | +- Press, W.H. et al., *Numerical Recipes in C++*, 3rd ed., Cambridge University Press, 2007 — Section 17.2. |
![github-actions[bot]](https://avatars.githubusercontent.com/in/15368?v=4&size=40){kind=avatar}
0 commit comments