Numerical Integration Via Runge-Kutta

Preface

Numerical methods are commonly employed in controls practice to integrate dynamical systems. For systems that can be modeled by ordinary differential equations (ODEs), the 4th order Runge-Kutta Method is known for its stability, precision, and simplicity. This post summarizes its concept, formulation, and C++ implementation for future reference.

For more background information, see Runge-Kutta Methods from Wikipedia.

Problem Statement

Let's say we have a trivial 1-DOF unit mass system $$\ddot{x} = u$$ where $u$ is the control input we have access of. Initially, the system rests at $x_0, \dot{x}_0$. We can design a simple PD-controller $$ u = k_p (x_d - x) + k_d (\dot{x}_d - \dot{x}) $$ that drives the system to a desired state $x_d, \dot{x}_d$. If we want to simulate the dynamics of such a controlled system, we can use numerical methods to help us do it. However, standard numerical methods take 1st order ODEs in the state space form of $$ \label{eq:system-state-space} \dot{X} = AX + Bu $$ Converting our trivial system into the standard form, it is $$ \underbrace{\begin{bmatrix} \ddot{x} \\ \dot{x} \end{bmatrix}}_{\dot{X}}\ = \underbrace{\begin{bmatrix} 0 & 0 \\ I & 0 \end{bmatrix}}_{A} \underbrace{\begin{bmatrix} \dot{x} \\ x \end{bmatrix}}_{X} + \underbrace{\begin{bmatrix} I \\ 0 \end{bmatrix}}_{B} u $$ where, in general, for 2nd-order dynamical systems that are commonly seen in robot dynamics, $x, u\in\mathbb{R}^n$, $X, \dot{X} \in\mathbb{R}^{2n}$, $A\in\mathbb{R}^{2n\times 2n}, B\in\mathbb{R}^{2n\times n}$, $I\in\mathbb{R}^{n\times n}$, where $n$ is the original system's DOF, and $2n$ is the state space DOF.

The Runge-Kutta Method

The 4-th order Runge-Kutta Methods, a.k.a RK4, states that, for a system in the form of $$ \dot{X} = f(X, t) $$ its discretized states in the next time step can be approximated by $$ X_{n+1} = X_n + \frac{1}{6}(k_1 + 2k_2 + 2k_3 + k_4) $$ $$ t_{n+1} = t_n + h $$ where $h$ is the step size and $$ \label{eq:slopes} \begin{split} k_1 &= f(X_n, t_n) \\ k_2 &= f(X_n + h\frac{k_1}{2}, t_n + \frac{h}{2}) \\ k_3 &= f(X_n + h\frac{k_2}{2}, t_n + \frac{h}{2}) \\ k_4 &= f(X_n + h k_3, t_n + h) \end{split} $$ are the chosen slopes. It also states that

Observe that our system in \eqref{eq:system-state-space} falls into this category, thus, we can rewrite \eqref{eq:slopes} as $$ \label{eq:simplified-slopes} \begin{split} k_1 &= f(X_n) = AX_n + Bu \\ k_2 &= f(X_n + h\frac{k_1}{2}) = A(X_n + h\frac{k_1}{2}) + Bu \\ k_3 &= f(X_n + h\frac{k_2}{2}) = A(X_n + h\frac{k_2}{2}) + Bu \\ k_4 &= f(X_n + h k_3) = A(X_n + h k_3) + Bu \end{split} $$ and say that its a good approximation of \eqref{eq:slopes}, because in our discretized system, the next state is only a function of the current state and its control input at a given time $t_n$.

C++ Implementation

With \eqref{eq:simplified-slopes}, it is straightforward to implement RK4 in C++, that is

Results Verification

Assuming the system's initial and desired states are $$ \dot{x}_0 = 0, x_0 = 0 $$ $$ \dot{x}_{\text{des}} = 0, x_{\text{des}} = 1 $$ and the input is generated by a classic underdamped controller $$ u = k_p (x-x_{\text{des}}) + \xi \cdot 2 \sqrt{k_p} \cdot (\dot{x} - \dot{x}_{\text{des}}) $$ with $\xi = 0.4$. The system simulation C++ code is shown below.

Plots generated by data produced from the above code are shown below. One can observe that they match very well the underdamped behavior with chosen damping ratio. They thus validate the derivation and C++ implementation of the Runge Kutta 4th order method.