Struct StrapdownState

pub struct StrapdownState {
    pub latitude: f64,
    pub longitude: f64,
    pub altitude: f64,
    pub velocity_north: f64,
    pub velocity_east: f64,
    pub velocity_down: f64,
    pub attitude: Rotation3<f64>,
    pub coordinate_convention: bool,
}

Basic structure for holding the strapdown mechanization state in the form of position, velocity, and attitude.

Attitude is stored in matrix form (rotation or direction cosine matrix) and position and velocity are stored as vectors. The order or the states depends on the coordinate system used. The struct does not care, but the coordinate system used will determine which functions you should use. Default is NED but nonetheless must be assigned. For computational simplicity, latitude and longitude are stored as radians.

Fields

latitude: f64

Latitude in radians

longitude: f64

Longitude in radians

altitude: f64

Altitude in meters

velocity_north: f64

Velocity north in m/s (NED frame)

velocity_east: f64

Velocity east in m/s (NED frame)

velocity_down: f64

Velocity down in m/s (NED frame)

attitude: Rotation3<f64>

Attitude as a rotation matrix (unchanged)

coordinate_convention: bool

Coordinate convention used for the state vector (NED or ENU; NED is true by default)

Implementations

impl StrapdownState

pub fn new() -> StrapdownState

Create a new StrapdownState with all zeros

pub fn new_from_components( latitude: f64, longitude: f64, altitude: f64, velocity_north: f64, velocity_east: f64, velocity_down: f64, attitude: Rotation3<f64>, in_degrees: bool, ned: bool, ) -> StrapdownState

Create a new StrapdownState from explicit position and velocity components, and attitude

Arguments

• latitude - Latitude in radians or degrees (see in_degrees).
• longitude - Longitude in radians or degrees (see in_degrees).
• altitude - Altitude in meters.
• velocity_north - North velocity in m/s.
• velocity_east - East velocity in m/s.
• velocity_down - Down velocity in m/s.
• attitude - Rotation3 attitude matrix.
• in_degrees - If true, angles are provided in degrees and will be converted to radians.
• ned - If true, the coordinate convention is NED (North, East, Down), otherwise ENU (East, North, Up).

pub fn new_from_vector( state: SVector<f64, 9>, in_degrees: bool, ) -> StrapdownState

Create a StrapdownState from a canonical state vector (NED order: lat, lon, alt, v_n, v_e, v_d, roll, pitch, yaw)

Arguments

• state - SVector<f64, 9> in the order [lat, lon, alt, v_n, v_e, v_d, roll, pitch, yaw]
• in_degrees - If true, angles are provided in degrees and will be converted to radians.

pub fn to_vector(&self, in_degrees: bool) -> SVector<f64, 9>

Convert the StrapdownState to a one dimensional vector, nalgebra style

Arguments

• in_degrees - If true, angles are returned in degrees.

Returns

• SVector<f64, 9> in the order [lat, lon, alt, v_n, v_e, v_d, roll, pitch, yaw]

pub fn to_vec(&self, in_degrees: bool) -> Vec<f64>

Convert the StrapdownState to a one dimensional vector, native Vec style

Arguments

• in_degrees - If true, angles are returned in degrees.

Returns

• Vec in the order [lat, lon, alt, v_n, v_e, v_d, roll, pitch, yaw]

pub fn get_velocity(&self) -> Vector3<f64>

pub fn forward(&mut self, 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};
use nalgebra::Vector3;
let mut state = StrapdownState::new();
let imu_data = IMUData::new_from_vector(
   Vector3::new(0.0, 0.0, -9.81), // free fall acceleration in m/s^2
   Vector3::new(0.0, 0.0, 0.0) // No rotation
);
let dt = 0.1; // Example time step in seconds
state.forward(&imu_data, dt);

Trait Implementations

impl Clone for StrapdownState

fn clone(&self) -> StrapdownState

Returns a duplicate of the value.

fn clone_from(&mut self, source: &Self)

Performs copy-assignment from source.

impl Debug for StrapdownState

fn fmt(&self, f: &mut Formatter<'_>) -> Result

Formats the value using the given formatter.

impl Default for StrapdownState

fn default() -> Self

Returns the “default value” for a type.

impl Copy for StrapdownState