inav — Inertial Navigation Toolbox

A Python reproduction of the methods from the PhD thesis on Inertial Navigation with Terrain- and Vision-Aided techniques for GNSS-denied environments.

It implements the full pipeline: INS mechanization, synthetic trajectory & IMU generation, IMU error models, simplified error analysis, master-slave/gimballed IMU generation, GNSS-INS fusion, the thesis's novel Multi-Hypothesis Filter (MHF) for terrain-aided navigation, and vision-based absolute positioning with the DLT.

• Plain-language thesis overview: THESIS_SUMMARY.md
• Build plan / stages: IMPLEMENTATION_PLAN.md

Install

python -m venv .venv
.venv/bin/python -m pip install -r requirements.txt

Core modules need numpy/scipy; plotting demos need matplotlib; the vision feature-matching needs opencv-python (SIFT/ORB/FAST — AKAZE requires opencv-contrib-python).

Run the tests

.venv/bin/python -m pytest

96 tests cover the math, filters and end-to-end behaviour (Kalman/NEES consistency, the trajectory→IMU→mechanization round-trip, the Schuler/vertical channels, MHF convergence, TERCOM/SITAN, the DLT pose recovery and the NASADEM loader). The DEM tests run against the real tiles in DEM/ (see below) and are skipped when that folder is absent.

Real DEM data (Chapter 5)

Drop NASADEM/SRTM 1-arc-sec .hgt tiles (zipped or raw) into DEM/:

DEM/NASADEM_HGT_n35e051.zip
DEM/NASADEM_HGT_n36e051.zip
...

• inav/terrain/nasadem.py loads/mosaics/crops them into a TerrainMap (load_tile, crop, available_tiles).
• The MHF, TERCOM and SITAN demos below use the real tiles automatically.
• DEM/ is git-ignored (it is ~70 MB); the loader tests skip without it.

Run the demos

.venv/bin/python -m examples.demo_imu_generation    # Ch 3: trajectory + IMU (Figs 3.2-3.5)
.venv/bin/python -m examples.demo_ins_drift         # Ch 2/3: unaided INS drift
.venv/bin/python -m examples.demo_master_slave      # Ch 3.4-3.5: gimballed/FLIR IMU (Figs 3.11-3.18)
.venv/bin/python -m examples.demo_gnss_fusion       # Ch 4: INS-GNSS fusion, both scenarios (Figs 4.5-4.14)
.venv/bin/python -m examples.demo_terrain_classic   # Ch 5.1: TERCOM cost surface + SITAN (real DEM)
.venv/bin/python -m examples.demo_terrain_mhf       # Ch 5.2: MHF terrain nav, real DEM (Figs 5.11-5.15)
.venv/bin/python -m examples.demo_vision_dlt        # Ch 6: AVP with DLT (Figs 6.6-6.12)

Figures are written to docs/figures/ (17+ PNGs, committed).

What each part does (mapped to the thesis)

Module	Thesis	Purpose
inav/constants.py, inav/frames.py, inav/earth.py	Ch 2.1-2.5	WGS84 constants, DCM/Euler rotations, radii of curvature, gravity, Earth/transport rates
inav/mechanization.py	Ch 2.6	Discrete INS update: attitude → velocity → position
inav/error_model.py	Ch 2.7-2.9	15-state INS error dynamics (F, G) + bias augmentation
inav/error_analysis.py	Ch 2.10	Simplified lateral/vertical error analysis (Schuler, unstable vertical)
inav/trajectory.py	Ch 3.1	Motion segments + quintic smoothing
inav/imu_generation.py	Ch 3.2	Inverse kinematics → synthetic gyro/accel truth
inav/imu_errors.py	Ch 3.3	Bias repeatability/instability, scale factor, misalignment, random walk
inav/master_slave.py	Ch 3.4-3.5	Master-slave, spinning gimbal and FLIR tracking IMU generation
inav/vibration.py	Ch 3.6	PSD-profile synthetic vibration
inav/gnss.py	Ch 4.1-4.2	Constellation + pseudorange least-squares positioning
inav/kalman.py	Ch 4.3	Kalman filter, Van Loan discretization, NEES
inav/fusion.py	Ch 4.4	Loosely-coupled closed-loop INS-GNSS
inav/terrain/dted.py	Ch 5	Terrain map (interpolation, gradients, synthetic)
inav/terrain/nasadem.py	Ch 5	Real NASADEM/SRTM .hgt loader (mosaic/crop)
inav/terrain/tercom.py	Ch 5.1.1	TERCOM batch matching (MAD/MSD)
inav/terrain/sitan.py	Ch 5.1.2	SITAN slope-based recursive filter
inav/terrain/mhf.py	Ch 5.2	Novel Multi-Hypothesis Filter (main contribution)
inav/vision/camera.py	Ch 6.3	Pinhole model, intrinsics, projection matrix
inav/vision/dlt.py	Ch 6.4	Direct Linear Transform + P decomposition
inav/vision/image_gen.py	Ch 6.5	Synthetic camera image + feature matching

Highlights (reproduced results)

• Round-trip fidelity: trajectory → IMU → mechanization reproduces an 80 s flight to sub-metre accuracy.
• Error analysis (Ch 2.10): the 84.4-minute Schuler period and the ~1.2 km position oscillation from a 0.1 mG accelerometer bias are reproduced exactly; the vertical channel grows exponentially as predicted.
• Master-slave (Ch 3.4-3.5): a 100 rpm spinning platform produces ~10.5 rad/s slave body rates; the FLIR gimbal keeps its boresight on a ground target with alignment 1.0000.
• INS-GNSS fusion (Ch 4): position error stays a few metres with GNSS and grows during outages; heading error grows in straight flight and drops during turns — matching the thesis's scenario-1/2 behaviour (square loop + outage legs in red, Fig 4.10).
• TERCOM (Ch 5.1.1): recovers a known 700 m shift exactly (−700 m) on the real DEM cost surface.
• SITAN (Ch 5.1.2): converges from small initial errors but diverges from large ones — the thesis's motivation for the MHF.
• MHF terrain nav (Ch 5.2, real DEM): converges from a ~1.7 km initial error to a steady-state 2D mean of 42 m (per-axis mean-abs 10 m N / 39 m E — tighter than the thesis's ~100 m per-axis floor), with the hypothesis count adapting 1 → ~18 → 1 as in thesis Fig. 5.15.
• Vision AVP (Ch 6): across 36 camera locations the DLT position error ranks SIFT (~4 m) < ORB (~6 m) ≪ FAST (unreliable), matching the thesis.

Notes & simplifications

• The MHF uses a 15-state per-hypothesis error model (the thesis augments with scale-factor/misalignment states as well). Terrain fixes are fed back to position/velocity by default; attitude/bias feedback is available via MHFParams(feedback_attitude=True) but is unstable on real data (see the verified configuration in inav/terrain/mhf.py).
• As the thesis discusses (Sec. 5.2.7), there is an accuracy-vs-divergence trade-off governed by the rlow coverage floor.
• RVP (Sec. 6.1) is literature-only — the thesis explicitly states it is out of scope; AVP is fully implemented.
• The orthophoto for the vision demo is synthetic (a georeferenced procedural image) since the USGS dataset is not redistributable; the DEM for terrain navigation is real NASADEM data.