INS/GNSS Integration Tutorial

This tutorial demonstrates how to integrate Inertial Navigation System (INS) and Global Navigation Satellite System (GNSS) measurements using loosely and tightly coupled architectures.

INS Basics

Inertial Navigation System Mechanization

INS mechanization propagates position, velocity, and attitude using IMU measurements (accelerometers and gyroscopes).

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

# Initialize INS state
# Position: latitude (rad), longitude (rad), altitude (m)
lat = np.radians(37.0)
lon = np.radians(-122.0)
alt = 100.0

# Initial velocity NED (m/s)
vel_ned = np.array([10.0, 5.0, 0.0])

# Initial attitude: roll, pitch, yaw (rad)
attitude = np.array([0.0, 0.0, np.radians(45.0)])

state = initialize_ins_state(lat, lon, alt, vel_ned, attitude)

# IMU data (gyros in rad/s, accels in m/s^2)
imu = IMUData(
    dt=0.01,
    gyro=np.array([0.001, 0.0, 0.005]),  # Small rotation
    accel=np.array([0.0, 0.0, -9.81])    # Gravity only
)

# Propagate one step
new_state = mechanize_ins_ned(state, imu)

Alignment

Before navigation, the INS must be aligned to determine initial attitude.

Coarse Alignment (stationary):

from pytcl.navigation import coarse_alignment

# Collect static IMU data
static_gyro = np.array([0.0, 0.0, 0.0])  # Earth rate negligible
static_accel = np.array([0.0, 0.0, -9.81])  # Gravity vector

# Compute initial attitude
roll, pitch, yaw = coarse_alignment(static_accel, lat)

Gyrocompass Alignment (finer heading):

from pytcl.navigation import gyrocompass_alignment

# Using Earth rate sensed by gyroscopes
yaw = gyrocompass_alignment(static_gyro, lat)

Loosely-Coupled Integration

In loosely-coupled integration, the GNSS receiver provides position and velocity solutions that are used to update the INS error states.

Initialization

from pytcl.navigation import (
    initialize_ins_gnss, INSGNSSState,
    ins_error_state_matrix, ins_process_noise_matrix
)

# Initial position from GNSS
gnss_pos = np.array([lat, lon, alt])
gnss_vel = np.array([10.0, 5.0, 0.0])

# Initialize integrated state
ins_gnss = initialize_ins_gnss(
    lat=lat, lon=lon, alt=alt,
    vel_ned=gnss_vel,
    attitude=attitude
)

# Error state covariance
P = np.diag([
    10.0, 10.0, 10.0,        # Position errors (m)
    0.1, 0.1, 0.1,           # Velocity errors (m/s)
    np.radians(1)**2,        # Roll error
    np.radians(1)**2,        # Pitch error
    np.radians(5)**2,        # Heading error
    1e-4, 1e-4, 1e-4,        # Accel biases
    1e-5, 1e-5, 1e-5,        # Gyro biases
])

Prediction Step

from pytcl.navigation import loose_coupled_predict
from pytcl.dynamic_estimation import kf_predict

# INS mechanization (predict INS state)
ins_state = mechanize_ins_ned(ins_gnss.ins_state, imu)

# Error state dynamics
dt = imu.dt
F = ins_error_state_matrix(ins_state, dt)
Q = ins_process_noise_matrix(
    dt,
    accel_noise=0.01,    # m/s^2/sqrt(Hz)
    gyro_noise=0.001,    # rad/s/sqrt(Hz)
    accel_bias=1e-6,     # m/s^3
    gyro_bias=1e-7       # rad/s^2
)

# Kalman prediction
dx = np.zeros(15)  # Error state
pred = kf_predict(dx, P, F, Q)
dx, P = pred.x, pred.P

GNSS Update

from pytcl.navigation import (
    loose_coupled_update,
    position_velocity_measurement_matrix
)
from pytcl.dynamic_estimation import kf_update

# GNSS measurement
gnss_pos_meas = np.array([lat + 1e-6, lon + 1e-6, alt + 2.0])
gnss_vel_meas = np.array([10.1, 5.05, 0.1])

# Measurement matrix
H = position_velocity_measurement_matrix()

# Innovation (GNSS - INS)
z_pos = gnss_pos_meas - np.array([
    ins_state.latitude, ins_state.longitude, ins_state.altitude
])
z_vel = gnss_vel_meas - ins_state.velocity
z = np.concatenate([z_pos, z_vel])

# Measurement noise
R = np.diag([2.5, 2.5, 5.0,   # Position (m)
             0.1, 0.1, 0.2])   # Velocity (m/s)

# Kalman update
upd = kf_update(dx, P, z, H, R)
dx, P = upd.x, upd.P

# Apply correction to INS state
result = loose_coupled_update(ins_state, dx)
ins_state = result.corrected_state

Tightly-Coupled Integration

Tightly-coupled integration uses raw GNSS pseudorange and Doppler measurements directly, providing better performance in degraded GNSS environments.

Pseudorange Measurement Model

from pytcl.navigation import (
    compute_line_of_sight, pseudorange_measurement_matrix,
    tight_coupled_pseudorange_innovation, tight_coupled_update,
    SatelliteInfo, GNSSMeasurement
)

# Satellite information (from GNSS receiver)
satellites = [
    SatelliteInfo(
        prn=1,
        x=15600e3, y=7540e3, z=20140e3,  # ECEF position
        vx=-50.0, vy=100.0, vz=20.0,     # ECEF velocity
        clock_bias=1e-6,
        clock_drift=1e-9
    ),
    # ... more satellites
]

# Pseudorange measurements
measurements = [
    GNSSMeasurement(prn=1, pseudorange=22345678.9, doppler=-1234.5),
    # ... more measurements
]

# Compute line-of-sight vectors
user_ecef = geodetic_to_ecef(lat, lon, alt)
los_vectors = [compute_line_of_sight(user_ecef, sat) for sat in satellites]

# Measurement matrix
H = pseudorange_measurement_matrix(los_vectors, len(satellites))

# Innovations
innovations = tight_coupled_pseudorange_innovation(
    ins_state, satellites, measurements
)

# Measurement noise (pseudorange accuracy)
R = np.eye(len(satellites)) * 5.0**2  # 5m pseudorange noise

DOP Computation

from pytcl.navigation import compute_dop, satellite_elevation_azimuth

# Dilution of precision
dop = compute_dop(los_vectors)
print(f"GDOP: {dop.gdop:.2f}")
print(f"PDOP: {dop.pdop:.2f}")
print(f"HDOP: {dop.hdop:.2f}")
print(f"VDOP: {dop.vdop:.2f}")

# Satellite geometry
for sat in satellites:
    el, az = satellite_elevation_azimuth(user_ecef, sat)
    print(f"PRN {sat.prn}: El={np.degrees(el):.1f}°, Az={np.degrees(az):.1f}°")

GNSS Outage Detection

from pytcl.navigation import gnss_outage_detection

# Monitor innovation consistency
innovation_norm = np.linalg.norm(innovations)
expected_norm = np.sqrt(np.trace(H @ P @ H.T + R))

is_outage = gnss_outage_detection(
    innovations, H, P, R, threshold=5.0
)

if is_outage:
    print("GNSS outage detected - using INS-only navigation")

Complete Integration Example

import numpy as np
from pytcl.navigation import (
    initialize_ins_state, mechanize_ins_ned, IMUData,
    ins_error_state_matrix, ins_process_noise_matrix,
    position_velocity_measurement_matrix, loose_coupled_update
)
from pytcl.dynamic_estimation import kf_predict, kf_update

# Simulation parameters
dt = 0.01       # IMU rate: 100 Hz
gnss_rate = 1.0 # GNSS rate: 1 Hz
duration = 60.0 # seconds

np.random.seed(42)

# Initialize
lat, lon, alt = np.radians(37.0), np.radians(-122.0), 100.0
vel = np.array([10.0, 5.0, 0.0])
att = np.array([0.0, 0.0, np.radians(45.0)])

ins_state = initialize_ins_state(lat, lon, alt, vel, att)
dx = np.zeros(15)
P = np.diag([
    10, 10, 10,          # Position
    0.1, 0.1, 0.1,       # Velocity
    0.001, 0.001, 0.01,  # Attitude
    1e-4, 1e-4, 1e-4,    # Accel bias
    1e-5, 1e-5, 1e-5     # Gyro bias
])

# GNSS measurement noise
R = np.diag([2.5, 2.5, 5.0, 0.1, 0.1, 0.2])
H = position_velocity_measurement_matrix()

# Process noise parameters
accel_noise = 0.01
gyro_noise = 0.001

# Simulation loop
time = 0.0
gnss_time = 0.0
trajectory = []

while time < duration:
    # Simulate IMU (with some noise)
    imu = IMUData(
        dt=dt,
        gyro=np.array([0.001, 0.0, 0.005]) + np.random.randn(3) * gyro_noise,
        accel=np.array([0.1, 0.05, -9.81]) + np.random.randn(3) * accel_noise
    )

    # INS mechanization
    ins_state = mechanize_ins_ned(ins_state, imu)

    # Error state prediction
    F = ins_error_state_matrix(ins_state, dt)
    Q = ins_process_noise_matrix(dt, accel_noise, gyro_noise, 1e-6, 1e-7)
    pred = kf_predict(dx, P, F, Q)
    dx, P = pred.x, pred.P

    # GNSS update (at lower rate)
    if time >= gnss_time:
        # Simulated GNSS measurement
        z = np.concatenate([
            np.random.randn(3) * np.array([2.5, 2.5, 5.0]),
            np.random.randn(3) * np.array([0.1, 0.1, 0.2])
        ])

        upd = kf_update(dx, P, z, H, R)
        dx, P = upd.x, upd.P

        # Apply correction
        result = loose_coupled_update(ins_state, dx)
        ins_state = result.corrected_state
        dx = np.zeros(15)  # Reset error state

        gnss_time += gnss_rate

    trajectory.append([
        ins_state.latitude, ins_state.longitude, ins_state.altitude
    ])
    time += dt

trajectory = np.array(trajectory)
print(f"Final position: {np.degrees(trajectory[-1, 0]):.6f}°, "
      f"{np.degrees(trajectory[-1, 1]):.6f}°, {trajectory[-1, 2]:.1f}m")

Next Steps