Navigation & Inertial Measurement Systems

Guide to the inertial navigation (INS) and GNSS/INS integration functions in pytcl.navigation.

This guide covers the complete workflow for building navigation systems with the shipped INS suite: strapdown mechanization, alignment, coning/sculling compensation, and loosely/tightly coupled GNSS integration.

Table of Contents:

• INS Fundamentals

• INS Mechanization

• Error Modeling & Propagation

• GNSS/INS Integration Architectures

• Practical Implementation

• Diagnostics & Troubleshooting

• Best Practices

INS Fundamentals

Inertial Navigation Systems compute position, velocity, and attitude by integrating acceleration and rotation rate measurements from accelerometers and gyroscopes.

Key Sensors:

• Accelerometers: Measure specific force (acceleration + gravity) in body frame

• Gyroscopes: Measure rotation rates about body axes

• Integrated IMU: Combined 6-DOF sensor package (3 accelerometers, 3 gyroscopes)

INS Coordinate Frames:

1. Body Frame (b-frame): Fixed to vehicle, rotated with aircraft/spacecraft

2. Navigation Frame (n-frame): Local tangent plane (NED or ENU)

3. ECEF Frame: Earth-centered, Earth-fixed (for global navigation)

4. Inertial Frame (i-frame): Inertial reference (for precision work)

Mechanization Equation:

The fundamental INS equations relate specific force and rotation rates to position and velocity:

\[ \begin{align}\begin{aligned}\dot{\mathbf{v}}^n = \mathbf{C}_b^n \mathbf{f}^b - (\mathbf{2}\boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n) \times \mathbf{v}^n + \mathbf{g}^n\\\dot{\mathbf{p}}^n = \mathbf{v}^n\\\dot{\mathbf{C}}_b^n = \mathbf{C}_b^n [\boldsymbol{\omega}_{ib}^b]_\times - [\boldsymbol{\omega}_{in}^n]_\times \mathbf{C}_b^n\end{aligned}\end{align} \]

Where:

• \(\mathbf{v}^n\): velocity in nav frame

• \(\mathbf{C}_b^n\): direction cosine matrix (body to nav)

• \(\mathbf{f}^b\): specific force measurements

• \(\boldsymbol{\omega}_{ie}^n\): Earth rotation rate

• \(\boldsymbol{\omega}_{en}^n\): transport rate (due to vehicle motion)

• \(\mathbf{g}^n\): gravity vector in nav frame

Basic INS Propagation

The library ships a full NED-frame strapdown mechanization following Groves (2013), Chapter 5: create an INSState with initialize_ins_state, wrap each IMU sample in IMUData, and step with mechanize_ins_ned.

INSState holds position as geodetic [latitude (rad), longitude (rad), altitude (m)], velocity as NED [vN, vE, vD] (m/s), and quaternion as the scalar-first body-to-NED attitude quaternion.

import numpy as np
from pytcl.navigation import IMUData, initialize_ins_state, mechanize_ins_ned

# Initialize at a known position, stationary and level
state = initialize_ins_state(
    lat=np.radians(40.7128),
    lon=np.radians(-74.0060),
    alt=100.0,
)

# Stationary IMU: the accelerometer measures the reaction to gravity
g = 9.80665
imu = IMUData(
    accel=np.array([0.0, 0.0, -g]),  # specific force (m/s^2)
    gyro=np.array([0.0, 0.0, 0.0]),  # angular rate (rad/s)
    dt=0.01,                          # sample period (s)
)

# Propagate 1 second of data
for _ in range(100):
    state = mechanize_ins_ned(state, imu)

print(f"Latitude:  {np.degrees(state.position[0]):.6f} deg")
print(f"Velocity NED: {state.velocity} m/s")

Passing the previous IMU sample via accel_prev / gyro_prev enables the built-in coning/sculling compensation (see below).

The mechanization building blocks are exported individually if you need them: earth_rate_ned, transport_rate_ned, gravity_ned, normal_gravity, update_attitude_ned, and radii_of_curvature.

from pytcl.navigation import earth_rate_ned, transport_rate_ned, gravity_ned

lat = np.radians(40.7128)
omega_ie = earth_rate_ned(lat)                            # rad/s
omega_en = transport_rate_ned(lat, alt=100.0, vN=100.0, vE=50.0)
g_ned = gravity_ned(lat, 100.0)                           # [0, 0, g]

INS Error Sources & Modeling

Real inertial sensors exhibit various error characteristics that cause INS drift:

Accelerometer Errors:

\[\mathbf{f}_{measured} = \mathbf{f}_{true} + \mathbf{b}_a + \mathbf{S}_a \mathbf{f}_{true} + \mathbf{n}_a + \text{temp effects}\]

Where:

• \(\mathbf{b}_a\): bias (constant offset, drifts over time)

• \(\mathbf{S}_a\): scale factor (gain error)

• \(\mathbf{n}_a\): white noise (high frequency)

Gyroscope Errors:

• Bias: Several types (constant, random walk, rate-dependent)

• Scale factor: Gain errors on rotation rates

• Noise: Angle random walk and rate random walk

• Coupling: Accelerometer-induced errors (g-sensitivity)

Practical Error Magnitudes (Mid-Grade INS):

Sensor	Typical Bias	Random Walk Rate
Accel	50-100 mg (0.5-1 m/s^2)	~0.01 m/s^2/sqrt(hr)
Gyro	50-200 deg/hr (0.01-0.055 deg/s)	~0.3 deg/hr/sqrt(hr)

INS Divergence Over Time (No Updates):

For unaided INS (no GNSS), position error grows approximately as:

\[\sigma_{\text{position}} \approx 0.5 \, \sigma_{\text{accel\_bias}} \cdot t^2\]

def estimate_gnss_outage_duration(accel_bias_std, desired_error=100.0):
    """
    Maximum GNSS outage before position error exceeds the limit.

    Position error ~ 0.5 * bias * t^2, so t = sqrt(2 * error / bias).
    """
    return np.sqrt(2.0 * desired_error / accel_bias_std)

# 1 mm/s^2 bias, 100 m budget -> about 7.5 minutes
print(f"Max outage: {estimate_gnss_outage_duration(0.001):.0f} s")

GNSS/INS Integration Architectures

1. Loosely Coupled Integration

GNSS and INS process measurements independently. GNSS provides position/velocity updates to a 15-state error-state Kalman filter.

Advantages:

• Simple implementation

• Works with standard GNSS receivers

• Easy to debug

Disadvantages:

• Slower convergence after GNSS outage

• Cannot use GNSS during high dynamics

The library ships this filter: initialize_ins_gnss builds an INSGNSSState (INS state + 15-state error covariance), loose_coupled_predict runs the mechanization and covariance propagation, and loose_coupled_update applies a GNSSMeasurement.

Important

The first three error states are [dlat, dlon, dheight] in [rad, rad, m] – the same units as INSState.position. A GNSS accuracy quoted in meters must be converted with position_std_to_error_state_units before it can go on the measurement covariance diagonal.

from pytcl.navigation import (
    GNSSMeasurement,
    IMUData,
    initialize_ins_gnss,
    initialize_ins_state,
    loose_coupled_predict,
    loose_coupled_update,
    position_std_to_error_state_units,
)

# Initialize INS and the integration filter
ins_state = initialize_ins_state(
    lat=np.radians(40.7128), lon=np.radians(-74.0060), alt=100.0
)
state = initialize_ins_gnss(ins_state, position_std=10.0, velocity_std=1.0)

# INS prediction at the IMU rate (100 Hz here)
imu = IMUData(
    accel=np.array([0.0, 0.0, -9.80665]),
    gyro=np.zeros(3),
    dt=0.01,
)
for _ in range(100):
    state = loose_coupled_predict(state, imu)

# GNSS update at 1 Hz: convert the 5 m accuracy to [rad, rad, m] units
pos_std = position_std_to_error_state_units(
    5.0, lat=state.ins_state.position[0]
)
gnss = GNSSMeasurement(
    position=np.array([np.radians(40.7128), np.radians(-74.0060), 100.0]),
    velocity=np.zeros(3),
    position_cov=np.diag(pos_std**2),
    velocity_cov=np.eye(3) * 0.1**2,
    time=1.0,
)
result = loose_coupled_update(state, gnss)
state = result.state

# result.innovation / result.innovation_cov feed integrity monitoring
print(f"Position error std (rad, rad, m): "
      f"{np.sqrt(np.diag(state.error_cov)[:3])}")

loose_coupled_update_position and loose_coupled_update_velocity apply position-only or velocity-only updates with the same interface.

2. Tightly Coupled Integration

INS and GNSS share a single Kalman filter. GNSS measurements are raw pseudoranges, not derived position.

Advantages:

• Better performance during signal degradation (works with < 4 satellites)

• Faster convergence

• More robust to GNSS outages

Disadvantages:

• Complex implementation

• Requires raw GNSS data

• Needs GNSS receiver control

Shipped building blocks: tight_coupled_update, tight_coupled_measurement_matrix, tight_coupled_pseudorange_innovation, pseudorange_measurement_matrix, satellite_elevation_azimuth, and compute_dop. The INSGNSSState carries the receiver clock_bias and clock_drift states these need.

3. Ultra-Tight Coupling

INS state is used to predict GNSS signal tracking parameters (carrier frequency, code delay). The tracking loops and INS filter are integrated.

Advantages:

• Works in severe signal degradation

• Maintains tracking in high-dynamic environments

Disadvantages:

• Requires custom GNSS receiver

• Complex real-time implementation

This architecture requires receiver internals and is out of scope for the library.

Practical Implementation Considerations

Coning and Sculling Compensation

When integrating gyro measurements over finite time steps, simple integration accumulates rotation errors. The shipped two-sample corrections follow Savage’s algorithm:

from pytcl.navigation import coning_correction, sculling_correction

dt = 0.01

gyro_prev = np.array([0.010, 0.002, 0.001])  # rad/s
gyro_curr = np.array([0.011, 0.001, 0.002])

accel_prev = np.array([0.1, 0.0, -9.8])  # m/s^2
accel_curr = np.array([0.2, 0.1, -9.8])

# Both functions take INCREMENTS, not rates: integrate over the interval
# first. Passing raw rates is wrong by a factor of 1/dt^2.
dtheta_prev, dtheta_curr = gyro_prev * dt, gyro_curr * dt
dv_prev, dv_curr = accel_prev * dt, accel_curr * dt

# Coning: cross product of successive angular increments
delta_coning = coning_correction(dtheta_prev, dtheta_curr)

# Sculling: the velocity-domain counterpart
delta_sculling = sculling_correction(dv_prev, dv_curr,
                                     dtheta_prev, dtheta_curr)

compensate_imu_data applies both at once, and mechanize_ins_ned / loose_coupled_predict do it internally when you pass accel_prev and gyro_prev.

Alignment & Initialization

Proper system alignment is critical for INS accuracy. coarse_alignment levels the platform from averaged stationary accelerometer data, and gyrocompass_alignment resolves heading from the sensed Earth rotation (requires navigation-grade gyros):

from pytcl.navigation import coarse_alignment, gyrocompass_alignment

# Average stationary accelerometer samples (removes noise)
accel_samples = np.random.default_rng(42).normal(
    [0.05, -0.02, -9.80665], 0.01, size=(100, 3)
)
accel_avg = accel_samples.mean(axis=0)

lat = np.radians(40.7128)
roll, pitch = coarse_alignment(accel_avg, lat)

# Averaged stationary gyro output senses Earth rotation
OMEGA_E = 7.292115e-5  # rad/s
gyro_avg = np.array([OMEGA_E * np.cos(lat), 0.0,
                     -OMEGA_E * np.sin(lat)])
yaw = gyrocompass_alignment(gyro_avg, roll, pitch, lat)

print(f"Roll:  {np.degrees(roll):.3f} deg")
print(f"Pitch: {np.degrees(pitch):.3f} deg")
print(f"Yaw:   {np.degrees(yaw):.3f} deg")

For consumer-grade gyros that cannot sense Earth rotation, initialize heading from a magnetometer or a known reference instead.

Handling GNSS Outages

During GNSS signal loss, only INS measurements are available and the error covariance grows through loose_coupled_predict. Before applying a GNSS update after an outage (or a suspect measurement at any time), gate it with the chi-square innovation test:

from pytcl.navigation import gnss_outage_detection

# From the last loose_coupled_update result
is_outlier = gnss_outage_detection(result.innovation,
                                   result.innovation_cov)
if not is_outlier:
    state = result.state  # accept the update

State Vector Estimation & Diagnostics

The 15-State Error Vector

INSGNSSState.error_cov is the covariance of the error state (INSErrorState):

ERROR_STATE_LABELS = [
    'dlat (rad)', 'dlon (rad)', 'dalt (m)',        # position error (0-2)
    'dvN (m/s)', 'dvE (m/s)', 'dvD (m/s)',         # velocity error (3-5)
    'att_x (rad)', 'att_y (rad)', 'att_z (rad)',   # attitude error (6-8)
    'accel_bias_x', 'accel_bias_y', 'accel_bias_z',  # m/s^2 (9-11)
    'gyro_bias_x', 'gyro_bias_y', 'gyro_bias_z',     # rad/s (12-14)
]

def diagnose_convergence(error_cov):
    """Check whether the filter has converged."""
    std = np.sqrt(np.diag(error_cov))
    diagnostics = {
        'pos_std': std[0:3],       # [rad, rad, m] - NOT meters for lat/lon
        'vel_std': std[3:6],       # m/s
        'att_std_deg': np.degrees(std[6:9]),
        'accel_bias_std': std[9:12],
        'gyro_bias_std': std[12:15],
    }
    is_converged = (
        np.all(diagnostics['pos_std'][:2] < 1e-4)   # ~600 m of latitude
        and diagnostics['pos_std'][2] < 500.0        # altitude (m)
        and np.all(diagnostics['vel_std'] < 50.0)
        and np.all(diagnostics['att_std_deg'] < 10.0)
    )
    return diagnostics, is_converged

diagnostics, converged = diagnose_convergence(state.error_cov)
print(f"Position std: {diagnostics['pos_std']}")
print(f"Converged: {converged}")

ins_error_state_matrix and ins_process_noise_matrix expose the underlying error dynamics F and process noise Q if you build a custom filter around the same error state.

Common Issues & Solutions

Problem: INS diverges without GNSS updates

Solution: this is physics, not a bug – unaided INS drift is unbounded. Keep loose_coupled_predict running so error_cov honestly reflects the drift, and bound the mission time using the outage-duration estimate above.

Problem: Large attitude errors at startup

Solution: perform stationary alignment for 30-60 seconds before navigating:

def stationary_alignment(accel_buffer, gyro_buffer, lat):
    """Align attitude while the vehicle is stationary."""
    roll, pitch = coarse_alignment(accel_buffer.mean(axis=0), lat)
    yaw = gyrocompass_alignment(gyro_buffer.mean(axis=0), roll, pitch, lat)
    return np.array([roll, pitch, yaw])

Problem: Altitude diverges when the filter loses GNSS

Solution: the vertical INS channel is inherently unstable. Use a barometer as a fallback altimeter with appropriate process/measurement noise.

Problem: Filter barely responds to GNSS updates

Solution: check the units of GNSSMeasurement.position_cov. Meters on the latitude/longitude diagonal makes the filter treat GNSS as vastly less accurate than it is; always convert with position_std_to_error_state_units.

Best Practices

1. Initialization & Alignment

   • Always perform stationary alignment on level ground

   • Verify heading with compass or known reference

   • Allow 1-2 minute convergence period

2. GNSS/INS Fusion Strategy

   • Use loosely coupled for robustness with standard receivers

   • Use tightly coupled when raw GNSS data available

   • Implement automatic switching based on GNSS signal quality

3. Sensor Fusion Architecture

   • Include barometer for altitude correction

   • Add magnetometer for heading reference

   • Use wheel speed/odometry if available

4. Error Monitoring

   • Check innovation sequences (should be white Gaussian noise)

   • Monitor covariance traces to detect filter divergence

   • Gate measurements with gnss_outage_detection (chi-square test)

5. Real-Time Performance

   • Use fixed-lag smoothing to avoid filter lag

   • Implement output buffering for consistent message rates

   • Profile computation time for embedded systems

6. Sensor Calibration

   • Pre-flight: accelerometer bias estimation from static data

   • In-flight: the filter’s bias states track slow drift

   • Temperature compensation for long mission duration

References & Further Reading

• Groves (2013): “Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems” (the mechanization here follows Ch. 5)

• Titterton & Weston (2004): “Strapdown Inertial Navigation Technology”

• Rogers (2007): “Applied Mathematics in Integrated Navigation Systems”

See Also

• Coordinate Systems Deep Dive - Coordinate transformations for navigation frames

• Astronomical & Celestial Mechanics - Precision reference frames (ECEF/ECI)

• Common Use Cases & Recipes - Ready-to-use Kalman filter implementations

• Troubleshooting Guide - Navigation system diagnostics

• examples/ins_gnss_navigation.py - Full INS/GNSS example

• docs/notebooks/07_ins_gnss_integration.ipynb - Interactive tutorial