Function forward 

pub fn forward(state: &mut StrapdownState, imu_data: IMUData, dt: f64)

NED form of the forward kinematics equations. Corresponds to section 5.4 Local-Navigation Frame Equations from the book Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, Second Edition by Paul D. Groves; Second Edition.

This function implements the forward kinematics equations for the strapdown navigation system. It takes the IMU data and the time step as inputs and updates the position, velocity, and attitude of the system. The IMU data is assumed to be pre-processed and ready for use in the mechanization equations (i.e. the gravity vector has already been filtered out and the data represents relative motion).

Arguments

• imu_data - A reference to an IMUData instance containing the acceleration and gyro data in the body frame.
• dt - A f64 representing the time step in seconds.

Example

use strapdown::{StrapdownState, IMUData, forward};
use nalgebra::Vector3;
let mut state = StrapdownState::default();
let imu_data = IMUData {
   accel: Vector3::new(0.0, 0.0, -9.81), // free fall acceleration in m/s^2
   gyro: Vector3::new(0.0, 0.0, 0.0) // No rotation
};
let dt = 0.1; // Example time step in seconds
forward(&mut state, imu_data, dt);