Struct StrapdownState

pub struct StrapdownState {
    pub position: Vector3<f64>,
    pub velocity: Vector3<f64>,
    pub attitude: Rotation3<f64>,
}

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

position: Vector3<f64>

velocity: Vector3<f64>

attitude: Rotation3<f64>

Implementations

impl StrapdownState

pub fn new() -> StrapdownState

Create a new StrapdownState with all zeros

pub fn new_from( position: Vector3<f64>, velocity: Vector3<f64>, attitude: Vector3<f64>, in_degrees: bool, ) -> StrapdownState

Create a new StrapdownState from a position, velocity, and attitude vectors.

This function initializes a new StrapdownState instance with the given position, velocity, and attitude vectors. The position is in the form of latitude, longitude, and altitude (meters). The corresponding velocities are in the NED/ENU frame (meters per second) and the attitude is given as roll, pitch, and yaw angles. These angles (both attitude angles, latitude, and longitude) can be specified in either degrees or radians using the in_degrees parameter. Note that internally, the angles are converted to radians for calculations.

Arguments

• position - A Vector3 representing the position in the form of latitude (degrees), longitude (degrees), and altitude (meters).
• velocity - A Vector3 representing the velocity in the NED/ENU frame (meters per second).
• attitude - A Vector3 representing the attitude in the form of roll, pitch, and yaw angles (degrees).

Returns

• A StrapdownState instance containing the position, velocity, and attitude.

Example

use strapdown::StrapdownState;
use nalgebra::Vector3;
let position = Vector3::new(37.7749, -122.4194, 0.0); // San Francisco coordinates
let velocity = Vector3::new(0.0, 0.0, 0.0); // No initial velocity
let attitude = Vector3::new(0.0, 0.0, 0.0); // No initial attitude
let state = StrapdownState::new_from(position, velocity, attitude, false);

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

Create a StrapdownState from a vector of states

Creates a StrapdownState object from a cannoincal strapdown state vector. The vector is in the form of: $\left(p_n, p_e, p_d, v_n, v_e, v_d, \phi, \theta, \psi\right)$. Radian or degree mode can be toggled via the in_degrees parameter.

Arguments

• state - A SVector of shape (9,) representing the strapdown state vector.
• in_degrees - A boolean indicating whether the angles are in degrees (true) or radians (false).

Returns

• A StrapdownState instance containing the position, velocity, and attitude.

Example

use strapdown::StrapdownState;
use nalgebra::SVector;

let state_vector: SVector<f64, 9> = SVector::from_vec(vec![37.7749, -122.4194, 0.0, 10.0, 0.0, 0.0, 0.0, 45.0, 0.0]); // Example state vector
let strapdown_state = StrapdownState::new_from_vector(state_vector, true);

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);

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

Convert the StrapdownState to a one dimensional vector, nalgebra style

StrapdownState internally stores the attitude as a direction cosine matrix (DCM) and the position and velocity as vectors. Outside of this object, it is useful to have the navigation state in a traditional cannonical vector form for use in various filters and algorithms. This function converts the internal state to a one dimensional vector in the form of: $\left[p_n, p_e, p_d, v_n, v_e, v_d, \phi, \theta, \psi \right]$

Arguments

• in_degrees - A boolean indicating whether to return the angles in degrees (true) or radians (false).

Returns

• An SVector of shape (9,) representing the strapdown state vector.

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

Convert the StrapdownState to a one dimensional vector, native vec (list) style

StrapdownState internally stores the attitude as a direction cosine matrix (DCM) and the position and velocity as vectors. Outside of this object, it is useful to have the navigation state in a traditional cannonical vector form for use in various filters and algorithms. This function converts the internal state to a one dimensional vector in the form of: $\left[p_n, p_e, p_d, v_n, v_e, v_d, \phi, \theta, \psi \right]$

Arguments

• in_degrees - A boolean indicating whether to return the angles in degrees (true) or radians (false).

Returns

• An SVector of shape (9,) representing the strapdown state vector.

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