State Derivative Calculation for RK4 Integration in Peng

Peng computes the state derivative for RK4 integration in the Quadrotor::state_derivative method defined in src/lib.rs, which calculates instantaneous rates of change for position, velocity, quaternion, and angular velocity using quadrotor physics including thrust, gravity, drag, and gyroscopic torques.

The Peng quadrotor simulation library (makeecat/peng) implements high-fidelity physics using a Runge-Kutta-4 (RK4) integrator to advance the system state through time. At the core of this process lies the state derivative calculation for RK4 integration, a function that translates the current 13-dimensional state vector and control inputs into time derivatives. This calculation is performed in src/lib.rs within the Quadrotor struct, enabling precise numerical integration of the quadrotor's dynamics.

The 13-Dimensional State Vector Structure

Peng represents the quadrotor's complete configuration using a 13-element f32 array with the following layout:

• Indices 0–2: Position coordinates (x, y, z) in the world frame
• Indices 3–5: Linear velocity components (vx, vy, vz)
• Indices 6–9: Quaternion orientation (x, y, z, w) representing body-to-world rotation
• Indices 10–12: Angular velocity vector (ωx, ωy, ωz) in the body frame

The state_derivative function receives this vector along with control inputs and returns a 13-element array containing the time derivatives of each component.

Core Implementation in src/lib.rs

The state derivative calculation is implemented in Quadrotor::state_derivative (lines 21–51 of src/lib.rs). The function accepts three parameters:

• state: &[f32] – The current 13-element state slice
• control_thrust: f32 – Total thrust force generated by the propellers
• control_torque: &Vector3<f32> – 3D torque command from the attitude controller

Extracting Position, Velocity, and Orientation

The function first extracts sub-states from the flat array using nalgebra types (lines 27–35):

let velocity = Vector3::from_column_slice(&state[3..6]);
let orientation = UnitQuaternion::from_quaternion(Quaternion::new(
    state[9], state[6], state[7], state[8],
));
let angular_velocity = Vector3::from_column_slice(&state[10..13]);

Here, velocity represents the linear velocity dp/dt, orientation is the unit quaternion q, and angular_velocity is the body-rate vector ω.

Computing Quaternion Derivatives

The derivative of the orientation quaternion follows the kinematic relation (\dot{q} = \frac{1}{2} q \otimes \Omega), where (\Omega) is the pure quaternion formed from angular velocity (lines 31–34):

let omega_quat = Quaternion::new(0.0, state[10], state[11], state[12]);
let q_dot = orientation.into_inner() * omega_quat * 0.5;

This operation converts the angular velocity into the instantaneous rate of change of the orientation quaternion.

Calculating Linear Forces and Acceleration

The linear acceleration derives from three forces acting on the body (lines 37–41):

• Gravity: (\mathbf{g} = [0, 0, -m g])
• Aerodynamic drag: (\mathbf{d} = -c_d |\mathbf{v}| \mathbf{v})
• Thrust in world frame: (\mathbf{T} = R(q) \cdot [0, 0, \text{control_thrust}])

let gravity_force = Vector3::new(0.0, 0.0, -self.mass * self.gravity);
let drag_force = -self.drag_coefficient * velocity.norm() * velocity;
let thrust_world = orientation * Vector3::new(0.0, 0.0, control_thrust);
self.acceleration = (thrust_world + gravity_force + drag_force) / self.mass;

The resulting self.acceleration represents dv/dt, the derivative of linear velocity.

Solving Angular Dynamics

Rotational acceleration accounts for control torques and gyroscopic effects using the rigid body equations of motion (lines 42–45):

[ \dot{\omega} = I^{-1} \left( \tau_{\text{control}} - \omega \times (I \omega) \right) ]

let inertia_angular_velocity = self.inertia_matrix * angular_velocity;
let gyroscopic_torque = angular_velocity.cross(&inertia_angular_velocity);
let angular_acceleration = self.inertia_matrix_inv * 
    (control_torque - gyroscopic_torque);

Here, inertia_matrix is the 3×3 inertia tensor, and gyroscopic_torque represents the cross-product term (\omega \times (I\omega)).

Assembling the 13-Element Derivative Vector

Finally, the function constructs the complete derivative array (lines 46–51):

let mut derivative = [0.0; 13];
derivative[0..3].copy_from_slice(velocity.as_slice());                 // dp/dt
derivative[3..6].copy_from_slice(self.acceleration.as_slice());        // dv/dt
derivative[6..10].copy_from_slice(q_dot.coords.as_slice());           // dq/dt
derivative[10..13].copy_from_slice(angular_acceleration.as_slice());   // dω/dt
derivative

This array contains the instantaneous rates of change required by the RK4 integrator to propagate the state forward in time.

Integration with the RK4 Solver

The update_dynamics_with_controls_rk4 method (also in src/lib.rs) implements the classic fourth-order Runge-Kutta algorithm. It invokes state_derivative four times per timestep to compute the intermediate slopes (k_1) through (k_4):

1. Evaluate derivative at current state ((k_1))
2. Evaluate at state (+\frac{dt}{2}k_1) ((k_2))
3. Evaluate at state (+\frac{dt}{2}k_2) ((k_3))
4. Evaluate at state (+dt \cdot k_3) ((k_4))

The final state update combines these derivatives with weights (\frac{1}{6}, \frac{2}{6}, \frac{2}{6}, \frac{1}{6}) respectively, achieving fourth-order accuracy in the numerical integration.

Practical Example: Running RK4 Integration

The following example demonstrates initializing a quadrotor and performing a single RK4 integration step:

use peng_quad::{Quadrotor, SimulationError};
use nalgebra::Vector3;

// Initialize simulation parameters
let (dt, mass, g, cd) = (0.01, 1.3, 9.81, 0.01);
let inertia = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
let mut quad = Quadrotor::new(dt, mass, g, cd, inertia)?;

// Hover control inputs
let thrust = mass * g;               // Counteract gravity
let torque = Vector3::zeros();       // Maintain level attitude

// Advance simulation by one RK4 step
quad.update_dynamics_with_controls_rk4(thrust, &torque);

This pattern repeats within the simulation loop to propagate the quadrotor state through time with high numerical accuracy.

Summary

• State derivative location: The Quadrotor::state_derivative function in src/lib.rs (lines 21–51) computes the physics-based derivatives required for RK4 integration.
• Vector composition: The 13-element state combines position (3), velocity (3), quaternion (4), and angular velocity (3), with the derivative calculated for each component.
• Physics modeling: The calculation incorporates gravitational forces, aerodynamic drag, thrust rotation into the world frame, and full rotational dynamics including gyroscopic torques.
• RK4 workflow: The update_dynamics_with_controls_rk4 driver calls the derivative function four times per timestep, weighting the results to achieve fourth-order integration accuracy.
• Dependencies: The implementation relies on the nalgebra crate for linear algebra operations, including quaternion multiplication and vector cross products.

Frequently Asked Questions

What is the state vector format used in Peng's RK4 integration?

Peng uses a 13-element array where indices 0–2 store world-frame position, 3–5 store linear velocity, 6–9 store the orientation quaternion (x, y, z, w), and 10–12 store body-frame angular velocity. This format allows the state_derivative function to compute coherent derivatives for both translational and rotational dynamics within a unified numerical framework.

How does the state_derivative function handle rotational dynamics?

The function calculates angular acceleration using the rigid body equation (\dot{\omega} = I^{-1}(\tau - \omega \times I\omega)), where (I) is the inertia matrix and (\tau) is the applied control torque. It also computes the quaternion derivative (\dot{q} = \frac{1}{2}q\omega) to ensure the orientation evolves correctly with the angular velocity, as implemented in lines 31–45 of src/lib.rs.

What external forces are considered in the linear acceleration calculation?

The derivative calculation accounts for three primary forces: gravity (acting in the negative z-direction), aerodynamic drag (opposing velocity with magnitude proportional to velocity squared), and thrust (rotated from the body frame to the world frame using the current orientation quaternion). These forces are summed and divided by mass to produce linear acceleration.

Why does Peng use RK4 instead of Euler integration for quadrotor simulation?

RK4 integration provides fourth-order accuracy, meaning the error per step scales with (dt^5) rather than (dt^2) as in Euler methods. This allows Peng to maintain numerical stability and accuracy with larger timestep sizes, which is crucial for real-time simulation of fast rotational dynamics where angular velocities can change rapidly due to control inputs and gyroscopic effects.

{
  "mcpServers": {
    "instagit": {
      "command": "npx",
      "args": ["-y", "instagit@latest"]
    }
  }
}