rk4#

Functions

template<typename State, typename DerivFunc>
void rk4_step(DerivFunc &&f, const State &x, double t, double dt, State &x_out)#

Perform a single step of fourth-order Runge-Kutta (RK4) integration.

This function performs one time step of the explicit fourth-order Runge-Kutta method for solving ordinary differential equations of the form:

\[ \frac{d\mathbf{x}}{dt} = \mathbf{f}(t, \mathbf{x}) \]

The RK4 method is one of the most widely used numerical integration schemes due to its excellent balance of accuracy and efficiency. It uses four weighted function evaluations per step:

\[\begin{split} \mathbf{k}_1 = \mathbf{f}(t, \mathbf{x}) \\ \mathbf{k}_2 = \mathbf{f}\left(t + \frac{\Delta t}{2}, \mathbf{x} + \frac{\Delta t}{2}\mathbf{k}_1\right) \\ \mathbf{k}_3 = \mathbf{f}\left(t + \frac{\Delta t}{2}, \mathbf{x} + \frac{\Delta t}{2}\mathbf{k}_2\right) \\ \mathbf{k}_4 = \mathbf{f}(t + \Delta t, \mathbf{x} + \Delta t \, \mathbf{k}_3) \\ \mathbf{x}_{\text{out}} = \mathbf{x} + \frac{\Delta t}{6}(\mathbf{k}_1 + 2\mathbf{k}_2 + 2\mathbf{k}_3 + \mathbf{k}_4) \end{split}\]

RK4 has a local truncation error of \(O(\Delta t^5)\) and is fourth-order accurate, making it suitable for high-precision orbit propagation and other dynamical systems.

Template Parameters:

  • State – Type of the state vector (must support scalar multiplication and addition).

  • DerivFunc – Type of the derivative function/functor.

Parameters:

  • f – Derivative function with signature: void f(double t, const State& x, State& dxdt). Computes the derivative at time t and state x, storing the result in dxdt.

  • x – Current state vector.

  • t – Current time.

  • dt – Time step size.

  • x_out – Output state vector after one RK4 step.

template<typename DynamicsJacFunc>
void rk4_jacobians(DynamicsJacFunc &&dynamics_jac, const Eigen::Ref<const Eigen::VectorXd> &x, const Eigen::Ref<const Eigen::VectorXd> &u, double t, double dt, Eigen::Ref<Eigen::MatrixXd> A_discrete, Eigen::Ref<Eigen::MatrixXd> B_discrete)#

Compute exact discrete-time Jacobians for RK4 integration.

This function computes the first-order sensitivities of the RK4 integrator, providing exact discrete-time Jacobians A and B such that:

\[ \mathbf{x}_{k+1} \approx \mathbf{x}_k + \mathbf{A}(\mathbf{x}_k - \mathbf{x}_{\text{ref}}) + \mathbf{B}(\mathbf{u}_k - \mathbf{u}_{\text{ref}}) \]
where A = ∂x_{k+1}/∂x_k and B = ∂x_{k+1}/∂u_k are evaluated at the linearization point.

The computation uses the chain rule through all four RK4 stages:

  • ∂k_1/∂x = f_x(t, x, u)

  • ∂k_2/∂x = f_x(…) · (I + dt/2 · ∂k_1/∂x)

  • ∂k_3/∂x = f_x(…) · (I + dt/2 · ∂k_2/∂x)

  • ∂k_4/∂x = f_x(…) · (I + dt · ∂k_3/∂x)

  • A = I + (dt/6)(∂k_1/∂x + 2·∂k_2/∂x + 2·∂k_3/∂x + ∂k_4/∂x)

This is significantly more accurate than first-order Euler discretization for iLQR, as it ensures the backward pass linearization exactly matches the forward pass integration.

Parameters:

  • dynamics_jac – Function that computes continuous-time Jacobians f_x and f_u. Signature: void(double t, const VecX& x, const VecX& u, MatXX& A_c, MatXU& B_c)

  • x – Current state vector (n × 1)

  • u – Current control vector (m × 1)

  • t – Current time

  • dt – Time step size

  • A_discrete – Output: discrete-time state Jacobian ∂x_{k+1}/∂x_k (n × n)

  • B_discrete – Output: discrete-time control Jacobian ∂x_{k+1}/∂u_k (n × m)

Template Parameters:

DynamicsJacFunc – Function type for computing continuous Jacobians