INS/GNSS Integration: Navigation System Fusion

This notebook covers Inertial Navigation System (INS) and Global Navigation Satellite System (GNSS) integration for navigation and tracking. We explore:

1. INS Fundamentals - Strapdown mechanization and error sources

2. GNSS Measurements - Pseudorange and position solutions

3. Loosely-Coupled Integration - Position/velocity updates

4. Tightly-Coupled Integration - Pseudorange updates

5. Dilution of Precision - Geometry effects on accuracy

Prerequisites

pip install nrl-tracker plotly numpy

import numpy as np
import plotly.graph_objects as go
from plotly.subplots import make_subplots

from pytcl.navigation import (
    # Constants
    OMEGA_EARTH, A_EARTH, F_EARTH,
    # INS
    INSState, IMUData, INSErrorState,
    mechanize_ins_ned, initialize_ins_state,
    gravity_ned, earth_rate_ned, transport_rate_ned,
    radii_of_curvature, coarse_alignment,
    coning_correction, sculling_correction,
    # GNSS
    SPEED_OF_LIGHT, GPS_L1_FREQ,
    SatelliteInfo, GNSSMeasurement,
    compute_dop, compute_line_of_sight,
    satellite_elevation_azimuth,
    # Integration
    initialize_ins_gnss, loose_coupled_predict,
    loose_coupled_update, tight_coupled_update,
    # Geodesy
    geodetic_to_ecef, ecef_to_geodetic,
    ecef_to_ned, ned_to_ecef,
)
from pytcl.coordinate_systems import euler2rotmat, rotmat2euler

np.random.seed(42)

# Plotly dark theme template
dark_template = go.layout.Template()
dark_template.layout = go.Layout(
    paper_bgcolor='#0d1117',
    plot_bgcolor='#0d1117',
    font=dict(color='#e6edf3'),
    xaxis=dict(gridcolor='#30363d', zerolinecolor='#30363d'),
    yaxis=dict(gridcolor='#30363d', zerolinecolor='#30363d'),
)

# Set up initial navigation parameters
# Initial position: Washington DC
lat0 = np.radians(38.9)
lon0 = np.radians(-77.0)
alt0 = 100.0  # meters

# Initial velocity: moving northeast at 10 m/s
v_north = 7.0
v_east = 7.0
v_down = 0.0

# Initial attitude (roll, pitch, yaw)
roll0 = np.radians(0)
pitch0 = np.radians(0)
yaw0 = np.radians(45)  # Heading NE

print("Initial Navigation Setup")
print("=" * 50)
print(f"Position: {np.degrees(lat0):.4f}°N, {np.degrees(lon0):.4f}°E, {alt0:.1f}m")
print(f"Velocity: N={v_north:.1f}, E={v_east:.1f}, D={v_down:.1f} m/s")
print(f"Attitude: Roll={np.degrees(roll0):.1f}°, Pitch={np.degrees(pitch0):.1f}°, Yaw={np.degrees(yaw0):.1f}°")

Initial Navigation Setup
==================================================
Position: 38.9000°N, -77.0000°E, 100.0m
Velocity: N=7.0, E=7.0, D=0.0 m/s
Attitude: Roll=0.0°, Pitch=0.0°, Yaw=45.0°

1. INS Fundamentals

Inertial Navigation Concept

An INS uses:

• Accelerometers: Measure specific force (acceleration minus gravity)

• Gyroscopes: Measure angular rate

By integrating these measurements:

1. Integrate angular rates → Attitude

2. Remove gravity → Acceleration

3. Integrate acceleration → Velocity

4. Integrate velocity → Position

Error Sources

Error Type	Effect	Typical Value
Gyro bias	Position error grows as \(t^2\)	0.01-1 °/hr
Accel bias	Position error grows as \(t^2\)	10-1000 µg
Gyro noise	Attitude random walk	0.001-0.1 °/√hr
Accel noise	Velocity random walk	10-100 µg/√Hz

# Get gravity at this location
g = gravity_ned(lat0, alt0)
print(f"Gravity at lat={np.degrees(lat0):.1f}°: {g[2]:.4f} m/s²")

# Get radii of curvature
R_M, R_N = radii_of_curvature(lat0)
print(f"Meridional radius: {R_M/1e6:.3f} million m")
print(f"Prime vertical radius: {R_N/1e6:.3f} million m")

# Earth rotation rate in NED frame
omega_ie = earth_rate_ned(lat0)
print(f"\nEarth rate vertical component: {omega_ie[2] * 3600 * np.degrees(1):.4f} °/hr")

# Transport rate due to motion over curved Earth
omega_en = transport_rate_ned(lat0, alt0, v_north, v_east)
print(f"Transport rate vertical component: {omega_en[2] * 3600 * np.degrees(1):.4f} °/hr")

Gravity at lat=38.9°: 9.8004 m/s²
Meridional radius: 6.361 million m
Prime vertical radius: 6.387 million m

Earth rate vertical component: -9.4452 °/hr
Transport rate vertical component: -0.1824 °/hr

# Initialize INS state
ins_state = initialize_ins_state(
    lat=lat0, lon=lon0, alt=alt0,
    vN=v_north, vE=v_east, vD=v_down,
    roll=roll0, pitch=pitch0, yaw=yaw0
)

print("Initial INS State")
print("=" * 50)
print(f"Position: {np.degrees(ins_state.latitude):.6f}°N, {np.degrees(ins_state.longitude):.6f}°E, {ins_state.altitude:.1f}m")
print(f"Velocity: N={ins_state.velocity_north:.2f}, E={ins_state.velocity_east:.2f}, D={ins_state.velocity_down:.2f} m/s")
euler = ins_state.euler_angles()
print(f"Attitude: Roll={np.degrees(euler[0]):.1f}°, Pitch={np.degrees(euler[1]):.1f}°, Yaw={np.degrees(euler[2]):.1f}°")

Initial INS State
==================================================
Position: 38.900000°N, -77.000000°E, 100.0m
Velocity: N=7.00, E=7.00, D=0.00 m/s
Attitude: Roll=0.0°, Pitch=-0.0°, Yaw=45.0°

# Simulate INS propagation with sensor errors
dt = 0.01  # 100 Hz IMU
duration = 60.0  # 60 seconds
n_steps = int(duration / dt)

# IMU error parameters
gyro_bias = np.array([0.01, -0.02, 0.015]) * np.pi / 180 / 3600  # rad/s
accel_bias = np.array([50, -30, 20]) * 1e-6 * 9.81  # m/s²
gyro_noise = 0.005 * np.pi / 180  # rad/s
accel_noise = 100 * 1e-6 * 9.81  # m/s²

# Storage
positions = []
velocities = []
attitudes = []
times = []

# True trajectory (for comparison)
true_positions = []
true_lat, true_lon, true_alt = lat0, lon0, alt0
true_vn, true_ve, true_vd = v_north, v_east, v_down

# Reset INS state
ins_state = initialize_ins_state(
    lat=lat0, lon=lon0, alt=alt0,
    vN=v_north, vE=v_east, vD=v_down,
    roll=roll0, pitch=pitch0, yaw=yaw0
)

for i in range(n_steps):
    t = i * dt

    # True IMU outputs (straight line motion, no rotation)
    # Specific force = -gravity in body frame for level flight
    true_accel = ins_state.dcm @ np.array([0, 0, gravity_ned(true_lat, true_alt)[2]])  # Body frame
    true_gyro = np.zeros(3)  # No rotation

    # Add biases and noise to simulate real IMU
    accel = true_accel + accel_bias + np.random.randn(3) * accel_noise
    gyro = true_gyro + gyro_bias + np.random.randn(3) * gyro_noise

    # Create IMU data
    imu = IMUData(
        accel=accel,
        gyro=gyro,
        dt=dt
    )

    # Mechanize (propagate INS)
    ins_state = mechanize_ins_ned(ins_state, imu)

    # True trajectory (perfect navigation)
    R_M, R_N = radii_of_curvature(true_lat)
    true_lat += true_vn * dt / (R_M + true_alt)
    true_lon += true_ve * dt / ((R_N + true_alt) * np.cos(true_lat))
    true_alt -= true_vd * dt

    # Store results
    if i % 10 == 0:  # Store every 10th point
        positions.append([ins_state.latitude, ins_state.longitude, ins_state.altitude])
        velocities.append([ins_state.velocity_north, ins_state.velocity_east, ins_state.velocity_down])
        euler = ins_state.euler_angles()
        attitudes.append(np.degrees(euler))
        times.append(t)
        true_positions.append([true_lat, true_lon, true_alt])

positions = np.array(positions)
velocities = np.array(velocities)
attitudes = np.array(attitudes)
true_positions = np.array(true_positions)

print(f"Propagated INS for {duration:.0f} seconds")
print(f"Final position error: {np.degrees(positions[-1, 0] - true_positions[-1, 0])*111000:.1f}m N, "
      f"{np.degrees(positions[-1, 1] - true_positions[-1, 1])*111000*np.cos(lat0):.1f}m E")

Propagated INS for 60 seconds
Final position error: -13.7m N, 117.6m E

# Visualize INS drift
fig = make_subplots(rows=2, cols=2,
                    subplot_titles=['INS Position Error (no GNSS)', 'INS Velocity Error',
                                   'INS Attitude', 'Position Error Trajectory'])

# Position error
north_error = (positions[:, 0] - true_positions[:, 0]) * (R_M + alt0)
east_error = (positions[:, 1] - true_positions[:, 1]) * (R_N + alt0) * np.cos(lat0)
fig.add_trace(
    go.Scatter(x=times, y=north_error, mode='lines', name='North',
               line=dict(color='#00d4ff', width=2)),
    row=1, col=1
)
fig.add_trace(
    go.Scatter(x=times, y=east_error, mode='lines', name='East',
               line=dict(color='#ff4757', width=2)),
    row=1, col=1
)

# Velocity error
fig.add_trace(
    go.Scatter(x=times, y=velocities[:, 0] - v_north, mode='lines', name='North V',
               line=dict(color='#00d4ff', width=2), showlegend=False),
    row=1, col=2
)
fig.add_trace(
    go.Scatter(x=times, y=velocities[:, 1] - v_east, mode='lines', name='East V',
               line=dict(color='#ff4757', width=2), showlegend=False),
    row=1, col=2
)

# Attitude
fig.add_trace(
    go.Scatter(x=times, y=attitudes[:, 2], mode='lines', name='Roll',
               line=dict(color='#00ff88', width=2)),
    row=2, col=1
)
fig.add_trace(
    go.Scatter(x=times, y=attitudes[:, 1], mode='lines', name='Pitch',
               line=dict(color='#00d4ff', width=2)),
    row=2, col=1
)
fig.add_trace(
    go.Scatter(x=times, y=attitudes[:, 0], mode='lines', name='Yaw',
               line=dict(color='#ff4757', width=2)),
    row=2, col=1
)

# Trajectory
fig.add_trace(
    go.Scatter(x=east_error, y=north_error, mode='lines',
               line=dict(color='#00d4ff', width=2), name='Error path', showlegend=False),
    row=2, col=2
)
fig.add_trace(
    go.Scatter(x=[0], y=[0], mode='markers', name='Start',
               marker=dict(color='#00ff88', size=12)),
    row=2, col=2
)
fig.add_trace(
    go.Scatter(x=[east_error[-1]], y=[north_error[-1]], mode='markers', name='End',
               marker=dict(color='#ff4757', size=12, symbol='square')),
    row=2, col=2
)

fig.update_xaxes(title_text='Time (s)', row=1, col=1)
fig.update_yaxes(title_text='Position Error (m)', row=1, col=1)
fig.update_xaxes(title_text='Time (s)', row=1, col=2)
fig.update_yaxes(title_text='Velocity Error (m/s)', row=1, col=2)
fig.update_xaxes(title_text='Time (s)', row=2, col=1)
fig.update_yaxes(title_text='Angle (deg)', row=2, col=1)
fig.update_xaxes(title_text='East Error (m)', row=2, col=2)
fig.update_yaxes(title_text='North Error (m)', row=2, col=2, scaleanchor='x4', scaleratio=1)

fig.update_layout(template=dark_template, height=700)
fig.show()

Data type cannot be displayed: application/vnd.plotly.v1+json

2. GNSS Measurements and Geometry

GNSS Measurement Model

The pseudorange measurement is:

\[\rho = r + c\cdot\delta t_{rx} + \epsilon\]

Where:

• \(r\) = geometric range to satellite

• \(c\) = speed of light

• \(\delta t_{rx}\) = receiver clock bias

• \(\epsilon\) = measurement noise + atmospheric delays

Dilution of Precision (DOP)

DOP Type	Meaning
GDOP	Geometric (3D position + time)
PDOP	Position (3D)
HDOP	Horizontal (2D)
VDOP	Vertical
TDOP	Time

# Simulate satellite constellation
# Generate satellites at various azimuths and elevations
n_sats = 8
sat_azimuths = np.linspace(0, 2*np.pi, n_sats, endpoint=False)
sat_elevations = np.array([45, 30, 60, 25, 55, 35, 50, 40]) * np.pi / 180

# Convert to ECEF positions (assume ~20,000 km altitude)
sat_altitude = 20200e3  # GPS orbit altitude
user_ecef = np.array(geodetic_to_ecef(lat0, lon0, alt0))

# Simple satellite record (lightweight data container)
class SimpleSatellite:
    def __init__(self, prn, x, y, z):
        self.prn = prn
        self.x = x
        self.y = y
        self.z = z

satellites = []
for i in range(n_sats):
    # Unit vector from user to satellite in NED
    az = sat_azimuths[i]
    el = sat_elevations[i]

    # NED components
    north = np.cos(el) * np.cos(az)
    east = np.cos(el) * np.sin(az)
    down = -np.sin(el)  # Negative because sat is up

    # Convert to ECEF
    ecef_unit = np.array(ned_to_ecef(north, east, down, lat_ref=lat0, lon_ref=lon0, alt_ref=alt0))
    sat_ecef = user_ecef + sat_altitude * ecef_unit

    satellites.append(SimpleSatellite(
        prn=i+1,
        x=sat_ecef[0],
        y=sat_ecef[1],
        z=sat_ecef[2]
    ))

print(f"Simulated {n_sats} GPS satellites")
print("\nSatellite elevation and azimuth:")
for sat in satellites:
    # Compute elevation and azimuth
    sat_pos = np.array([sat.x, sat.y, sat.z])
    el, az = satellite_elevation_azimuth(user_ecef, sat_pos)
    print(f"  PRN {sat.prn:2d}: El={np.degrees(el):5.1f}°, Az={np.degrees(az):6.1f}°")

Simulated 8 GPS satellites

Satellite elevation and azimuth:
  PRN  1: El= 46.0°, Az=  51.3°
  PRN  2: El= 46.0°, Az=  51.3°
  PRN  3: El= 46.0°, Az=  51.3°
  PRN  4: El= 46.0°, Az=  51.3°
  PRN  5: El= 46.0°, Az=  51.3°
  PRN  6: El= 46.0°, Az=  51.3°
  PRN  7: El= 46.0°, Az=  51.3°
  PRN  8: El= 46.0°, Az=  51.3°

# Compute DOP values
# Build geometry matrix (line-of-sight unit vectors + clock column)
H = np.zeros((len(satellites), 4))
for i, sat in enumerate(satellites):
    sat_ecef_pos = np.array([sat.x, sat.y, sat.z])
    los = sat_ecef_pos - user_ecef
    range_val = np.linalg.norm(los)
    H[i, :3] = -los / range_val  # Unit vector toward satellite
    H[i, 3] = 1.0  # Clock column

gdop, pdop, hdop, vdop = compute_dop(H)

print("Dilution of Precision")
print("=" * 40)
print(f"GDOP (Geometric):   {gdop:.2f}")
print(f"PDOP (Position):    {pdop:.2f}")
print(f"HDOP (Horizontal):  {hdop:.2f}")
print(f"VDOP (Vertical):    {vdop:.2f}")

# Position accuracy estimate
sigma_uere = 3.0  # User Equivalent Range Error (m)
print(f"\nPosition Accuracy (1σ) with UERE={sigma_uere}m:")
print(f"  Horizontal: {hdop * sigma_uere:.1f} m")
print(f"  Vertical:   {vdop * sigma_uere:.1f} m")
print(f"  3D:         {pdop * sigma_uere:.1f} m")

Dilution of Precision
========================================
GDOP (Geometric):   86672333.73
PDOP (Position):    61442924.29
HDOP (Horizontal):  47370972.88
VDOP (Vertical):    39130855.77

Position Accuracy (1σ) with UERE=3.0m:
  Horizontal: 142112918.6 m
  Vertical:   117392567.3 m
  3D:         184328772.9 m

# Visualize satellite geometry
fig = make_subplots(rows=1, cols=2,
                    specs=[[{'type': 'polar'}, {'type': 'scene'}]],
                    subplot_titles=[f'Sky Plot (HDOP={hdop:.2f}, VDOP={vdop:.2f})',
                                   'Satellite Geometry'])

# Sky plot
for sat in satellites:
    el, az = satellite_elevation_azimuth([lat0, lon0, alt0], [sat.x, sat.y, sat.z])
    r = 90 - np.degrees(el)  # Elevation inverted for polar plot
    fig.add_trace(
        go.Scatterpolar(r=[r], theta=[np.degrees(az)], mode='markers+text',
                        marker=dict(size=12), text=[str(sat.prn)],
                        textposition='top center', name=f'PRN {sat.prn}'),
        row=1, col=1
    )

3. Loosely-Coupled Integration

In loosely-coupled integration:

• GNSS provides position/velocity solutions

• INS provides attitude and high-rate navigation

• Kalman filter combines both

Error State Model

The error state vector is:

\[\delta\mathbf{x} = [\delta\phi_n, \delta\phi_e, \delta\phi_d, \delta v_n, \delta v_e, \delta v_d, \delta\text{lat}, \delta\text{lon}, \delta\text{alt}, b_{g_x}, b_{g_y}, b_{g_z}, b_{a_x}, b_{a_y}, b_{a_z}]^T\]

# Simulate integrated INS/GNSS navigation
dt_imu = 0.01  # 100 Hz IMU
dt_gnss = 1.0  # 1 Hz GNSS
duration = 120.0  # 2 minutes

n_imu_steps = int(duration / dt_imu)
gnss_interval = int(dt_gnss / dt_imu)

# Error state covariance
P = np.diag([
    np.radians(1)**2, np.radians(1)**2, np.radians(1)**2,  # Attitude (rad²)
    0.1**2, 0.1**2, 0.1**2,  # Velocity (m/s)²
    (10/R_M)**2, (10/R_N/np.cos(lat0))**2, 10**2,  # Position (rad², m²)
    (0.01*np.pi/180/3600)**2,  # Gyro bias x
    (0.01*np.pi/180/3600)**2,  # Gyro bias y
    (0.01*np.pi/180/3600)**2,  # Gyro bias z
    (100e-6*9.81)**2,  # Accel bias x
    (100e-6*9.81)**2,  # Accel bias y
    (100e-6*9.81)**2,  # Accel bias z
])

# Process noise
Q = np.diag([
    (0.001*np.pi/180)**2,  # Gyro noise
    (0.001*np.pi/180)**2,
    (0.001*np.pi/180)**2,
    (100e-6*9.81)**2,  # Accel noise
    (100e-6*9.81)**2,
    (100e-6*9.81)**2,
    1e-20, 1e-20, 1e-20,  # Position (small)
    (0.0001*np.pi/180/3600)**2,  # Gyro bias stability
    (0.0001*np.pi/180/3600)**2,
    (0.0001*np.pi/180/3600)**2,
    (1e-6*9.81)**2,  # Accel bias stability
    (1e-6*9.81)**2,
    (1e-6*9.81)**2,
]) * dt_imu

# GNSS measurement noise
R_gnss = np.diag([
    (3.0/R_M)**2,  # Latitude (3m)
    (3.0/R_N/np.cos(lat0))**2,  # Longitude (3m)
    5.0**2,  # Altitude (5m)
    0.1**2, 0.1**2, 0.1**2,  # Velocity (0.1 m/s)
])

print("Filter initialized")
print(f"State dimension: {len(P)}")
print(f"GNSS update rate: {1/dt_gnss:.0f} Hz")
print(f"IMU rate: {1/dt_imu:.0f} Hz")

Filter initialized
State dimension: 15
GNSS update rate: 1 Hz
IMU rate: 100 Hz

# Run integrated navigation
ins_state = initialize_ins_state(
    lat=lat0, lon=lon0, alt=alt0,
    vN=v_north, vE=v_east, vD=v_down,
    roll=roll0, pitch=pitch0, yaw=yaw0
)

# Error state
error_state = np.zeros(15)

# Storage
integrated_positions = []
ins_only_positions = []
gnss_positions = []
covariance_trace = []
times = []

# True trajectory
true_lat, true_lon, true_alt = lat0, lon0, alt0
true_vn, true_ve, true_vd = v_north, v_east, v_down

# INS-only state for comparison
ins_only_state = initialize_ins_state(
    lat=lat0, lon=lon0, alt=alt0,
    vN=v_north, vE=v_east, vD=v_down,
    roll=roll0, pitch=pitch0, yaw=yaw0
)

for i in range(n_imu_steps):
    t = i * dt_imu

    # Simulate true trajectory (constant velocity)
    R_M_t, R_N_t = radii_of_curvature(true_lat)
    true_lat += true_vn * dt_imu / (R_M_t + true_alt)
    true_lon += true_ve * dt_imu / ((R_N_t + true_alt) * np.cos(true_lat))
    true_alt -= true_vd * dt_imu

    # Generate IMU data with errors
    true_accel = ins_state.dcm @ np.array([0, 0, gravity_ned(true_lat, true_alt)[2]])
    true_gyro = np.zeros(3)
    accel = true_accel + accel_bias + np.random.randn(3) * accel_noise
    gyro = true_gyro + gyro_bias + np.random.randn(3) * gyro_noise

    imu = IMUData(accel=accel, gyro=gyro, dt=dt_imu)

    # INS mechanization
    ins_state = mechanize_ins_ned(ins_state, imu)
    ins_only_state = mechanize_ins_ned(ins_only_state, imu)

    # Predict covariance
    P = P + Q  # Simplified prediction

    # GNSS update
    if i > 0 and i % gnss_interval == 0:
        # Simulate GNSS measurement with noise
        gnss_lat = true_lat + np.random.randn() * 3.0 / R_M_t
        gnss_lon = true_lon + np.random.randn() * 3.0 / (R_N_t * np.cos(true_lat))
        gnss_alt = true_alt + np.random.randn() * 5.0
        gnss_vn = true_vn + np.random.randn() * 0.1
        gnss_ve = true_ve + np.random.randn() * 0.1
        gnss_vd = true_vd + np.random.randn() * 0.1

        # Innovation
        z = np.array([
            gnss_lat - ins_state.latitude,
            gnss_lon - ins_state.longitude,
            gnss_alt - ins_state.altitude,
            gnss_vn - ins_state.velocity_north,
            gnss_ve - ins_state.velocity_east,
            gnss_vd - ins_state.velocity_down,
        ])

        # Measurement matrix (simplified)
        H = np.zeros((6, 15))
        H[0, 6] = 1  # lat
        H[1, 7] = 1  # lon
        H[2, 8] = 1  # alt
        H[3, 3] = 1  # v_n
        H[4, 4] = 1  # v_e
        H[5, 5] = 1  # v_d

        # Kalman gain
        S = H @ P @ H.T + R_gnss
        K = P @ H.T @ np.linalg.inv(S)

        # Update error state
        error_state = error_state + K @ z
        P = (np.eye(15) - K @ H) @ P

        # Store state estimates
        integrated_positions.append([ins_state.latitude, ins_state.longitude, ins_state.altitude])
        ins_only_positions.append([ins_only_state.latitude, ins_only_state.longitude, ins_only_state.altitude])
        gnss_positions.append([gnss_lat, gnss_lon, gnss_alt])
        covariance_trace.append(np.trace(P[:3, :3]))
        times.append(t)

integrated_positions = np.array(integrated_positions)
ins_only_positions = np.array(ins_only_positions)
gnss_positions = np.array(gnss_positions)
covariance_trace = np.array(covariance_trace)

print(f"Integrated navigation complete")
print(f"Final integrated position error: {np.degrees(integrated_positions[-1, 0] - true_lat)*111000:.1f}m N, {np.degrees(integrated_positions[-1, 1] - true_lon)*111000*np.cos(lat0):.1f}m E")

Integrated navigation complete
Final integrated position error: -114.0m N, 931.2m E

# Visualize integration results
fig = make_subplots(rows=2, cols=2,
                    subplot_titles=['Trajectory Comparison', 'Position Error Comparison',
                                   'Filter Covariance', 'Final Position Error Distribution'])

# Position comparison
true_north = np.array(times) * v_north
true_east = np.array(times) * v_east

integrated_north = (integrated_positions[:, 0] - lat0) * (R_M + alt0)
integrated_east = (integrated_positions[:, 1] - lon0) * (R_N + alt0) * np.cos(lat0)
ins_only_north = (ins_only_positions[:, 0] - lat0) * (R_M + alt0)
ins_only_east = (ins_only_positions[:, 1] - lon0) * (R_N + alt0) * np.cos(lat0)

fig.add_trace(
    go.Scatter(x=true_east, y=true_north, mode='lines', name='True',
               line=dict(color='#00ff88', width=2)),
    row=1, col=1
)
fig.add_trace(
    go.Scatter(x=ins_only_east, y=ins_only_north, mode='lines', name='INS Only',
               line=dict(color='#ff4757', width=1, dash='dash'), opacity=0.7),
    row=1, col=1
)
fig.add_trace(
    go.Scatter(x=integrated_east, y=integrated_north, mode='lines', name='INS/GNSS',
               line=dict(color='#00d4ff', width=1.5)),
    row=1, col=1
)
if len(gnss_positions) > 0:
    gnss_north = (gnss_positions[:, 0] - lat0) * (R_M + alt0)
    gnss_east = (gnss_positions[:, 1] - lon0) * (R_N + alt0) * np.cos(lat0)
    fig.add_trace(
        go.Scatter(x=gnss_east, y=gnss_north, mode='markers', name='GNSS',
                   marker=dict(color='#ffb800', size=4, opacity=0.5)),
        row=1, col=1
    )

# Position error
int_error_n = integrated_north - true_north
int_error_e = integrated_east - true_east
ins_error_n = ins_only_north - true_north
ins_error_e = ins_only_east - true_east

fig.add_trace(
    go.Scatter(x=times, y=np.sqrt(int_error_n**2 + int_error_e**2), mode='lines',
               name='INS/GNSS', line=dict(color='#00d4ff', width=2), showlegend=False),
    row=1, col=2
)
fig.add_trace(
    go.Scatter(x=times, y=np.sqrt(ins_error_n**2 + ins_error_e**2), mode='lines',
               name='INS Only', line=dict(color='#ff4757', width=1, dash='dash'),
               opacity=0.7, showlegend=False),
    row=1, col=2
)

# Covariance
fig.add_trace(
    go.Scatter(x=times, y=covariance_trace, mode='lines',
               line=dict(color='#00d4ff', width=2), showlegend=False),
    row=2, col=1
)

# Error histogram
final_errors = np.sqrt(int_error_n[-100:]**2 + int_error_e[-100:]**2)
fig.add_trace(
    go.Histogram(x=final_errors, nbinsx=20, name='Error dist',
                 marker=dict(color='#00d4ff'), showlegend=False),
    row=2, col=2
)
fig.add_vline(x=np.mean(final_errors), line=dict(color='#ff4757', dash='dash'),
              annotation_text=f'Mean: {np.mean(final_errors):.2f}m', row=2, col=2)

fig.update_xaxes(title_text='East (m)', row=1, col=1)
fig.update_yaxes(title_text='North (m)', row=1, col=1, scaleanchor='x', scaleratio=1)
fig.update_xaxes(title_text='Time (s)', row=1, col=2)
fig.update_yaxes(title_text='Horizontal Error (m)', row=1, col=2)
fig.update_xaxes(title_text='Time (s)', row=2, col=1)
fig.update_yaxes(title_text='Covariance Trace', type='log', row=2, col=1)
fig.update_xaxes(title_text='Position Error (m)', row=2, col=2)
fig.update_yaxes(title_text='Count', row=2, col=2)

fig.update_layout(template=dark_template, height=700)
fig.show()

print(f"Final INS-only error: {np.sqrt(ins_error_n[-1]**2 + ins_error_e[-1]**2):.1f} m")
print(f"Final INS/GNSS error: {np.sqrt(int_error_n[-1]**2 + int_error_e[-1]**2):.1f} m")

Data type cannot be displayed: application/vnd.plotly.v1+json

Final INS-only error: 948.2 m
Final INS/GNSS error: 948.2 m

4. GNSS Outage Handling

A key advantage of INS/GNSS integration is graceful degradation during GNSS outages.

# Simulate navigation with GNSS outage
outage_start = 30.0  # seconds
outage_duration = 30.0  # 30 second outage

# Re-run with outage
ins_state = initialize_ins_state(
    lat=lat0, lon=lon0, alt=alt0,
    vN=v_north, vE=v_east, vD=v_down,
    roll=roll0, pitch=pitch0, yaw=yaw0
)

P = np.diag([
    np.radians(1)**2, np.radians(1)**2, np.radians(1)**2,
    0.1**2, 0.1**2, 0.1**2,
    (10/R_M)**2, (10/R_N/np.cos(lat0))**2, 10**2,
    (0.01*np.pi/180/3600)**2, (0.01*np.pi/180/3600)**2, (0.01*np.pi/180/3600)**2,
    (100e-6*9.81)**2, (100e-6*9.81)**2, (100e-6*9.81)**2,
])

error_state = np.zeros(15)

outage_positions = []
outage_cov = []
outage_times = []
gnss_available = []

true_lat, true_lon, true_alt = lat0, lon0, alt0

for i in range(n_imu_steps):
    t = i * dt_imu

    # True trajectory
    R_M_t, R_N_t = radii_of_curvature(true_lat)
    true_lat += true_vn * dt_imu / (R_M_t + true_alt)
    true_lon += true_ve * dt_imu / ((R_N_t + true_alt) * np.cos(true_lat))

    # IMU data
    true_accel = ins_state.dcm @ np.array([0, 0, gravity_ned(true_lat, true_alt)[2]])
    accel = true_accel + accel_bias + np.random.randn(3) * accel_noise
    gyro = gyro_bias + np.random.randn(3) * gyro_noise
    imu = IMUData(accel=accel, gyro=gyro, dt=dt_imu)

    # INS mechanization
    ins_state = mechanize_ins_ned(ins_state, imu)
    P = P + Q

    # GNSS update (with outage)
    gnss_ok = not (outage_start <= t < outage_start + outage_duration)

    if i > 0 and i % gnss_interval == 0 and gnss_ok:
        gnss_lat = true_lat + np.random.randn() * 3.0 / R_M_t
        gnss_lon = true_lon + np.random.randn() * 3.0 / (R_N_t * np.cos(true_lat))
        gnss_alt = true_alt + np.random.randn() * 5.0

        z = np.array([
            gnss_lat - ins_state.latitude,
            gnss_lon - ins_state.longitude,
            gnss_alt - ins_state.altitude,
            true_vn + np.random.randn() * 0.1 - ins_state.velocity_north,
            true_ve + np.random.randn() * 0.1 - ins_state.velocity_east,
            np.random.randn() * 0.1 - ins_state.velocity_down,
        ])

        H = np.zeros((6, 15))
        H[0, 6] = 1; H[1, 7] = 1; H[2, 8] = 1
        H[3, 3] = 1; H[4, 4] = 1; H[5, 5] = 1

        S = H @ P @ H.T + R_gnss
        K = P @ H.T @ np.linalg.inv(S)
        error_state = error_state + K @ z
        P = (np.eye(15) - K @ H) @ P

        # Reconstruct INSState with corrected fields
        new_position = ins_state.position + error_state[6:9]
        new_velocity = ins_state.velocity + error_state[3:6]
        ins_state = INSState(
            position=new_position,
            velocity=new_velocity,
            quaternion=ins_state.quaternion,
            time=ins_state.time,
        )
        error_state = np.zeros(15)

    if i % 10 == 0:
        outage_positions.append([ins_state.latitude, ins_state.longitude, ins_state.altitude])
        outage_cov.append(np.sqrt(P[6, 6]) * R_M)  # Position uncertainty in meters
        outage_times.append(t)
        gnss_available.append(gnss_ok)

outage_positions = np.array(outage_positions)

print(f"GNSS outage trial complete ({outage_duration:.0f}s outage)")
print(f"Position covariance increased by {outage_cov[-1] / outage_cov[0]:.0f}x during outage")

GNSS outage trial complete (30s outage)
Position covariance increased by 0x during outage

# Visualize outage behavior
fig = make_subplots(rows=2, cols=1,
                    subplot_titles=['Navigation Error During GNSS Outage',
                                   'Trajectory During GNSS Outage'],
                    vertical_spacing=0.12)

# Position error during outage
outage_north = (outage_positions[:, 0] - lat0) * (R_M + alt0)
outage_east = (outage_positions[:, 1] - lon0) * (R_N + alt0) * np.cos(lat0)
true_north = np.array(outage_times) * v_north
true_east = np.array(outage_times) * v_east

error_north = outage_north - true_north
error_east = outage_east - true_east
total_error = np.sqrt(error_north**2 + error_east**2)

# Outage shading
outage_times_arr = np.array(outage_times)
gnss_available_arr = np.array(gnss_available)
outage_mask = ~gnss_available_arr
if np.any(outage_mask):
    # Find outage start and end indices
    outage_indices = np.where(outage_mask)[0]
    if len(outage_indices) > 0:
        fig.add_vrect(x0=outage_times_arr[outage_indices[0]],
                      x1=outage_times_arr[outage_indices[-1]],
                      fillcolor='rgba(255, 71, 87, 0.2)', line_width=0,
                      row=1, col=1)

fig.add_trace(
    go.Scatter(x=outage_times, y=total_error, mode='lines', name='Position Error',
               line=dict(color='#00d4ff', width=2)),
    row=1, col=1
)
fig.add_trace(
    go.Scatter(x=outage_times, y=outage_cov, mode='lines', name='1σ Uncertainty',
               line=dict(color='#00ff88', width=1.5, dash='dash')),
    row=1, col=1
)

# Trajectory with color-coded GNSS availability
fig.add_trace(
    go.Scatter(x=true_east, y=true_north, mode='lines', name='True',
               line=dict(color='#00ff88', width=2)),
    row=2, col=1
)

# Split trajectory by GNSS availability
gnss_avail_x, gnss_avail_y = [], []
gnss_outage_x, gnss_outage_y = [], []

for i in range(len(outage_times)):
    if gnss_available[i]:
        gnss_avail_x.append(outage_east[i])
        gnss_avail_y.append(outage_north[i])
    else:
        gnss_outage_x.append(outage_east[i])
        gnss_outage_y.append(outage_north[i])

fig.add_trace(
    go.Scatter(x=gnss_avail_x, y=gnss_avail_y, mode='markers', name='GNSS Available',
               marker=dict(color='#00d4ff', size=3)),
    row=2, col=1
)
fig.add_trace(
    go.Scatter(x=gnss_outage_x, y=gnss_outage_y, mode='markers', name='GNSS Outage',
               marker=dict(color='#ff4757', size=3)),
    row=2, col=1
)

fig.update_xaxes(title_text='Time (s)', row=1, col=1, range=[0, duration])
fig.update_yaxes(title_text='Error (m)', row=1, col=1)
fig.update_xaxes(title_text='East (m)', row=2, col=1)
fig.update_yaxes(title_text='North (m)', row=2, col=1, scaleanchor='x2', scaleratio=1)

fig.update_layout(template=dark_template, height=700)
fig.show()

# Find max error during outage
max_outage_error = np.max(total_error[outage_mask])
print(f"\nMax error during {outage_duration:.0f}s outage: {max_outage_error:.1f} m")
print(f"Error growth rate: {max_outage_error/outage_duration:.2f} m/s")

Data type cannot be displayed: application/vnd.plotly.v1+json

Max error during 30s outage: 70.3 m
Error growth rate: 2.34 m/s

Summary & Learning Path

INS/GNSS Integration Overview

Core Disciplines:

1. Inertial Navigation - Physics of accelerometer & gyro integration

2. GNSS Positioning - Satellite pseudorange equations and DOP

3. Kalman Filtering - Optimal state estimation

4. Error Modeling - IMU biases, noise, GNSS multipath

5. Sensor Fusion - Loosely-coupled vs Tightly-coupled vs Ultra-tight

Integration Architecture Comparison

Aspect	Loosely-Coupled	Tightly-Coupled	Ultra-Tight
Implementation	Simpler	Complex	Very Complex
Min satellites	4	1	0.5 (predicted)
Update rate	1-10 Hz	1000+ Hz	Continuous
Outage performance	Good (2-3 min)	Excellent (5-10 min)	Best (>30 min)
Urban canyon	Poor	Better	Best
Computation	2-3 ms	50-100 ms	100+ ms

4-Week Learning Curriculum

Week 1: INS Fundamentals

• Day 1-2: Coordinate frames (ECEF, NED, body), direction cosine matrices

• Day 3-4: Coriolis effect, transport rate, gravity anomaly modeling

• Day 5: IMU mechanization, specific force decomposition, attitude propagation

Week 2: GNSS & Sensor Fusion

• Day 1-2: GNSS pseudorange equations, dilution of precision (DOP)

• Day 3-4: Kalman filter architecture, measurement equation design

• Day 5: Loosely-coupled architecture: position/velocity measurement model

Week 3: Error Modeling & Tightly-Coupled

• Day 1-2: IMU bias modeling, Allan variance, random walk processes

• Day 3-4: Receiver clock bias, atmospheric delays

• Day 5: Tightly-coupled architecture: pseudorange residuals as measurements

Week 4: Advanced Integration & Applications

• Day 1-2: GNSS denial/outage scenarios, INS mechanization during outage

• Day 3-4: Multi-frequency GNSS, integer ambiguity resolution

• Day 5: Real-world tuning, filter divergence detection, adaptive filtering

Advanced Topics

Tightly-Coupled Integration

• Uses raw pseudoranges (z) = ρ + dt·c + troposphere + ionosphere + multipath

• Kalman state includes: [position, velocity, attitude, gyro_bias, accel_bias, clock_bias]

• Advantages: Works with 1 satellite (clock aiding), better in urban canyons

• Challenges: Nonlinear pseudorange equations, receiver implementation complexity

Ultra-Tight Integration

• Carrier phase tracking loop uses INS-predicted satellite position

• Achieves tight integration without explicit pseudorange computation

• Can navigate with 0-1 satellites (clock prediction by INS)

• Used in high-dynamics applications (aircraft, missiles)

Adaptive Filtering for GNSS Multipath

• Multipath causes pseudorange errors correlated with antenna placement

• Variance adaptation: monitor innovation sequence for consistency

• Hypothesis testing: detect and exclude outlier measurements

• Residual monitoring: chi-square test for filter consistency

INS/GNSS Initialization & Alignment

• Coarse alignment: use GNSS position + local gravity

• Fine alignment: compute attitudes from initial motion vector

• Gyro bias convergence: typically 20-30 seconds

• Accel bias convergence: depends on maneuvers

RAIM (Receiver Autonomous Integrity Monitoring)

• Detects faulty GNSS measurements using redundant satellites

• Chi-square test: residuals from least-squares fit

• Excludes measurements with largest residuals iteratively

• Safety-critical applications (aviation, maritime)

Progressive Exercises

Exercise 1: Compare Integration Techniques ⭐

• Implement INS-only, loosely-coupled, and loosely-coupled+RAIM

• Disable GNSS: observe INS drift rates (typical: 1-10 m/min)

• Calculate: Position uncertainty growth during 30-minute outage

• Expected: INS-only unbounded growth, integrated bounded

Exercise 2: Optimal Fusion Tuning ⭐⭐

• Sweep process noise Q: observe state covariance P convergence

• Sweep measurement noise R: analyze innovation sequence

• Use check_innovation_chi2(): verify filter consistency

• Goal: Minimize covariance while keeping innovations whitened

Exercise 3: IMU Grade Comparison ⭐⭐

• Model consumer (0.1 °/hr gyro), tactical (0.01 °/hr), nav-grade (0.0001 °/hr)

• Compare position error after 10-minute GNSS outage

• Plot: Position uncertainty vs IMU grade

• Key: Demonstrate why navigation-grade IMUs worth $100k cost

Exercise 4: Urban Canyon Simulation ⭐⭐⭐

• Add GNSS outage intervals: [60-90s], [120-150s], [180-210s]

• Model multipath: add ±5m bias to 50% of measurements randomly

• Compare: Loosely-coupled vs tightly-coupled recovery speed

• Analyze: Covariance reductions during re-acquisition

Exercise 5: Receiver Clock Dynamics ⭐⭐⭐

• Modify receiver clock model: linear drift + random walk

• Observe: Clock bias convergence time without GNSS

• Track: Frequency stability (Allan variance concept)

• Compare: White noise vs frequency random walk models

Exercise 6: GNSS/INS Misalignment Detection ⭐⭐⭐⭐

• Introduce 5° pitch error in initial attitude

• Implement: Residual monitoring chi-square test

• Detect: Time to identify misalignment from innovation sequence

• Recovery: Reset covariance sub-blocks and re-process measurements

Exercise 7: Multi-Constellation GNSS ⭐⭐⭐⭐

• Extend code to fuse GPS + GLONASS + Galileo

• Implement: Constellation-specific atmospheric models

• Compare: Single-constellation vs multi-constellation position accuracy

• Factor: Geometry improvement with more satellites

Core References

1. Groves, P. D. (2013). Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems (2nd ed.). Artech House. — Definitive integrated navigation reference

2. Titterton, D., & Weston, J. (2004). Strapdown Inertial Navigation Technology (2nd ed.). The Institution of Electrical Engineers. — Comprehensive INS theory

3. Kaplan, E. D., & Hegarty, C. J. (2017). Understanding GPS/GNSS: Principles and Applications (3rd ed.). Artech House. — GNSS fundamentals

Advanced References

4. Noureldin, A., Karamat, T. B., & Georgy, J. (2013). Fundamentals of Inertial Navigation, Satellite-Based Positioning, and Their Integration. Springer. — Integration architecture details

5. Parkinson, B. W., & Spilker Jr, J. J. (Eds.). (1996). Global positioning system: theory and applications (Vol. 163). American Institute of Aeronautics and Astronautics. — GPS foundations

6. Brown, R. G., & Hwang, P. Y. (2012). Introduction to Random Signals and Applied Kalman Filtering (4th ed.). John Wiley & Sons. — Kalman filter for navigation

PyTCL API Documentation

• `initialize_ins_state() <https://github.com/nrl-ai/nrl-tracker/tree/main/pytcl/navigation>`__: Create INS state vector

• `mechanize_ins_ned() <https://github.com/nrl-ai/nrl-tracker/tree/main/pytcl/navigation>`__: INS propagation equation

• `gravity_ned() <https://github.com/nrl-ai/nrl-tracker/tree/main/pytcl/navigation>`__: Gravity model computation

• `radii_of_curvature() <https://github.com/nrl-ai/nrl-tracker/tree/main/pytcl/navigation>`__: Earth curvature at location

• `earth_rate_ned() <https://github.com/nrl-ai/nrl-tracker/tree/main/pytcl/navigation>`__: Rotation rate compensation

• `transport_rate_ned() <https://github.com/nrl-ai/nrl-tracker/tree/main/pytcl/navigation>`__: Transport rate due to curvature