inu

Inertial Navigation Utilities

The inu.py Python library provides a comprehensive set of tools for inertial navigation, focusing on the mechanization of Inertial Measurement Unit (IMU) sensor data (accelerometer and gyroscope readings) to derive position, velocity, and attitude, as well as the inverse process to compute sensor values from pose data. It includes utilities for generating navigation paths, estimating velocities and attitudes, and handling barometric altitude aiding. This library is well-suited for inertial navigation system simulations, flight path planning, and state estimation.

Inertial Mechanization

Mechanization

llh_t, vne_t, rpy_t = inu.mech(
    fbbi_t: np.ndarray,
    wbbi_t: np.ndarray,
    llh0: np.ndarray,
    vne0: np.ndarray,
    rpy0: np.ndarray,
    T: float,
    hae_t: np.ndarray | None = None,
    baro_name: str | None = None,
    grav_model: Callable[[np.ndarray], np.ndarray] = somigliana)
Dllh, Dvne, wbbn = inu.mech_step(
    fbbi: np.ndarray,
    wbbi: np.ndarray,
    llh: np.ndarray,
    vne: np.ndarray,
    Cnb: np.ndarray,
    hb: float | None = None,
    baro: Baro | None = None,
    grav_model: Callable[[np.ndarray], np.ndarray] = somigliana)

The mech function performs forward mechanization, integrating IMU sensor data (fbbi_t and wbbi_t) to compute position (llh_t), velocity (vne_t), and attitude (rpy_t). It supports barometric altitude aiding or direct height override. This function processes an entire time-history profile of sensor values and returns the path solution for the corresponding span of time. If you would prefer to mechanize only one step at a time, you can call the mech_step function instead. Actually, the mech function does call the mech_step function within a for loop.

The mech function can override the height solution with whatever is provided for hae_t. If a barometric altimeter is named (one of eight names), hae_t will be treated as the barometric altitude. Similarly, the mech_step function can take a barometric model, generated by the Baro class.

Inverse Mechanization

fbbi_t, wbbi_t = inu.mech_inv(
    llh_t: np.ndarray,
    rpy_t: np.ndarray,
    T: float,
    grav_model: Callable[[np.ndarray], np.ndarray] = somigliana)

The mech_inv function performs inverse mechanization, taking path information in the form of position (llh_t) and attitude (rpy_t) over time and estimates the corresponding sensor values for an accelerometer (fbbi_t) and gyroscope (wbbi_t). This function is fully vectorized (i.e., no for loop internally), which means it processes a profile very quickly. Note that the velocity is internally calculated from position over time. This function is the perfect dual of the mech (forward mechanization) algorithm. This means a navigation path could be input into mech_inv to generate sensor profiles; those profiles could be fed into mech; and the resulting navigation path would match the original.

Dynamics Jacobian

F = inu.jacobian(fbbi, llh, vne, Cnb, baro=None)

The Jacobian of the dynamics is calculated using the jacobian function. This can be used in state estimation filters (e.g., EKF). This is a square matrix whose elements are the derivatives with respect to state of the continuous-domain, time-derivatives of states. For example, the time derivative of latitude is

So, the derivative of this with respect to height above ellipsoid is

The order of the states is position (latitude, longitude, and height), velocity (North, East, and Down), and attitude. So, the above partial derivative would be found in row 1, column 3 of the Jacobian matrix.

The representation of attitude is naturally complicated. This library uses 3x3 direction cosine matrices (DCMs) to process attitude. The change in attitude is represented by a tilt error vector, which means the last three states in the Jacobian are the x, y, and z tilt errors. This makes a grand total of 9 states, so the Jacobian is a 9x9 matrix.

Truth Generation

Waypoints

way = inu.waypoints(
    points: np.ndarray,
    seg_len: float = 1.0,
    radius_min: float = 0.0,
    plot: bool = True,
    ax: axes= None,
    color: str = "tab:blue",
    warncolor: str = "tab:orange",
    bounds: Callable[[np.ndarray, np.ndarray],
        np.ndarray | float] | list | tuple | None = None,
    ned: bool = True)

The waypoints class generates smooth navigation paths using Bezier curves from waypoints, ensuring constant velocity for a uniform sampling rate. It takes a (2, N) array of North and East waypoints, (points) and creates an interactive plot connecting the waypoints with quadratic Bezier curves. These curves can be manipulated by moving, adding, and deleting waypoints. The modified waypoints are accesible from way.points. The field way.path contains the final North, East, Down (NED) coordinates of the navigation path. The down coordinates will all be zero.

Strictly speaking, the term "waypoints" is not accurate because the path does not pass through these points; however, it is believed that "waypoints" does a better job of communicating the idea of path planning than "control points".

Built-in Paths

points = inu.points_box(
    width: float = 2000.0,
    height: float = 2000.0,
    radius: float = 300.0,
    cycles: int = 3)
points = inu.points_clover(
    radius: float = 10000.0,
    cycles: int = 3)
points = inu.points_grid(
    spacing: float = 300.0,
    length: float = 1600.0,
    rows: int = 6)
points = inu.points_spiral(
    spacing: float = 300.0,
    cycles: int = 3)

Several functions have been provided to generate the control points necessary to pass to waypoints in order to produce interesting navigation paths.

pc_t = inu.path_box(
    seg_len: float,
    width: float = 2000.0,
    height: float = 2000.0,
    radius: float = 300.0,
    cycles: int = 3,
    ned: bool = True,
    plot: bool = False)
pc_t = inu.path_circle(
    seg_len: float,
    radius: float = 1000.0,
    cycles: int = 5,
    ned: bool = True)
pc_t = inu.path_clover(
    seg_len: float,
    radius: float = 10000.0,
    cycles: int = 3,
    ned: bool = True,
    plot: bool = False)
pc_t = inu.path_grid(
    seg_len: float,
    spacing: float = 300.0,
    length: float = 1600.0,
    rows: int = 6,
    ned: bool = True,
    plot: bool = False)
pc_t = inu.path_pretzel(
    K: int,
    radius: float = 1000.0,
    height: float = 100.0,
    cycles: float = 1.0,
    twists: int = 1,
    ned: bool = True)
pc_t = inu.path_spiral(
    seg_len: float,
    spacing: float = 300.0,
    cycles: int = 3,
    ned: bool = True,
    plot: bool = False)

Several, pre-defined navigation paths generated using the control-point generation are also provided. These will return the North, East, Down coordinates of the navigation path. The user can then convert these to geodetic coordinates with either the r3f.curvilinear_to_geodetic or r3f.tangent_to_geodetic function.

Attitude and Velocity from Position

t, vne_t, rpy_t = inu.llh_to_tva(llh_t, T)
vne_t = inu.llh_to_vne(llh_t, T)
rpy_t = inu.vne_to_rpy(vne_t, grav_t, T, alpha=0.06, wind=None)

With a navigation path, llh_t, the velocity and attitude can be estimated assuming coordinated turns.

Gravity

grav = inu.somigliana(llh: np.ndarray)

Calculate local gravity acceleration using the Somigliana equation. The gravity vector is in North, East, Down (NED) coordinates.

Support Functions

Orientation

vec = inu.ned_enu(vec)

This library assumes all local-level coordinates are in the North, East, Down (NED) orientation. If your coordinates are in the East, North, Up (ENU) orientation or you wish for the final results to be converted to that orientation, use the ned_enu function.

Discretization

Phi, Bd, Qd = inu.vanloan(F, B=None, Q=None, T=None)

The extended Kalman filter (EKF) examples in the examples/ directory show a reduced-order approximation to the matrix exponential of the Jacobian. The Q dynamics noise covariance matrix also needs to be discretized. This was done with a first-order approximation by just multiplying by the sampling period T. This is reasonably accurate and computationally fast. However, it is an approximation. The mathematically accurate way to discretize the Jacobian and Q is to use the van Loan method. This is implemented with the vanloan function.

Path Offset

xo, yo = inu.offset_path(
    x: np.ndarray,
    y: np.ndarray,
    d: np.ndarray | float)

Compute the coordinates of a closed polygon outlining a filled area offset from a 2D path.

The input path is defined by coordinates (x, y), and the offset distance d specifies the perpendicular distance to shift the path outward on both sides. The resulting polygon is formed by concatenating the outward offset paths on either side, forming a closed loop, a clockwise encirclement of the input path.

Extended Kalman Filter

An extended Kalman filter can be implemented using this library. The mech_step function applies the mechanization equations to a single time step. It returns the time derivatives of the states. The jacobian function calculates the continuous-domain Jacobian of the dynamics function. While this does mean that the user must then manually integrate the derivatives and discretize the Jacobian, this gives the user greater flexibility to decide how to discretize them. There are a few example scripts provided in the examples/ folder.

The example code below is meant to run within a for loop stepping through time, where k is the time index:

# Inputs
fbbi = fbbi_t[:, k] # specific forces (m/s^2)
wbbi = wbbi_t[:, k] # rotation rates (rad/s)
z = z_t[:, k] # GPS position (rad, rad, m)

# Update
S = H @ Ph @ H.T + R # innovation covariance (3, 3)
Si = np.linalg.inv(S) # inverse (3, 3)
Kg = Ph @ H.T @ Si # Kalman gain (9, 3)
Ph -= Kg @ H @ Ph # update to state covariance (9, 9)
r = z - llh # innovation (3,)
dx = Kg @ r # changes to states (9,)
llh += dx[:3] # add change in position
vne += dx[3:6] # add change in velocity
# matrix exponential of skew-symmetric matrix
Psi = r3f.rodrigues(dx[6:])
Cnb = Psi.T @ Cnb

# Save results.
tllh_t[:, k] = llh.copy()
tvne_t[:, k] = vne.copy()
trpy_t[:, k] = r3f.dcm_to_rpy(Cnb.T)

# Get the Jacobian and propagate the state covariance.
F = inu.jacobian(fbbi, llh, vne, Cnb)
Phi = I + (F*T)@(I + (F*T/2)) # 2nd-order expm(F T)
Ph = Phi @ Ph @ Phi.T + Qd

# Get the state derivatives.
Dllh, Dvne, wbbn = inu.mech_step(fbbi, wbbi, llh, vne, Cnb)

# Integrate (forward Euler).
llh += Dllh * T # change applies linearly
vne += Dvne * T # change applies linearly
Cnb[:, :] = Cnb @ r3f.rodrigues(wbbn * T)
Cnb[:, :] = r3f.mgs(Cnb)

In the example above, H should be a (3, 9) matrix with ones along the diagonal. The Qd should be the (9, 9) discretized dynamics noise covariance matrix. The R should be the (3, 3) measurement noise covariance matrix. Note that forward Euler integration has been performed on the state derivatives and a second-order approximation to the matrix exponential has been implemented to discretize the continuous-time Jacobian.

Key Features

Accuracy

The mechanization algorithms in this library make no simplifying assumptions. The Earth is defined as an ellipsoid. Any deviations of the truth from this simple shape can be captured by more complex gravity models. The algorithms use a single frequency update structure which is much simpler than the common two-frequency update structure and just as, if not more, accurate.

Duality

The forward and inverse mechanization functions are perfect duals of each other. This means that if you started with a profile of position, velocity, and attitude and passed these into the inverse mechanization algorithm to get sensor values and then passed those sensor values into the forward mechanization algorithm, you would get back the original position, velocity, and attitude profiles. The only error would be due to finite-precision rounding.

Vectorization

When possible, the functions are vectorized in order to handle processing batches of values. A set of scalars is a 1D array. A set of vectors is a 2D array, with each vector in a column. So, a (3, 7) array is a set of seven vectors, each with 3 elements. If an input matrix does not have 3 rows, it will be assumed that the rows of the matrix are vectors.

An example of the vectorization in this library is the mech_inv (inverse mechanization) algorithm. There is no for loop to iterate through time; rather the entire algorithm has been vectorized. This results in an over 100x speed increase.

Numerical Methods

Employs forward Euler for integration and differentiation and Rodrigues rotation for attitude updates.

Flexibility

Supports custom gravity models and barometric aiding for altitude correction.