rk4

Functions

template<typename State, typename DerivFunc>
void rk4_step(DerivFunc &&f, const State &x, double t, double dt, State &x_out, int quat_idx = -1)

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.

• quat_idx – Starting index of a unit quaternion in the state, or a negative value (default) to disable on-manifold normalization. When non-negative this realizes “convention B”: the input is normalized first (m = norm(x)), every RK4 substage quaternion is renormalized before the derivative evaluation, and the output is normalized. This makes the integrated discrete map exactly the one that rk4_jacobians and rk4_hessians linearize. Callers without a quaternion (or that normalize externally) leave this at the default and get the classic un-normalized RK4 step, byte-for-byte as before.

inline Eigen::MatrixXd stateNormJacobian(const Eigen::Ref<const Eigen::VectorXd> &x, int nx, int quat_idx = 3)

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.

Compute the state normalization Jacobian.

Returns an nx×nx identity matrix with the quaternion block (4×4) replaced by (I/||q|| - q*q^T/||q||^3). For unit quaternions this is approximately (I - q*q^T), the tangent-space projector on S³.

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)

• x – Full state vector (quaternion at indices quat_idx..quat_idx+3)

• nx – State dimension

• quat_idx – Starting index of the quaternion in the state vector

Template Parameters:

DynamicsJacFunc – Function type for computing continuous Jacobians

inline std::vector<Eigen::MatrixXd> stateNormHessian(const Eigen::Ref<const Eigen::VectorXd> &x, int nx, int quat_idx = 3)

Second derivative of the state normalization map.

For the normalization map norm_a(q) = q_a / ‖q‖ (a = 0..3), this returns the full Hessian as nx slices, one per output component. Only the four quaternion-output slices (indices quat_idx+a) are non-zero, and within those the non-zero block is the quat×quat block: ∂²norm_a/∂q_m∂q_j = 3 q_a q_m q_j / r⁵ − (δ_am q_j + δ_aj q_m + δ_mj q_a) / r³, r = ‖q‖. Every other state component is an identity passthrough, so its second derivative is zero. This is the “Planning with Attitude” eq.-15 curvature term of the dynamics retraction (the term PR #71 was missing).

Parameters:

• x – Full state vector (quaternion at quat_idx..quat_idx+3).

• nx – State dimension.

• quat_idx – Starting index of the quaternion.

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, int quat_idx = 3)

template<typename DynHessFunc>
void rk4_hessians(DynHessFunc &&dynamics_hess, const Eigen::Ref<const Eigen::VectorXd> &x, const Eigen::Ref<const Eigen::VectorXd> &u, double t, double dt, std::vector<Eigen::MatrixXd> &F_xx, std::vector<Eigen::MatrixXd> &F_ux, std::vector<Eigen::MatrixXd> &F_uu, int quat_idx = 3)

Compute discrete-time dynamics Hessians for an RK4 integrator step.

Given a continuous-time dynamics callback that supplies Jacobians (A_c = ∂f/∂x, B_c = ∂f/∂u) and Hessians (f_xx = ∂²f/∂x², f_ux = ∂²f/∂u∂x, f_uu = ∂²f/∂u²), this composes them through all four RK4 stages to produce the discrete-time Hessians of x_{k+1} w.r.t. x_k and u_k: F_xx[l] = ∂²x_{k+1}_l / ∂x_k∂x_k (nx slices, each nx×nx) F_ux[l] = ∂²x_{k+1}_l / ∂u_k∂x_k (nx slices, each nu×nx) F_uu[l] = ∂²x_{k+1}_l / ∂u_k∂u_k (nx slices, each nu×nu)

Convention B (consistent with rk4_jacobians/rk4_step): the discrete map is F(x) = norm(Φ(m)), m = norm(x), where Φ is RK4 with per-substage and output renormalization applied to the already-normalized base m. This routine builds Φ_x/Φ_xx/Φ_ux/Φ_uu w.r.t. m and then chains the input normalization (dm/dx = N0, d²m/dx² = d²N0). Unlike earlier drafts, the SECOND derivative of the normalization map IS modeled — both at every substage/output (the Jᵀ·d²N·J curvature term) and at the input (Σ_a Φ_x[l,a]·d²N0[a], the “Planning with Attitude” eq.-15 term). This makes the discrete second-order model exact in the quaternion block.

The callback signature matches rk4_jacobians extended with three Hessian output cubes (std::vector<Eigen::MatrixXd>, nx slices each).

Template Parameters:

DynHessFunc – callback type.

Parameters:

• dynamics_hess – Function returning continuous-time Jacobians + Hessians.

• x – Current state (nx).

• u – Current control (nu).

• t – Current time.

• dt – Step size.

• F_xx – [out] discrete-time ∂²x_{k+1}/∂x² (nx slices, each nx×nx).

• F_ux – [out] discrete-time ∂²x_{k+1}/∂u∂x (nx slices, each nu×nx).

• F_uu – [out] discrete-time ∂²x_{k+1}/∂u² (nx slices, each nu×nu).

namespace saltro_rk4_detail

Functions

inline std::vector<Eigen::MatrixXd> matTimesCube(const Eigen::MatrixXd &M, const std::vector<Eigen::MatrixXd> &cube)

inline std::vector<Eigen::MatrixXd> cubeTimesMat(const std::vector<Eigen::MatrixXd> &cube, const Eigen::MatrixXd &M)

inline std::vector<Eigen::MatrixXd> matTimesCubeT(const Eigen::MatrixXd &M, const std::vector<Eigen::MatrixXd> &cube)

inline std::vector<Eigen::MatrixXd> matOverCube(const Eigen::MatrixXd &A, const std::vector<Eigen::MatrixXd> &cube)

inline std::vector<Eigen::MatrixXd> cubeAdd(const std::vector<Eigen::MatrixXd> &a, const std::vector<Eigen::MatrixXd> &b)

inline std::vector<Eigen::MatrixXd> cubeScale(double s, const std::vector<Eigen::MatrixXd> &cube)

inline void addNormHessianTerm(std::vector<Eigen::MatrixXd> &Hxx, std::vector<Eigen::MatrixXd> &Hux, std::vector<Eigen::MatrixXd> &Huu, const std::vector<Eigen::MatrixXd> &d2N, const Eigen::MatrixXd &Jx, const Eigen::MatrixXd &Ju, int quat_idx)