Crate strapdown_core

Strapdown navigation toolbox for various navigation filters

This crate provides a set of tools for implementing navigation filters in Rust. The filters are implemented as structs that can be initialized and updated with new sensor data. The filters are designed to be used in a strapdown navigation system, where the orientation of the sensor is known and the sensor data can be used to estimate the position and velocity of the sensor. While utilities exist for IMU data, this crate does not currently support IMU output directly and should not be thought of as a full inertial navigation system (INS). This crate is designed to be used to test the filters that would be used in an INS, but the raw IMU output firmware. As such the IMU data is assumed to be relative accelerations and rotations. Additional signals that can be derived using IMU data, such as gravity or magnetic vector and anomalies, should come from a separate IMU channel. In other words, to calculate the gravity vector the IMU output should be parsed to seperately output the overall acceleration and rotation of the sensor whereas the navigation filter will use the gravity and orientation corrected acceleration and rotation to estimate the position

Primarily built off of three crate dependencies:

• nav-types: Provides basic coordinate types and conversions.
• nalgebra: Provides the linear algebra tools for the filters.
• haversine-rs: Provides the haversine formula for calculating distances between two points on the Earth’s surface, which is the primary error metric. All other functionality is built on top of these crates. The primary reference text is Prinicples of GNSS, Inertial, and Multisensor Integrated Navigation Systems, 2nd Edition by Paul D. Groves. Where applicable, calculations will be referenced by the appropriate equation number tied to the book. In general, variables will be named according to the quantity they represent and not the symbol used in the book. For example, the Earth’s equitorial radius is named EQUITORIAL_RADIUS instead of a. This style is sometimes relaxed within the body of a given function, but the general rule is to use descriptive names for variables and not mathematical symbols. Strapdown mechanization data and equations

This crate contains the implementation details for the strapdown navigation equations implemented in the Local Navigation Frame. The equations are based on the book “Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, Second Edition” by Paul D. Groves. This file corresponds to Chapter 5.4 and 5.5 of the book. Effort has been made to reproduce most of the equations following the notation from the book. However, variable and constants should generally been named for the quatity they represent rather than the symbol used in the book.

Coordinate and state definitions

The typical nine-state NED/ENU navigation state vector is used in this implementation. The state vector is defined as:

x = [pn, pe, pd, v_n, v_e, v_d, phi, theta, psi]

Where:

• pn, pe, and pd are the WGS84 geodetic positions (degrees latitude, degrees longitude, meters relative to the ellipsoid).
• v_n, v_e, and v_d are the local level frame (NED/ENU) velocities (m/s) along the north axis, east axis, and vertical axis.
• phi, theta, and psi are the Euler angles (radians) representing the orientation of the body frame relative to the local level frame.

The coordinate convention and order is in NED. ENU implementations are to be added in the future.

Modules

earth

Earth-related constants and functions

filter

Inertial Navigation Filters This module contains implementations of various inertial navigation filters, including Kalman filters and particle filters. These filters are used to estimate the state of a strapdown inertial navigation system based on IMU measurements and other sensor data. The filters use the strapdown equations (provided by the StrapdownState) to propagate the state in the local level frame. The filters also use a generic position measurement model to update the state based on position measurements in the local level frame.

Structs

IMUData

Basic structure for holding IMU data in the form of acceleration and angular rate vectors. The vectors are in the body frame of the vehicle and represent relative movement. This structure and library is not intended to be a hardware driver for an IMU, thus the data is assumed to be pre-processed and ready for use in the mechanization equations (the IMU processing has already filtered out gravitational acceleration).

StrapdownState

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 compuational simplicity, latitude and logitude are stored as radians.

Functions

add

wrap_to_2pi

Wrap an angle to the range 0 to 2*pi radians This function is generic and can be used with any type that implements the necessary traits.

wrap_to_180

Wrap an angle to the range -180 to 180 degrees This function is generic and can be used with any type that implements the necessary traits.

wrap_to_360

Wrap an angle to the range 0 to 360 degrees This function is generic and can be used with any type that implements the necessary traits.

wrap_to_pi

Wrap an angle to the range 0 to 2*pi radians This function is generic and can be used with any type that implements the necessary traits.