Crate strapdown

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. It does not provide utilities for reading raw output from the IMU or act as IMU firmware or driver. As such the IMU data is assumed to be pre-filtered and contain the total accelerations and relative rotations.

This crate is primarily built off of three additional dependencies:

• nav-types: Provides basic coordinate types and conversions.
• nalgebra: Provides the linear algebra tools for the filters.
• rand and rand_distr: Provides random number generation for noise and simulation (primarily for particle filter methods).

All other functionality is built on top of these crates or is auxiliary functionality (e.g. I/O). The primary reference text is Principles 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 equatorial radius is named EQUATORIAL_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 quantity 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 = [p_n, p_e, p_d, v_n, v_e, v_d, \phi, \theta, \psi] $$

Where:

• $p_n$, $p_e$, and $p_d$ 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 (XYZ Euler rotation).

The coordinate convention and order is in NED.

Strapdown equations in the Local-Level Frame

This module implements the strapdown mechanization equations in the Local-Level Frame. These equations form the basis of the forward propagation step (motion/system/state-transition model) of all the filters implemented in this crate. The rational for this was to design and test it once, then re-use it on the various filters which really only need to act on the given probability distribution and are largely ambivilent to the actual function and use generic representations in thier mathematics.

The equations are based on the book Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, Second Edition by Paul D. Groves. Below is a summary of the equations implemented in Chapter 5.4 implemented by this module.

Skew-Symmetric notation

Groves uses a direction cosine matrix representation of orientation (attitude, rotation). As such, to make the matrix math work out, rotational quantities need to also be represented using matricies. Groves’ convention is to use a lower-case letter for vector quantities (arrays of shape (N,) Python-style, or (N,1) nalgebra/Matlab style) and capital letters for the skew-symmetric matrix representation of the same vector.

$$ x = \begin{bmatrix} a \\ b \\ c \end{bmatrix} \rightarrow X = \begin{bmatrix} 0 & -c & b \\ c & 0 & -a \\ -b & a & 0 \end{bmatrix} $$

Attitude update

Given a direction-cosine matrix $C_b^n$ representing the orientation (attitude, rotation) of the platform’s body frame ($b$) with respect to the local level frame ($n$), the transport rate $\Omega_{en}^n$ representing the rotation of the local level frame with respect to the Earth-fixed frame ($e$), the Earth’s rotation rate $\Omega_{ie}^e$, and the angular rate $\Omega_{ib}^b$ representing the rotation of the body frame with respect to the inertial frame ($i$), the attitude update equation is given by:

$$ C_b^n(+) \approx C_b^n(-) \left( I + \Omega_{ib}^b t \right) - \left( \Omega_{ie}^e - \Omega_{en}^n \right) C_b^n(-) t $$

where $t$ is the time differential and $C(-)$ is the prior attitude. These attitude matricies are then used to transform the specific forces from the IMU:

$$ f_{ib}^n \approx \frac{1}{2} \left( C_b^n(+) + C_b^n(-) \right) f_{ib}^b $$

Velocity Update

The velocity update equation is given by:

$$ v(+) \approx v(-) + \left( f_{ib}^n + g_{b}^n - \left( \Omega_{en}^n - \Omega_{ie}^e \right) v(-) \right) t $$

Position update

Finally, we update the base position states in three steps. First we update the altitude:

$$ p_d(+) = p_d(-) + \frac{1}{2} \left( v_d(-) + v_d(+) \right) t $$

Next we update the latitude:

$$ p_n(+) = p_n(-) + \frac{1}{2} \left( \frac{v_n(-)}{R_n + p_d(-)} + \frac{v_n(+)}{R_n + p_d(+) } \right) t $$

Finally, we update the longitude:

$$ p_e = p_e(-) + \frac{1}{2} \left( \frac{v_e(-)}{R_e + p_d(-) \cos(p_n(-))} + \frac{v_e(+)}{R_e + p_d(+) \cos(p_n(+))} \right) t $$

This top-level module provides a public API for each step of the forward mechanization equations, allowing users to easily pass data in and out.

Modules

earth

Earth-related constants and functions

filter

Inertial Navigation Filters

linalg

sim

Simulation utilities and CSV data loading for strapdown inertial navigation.

Structs

IMUData

Basic structure for holding IMU data in the form of acceleration and angular rate vectors.

StrapdownState

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

Functions

forward

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.

position_update

Position update in NED

wrap_latitude

Wrap latitude to the range -90 to 90 degrees

wrap_to_2pi

Wrap an angle to the range 0 to $2 \pi$ radians

wrap_to_180

Wrap an angle to the range -180 to 180 degrees

wrap_to_360

Wrap an angle to the range 0 to 360 degrees

wrap_to_pi

Wrap an angle to the range 0 to $\pm\pi$ radians