# 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

```python
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

```python
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

```python
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

![](https://gitlab.com/davidwoodburn/inu/-/raw/main/figures/fig_lat_rate.svg)

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

![](https://gitlab.com/davidwoodburn/inu/-/raw/main/figures/fig_partial.svg)

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

```python
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")
```

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.

### Built-in Paths

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

Several, pre-defined navigation paths 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

```python
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

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

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

### Discretization

```python
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.

### Orientation

```python
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.

## 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:

```python
# 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.
