Python TERCOM Demo for GPS-Denied Navigation

This repository contains a Python implementation demonstrating a basic TERCOM (Terrain Contour Matching) approach for estimating the position of a vehicle (like a drone or missile) without GPS, by matching a measured terrain altitude profile to a pre-existing digital elevation map (DEM).

The script estimates the vehicle's latitude and longitude based on the best match found and outputs it as a mock NMEA GPGGA sentence, simulating the output of a GPS device.

How it Works

1. Data Loading:
   • Loads a digital elevation map (ground truth) from a NumPy file (.npy). This map is a 2D array where values represent altitude.
   • Loads corresponding metadata (e.g., geographic boundaries, dimensions) from a JSON file.
   • (Includes a function to generate simple example data if none exists).
2. Sensor Simulation (for Demo):
   • Simulates an altitude profile measurement (e.g., from a radar altimeter) by extracting a path of altitude values from the ground truth map at a known "true" location and heading.
   • Adds Gaussian noise to simulate sensor inaccuracies.
3. TERCOM Matching (find_best_match):
   • Takes the simulated observed_profile (1D array of altitudes).
   • Normalizes both the observed profile and candidate profiles from the map by subtracting their mean altitude. This makes the matching less sensitive to absolute altitude errors or offsets.
   • Iterates through possible starting locations (y, x) on the terrain_map.
   • For each location, iterates through a set of possible headings.
   • Extracts a terrain profile from the terrain_map corresponding to the current location, heading, and the length of the observed_profile.
   • Calculates the Mean Squared Error (MSE) between the normalized observed_profile and the normalized map_profile.
   • Identifies the starting location (best_y, best_x) and best_heading on the map that yield the minimum MSE.
4. Coordinate Conversion (indices_to_latlon):
   • Converts the best matching map indices (best_y, best_x) into geographic coordinates (latitude, longitude) using linear interpolation based on the map's metadata (bounding box).
5. NMEA Sentence Generation (create_gpgga_sentence):
   • Formats the calculated latitude, longitude, and an estimated altitude (taken from the map at the best match start point) into a standard NMEA GPGGA sentence.
   • Includes a calculated checksum.
6. Output:
   • Prints the true simulated location, the estimated location, the matching error (MSE), and the generated NMEA sentence to the console.
   • Appends the NMEA sentence to a file named nmea_tercom_output.csv.

Requirements

• Python 3.12+ (as specified in .python-version)
• NumPy (numpy)

You can install the required libraries using pip and your preferred environment manager (like uv, pipenv, conda, etc.). If using uv (based on the uv.lock file):

uv sync

Usage

1. Prepare Data (Optional):
   • Place your ground truth terrain map as a .npy file (e.g., my_map.npy) inside the data/ directory.
   • Create a corresponding .json metadata file (e.g., my_map_metadata.json) in data/ containing at least:

     {
         "lat_top": 40.123,    // Northernmost latitude
         "lon_left": -74.567,  // Westernmost longitude
         "lat_bottom": 40.000, // Southernmost latitude
         "lon_right": -74.000, // Easternmost longitude
         "height": 500,       // Number of rows in the map array
         "width": 600         // Number of columns in the map array
     }

   • If you don't provide data, the script will generate simple example data in data/terrain_map.npy and data/map_metadata.json on the first run.
2. Modify main.py (Optional):
   • Adjust simulation parameters (true_start_y, true_start_x, profile_length, true_heading, sensor_noise).
   • Change the search_headings variable if you want a finer or coarser search grid for the heading.
   • If using your own data files, update the filenames in tercom.load_terrain_data() call within main.py.
3. Run the Script:

   uv run main.py

Output

The script will print the comparison between the simulated true location and the TERCOM estimated location, along with the NMEA sentence.

Example Console Output:

--- TERCOM Python Demo ---
Generating example terrain data...
Saved example map to data/terrain_map.npy
Saved example metadata to data/map_metadata.json
Loaded terrain map 'data/terrain_map.npy' ((100, 150))
Map boundaries: Lat (40.0, 39.8), Lon (-74.0, -73.7)

Simulating observed profile from true start (33, 37), heading 45 deg...
Generated observed profile with 25 points.

Searching for best match using TERCOM...
Searching headings: [  0.  30.  60.  90. 120. 150. 180. 210. 240. 270. 300. 330.]
Best match found: Start Index=(33, 37), Heading=60.0 deg, MSE=3.8742

--- Results ---
True Start Location (Simulation): Index=(33, 37), Lat=39.933333, Lon=-73.926174, Heading=45 deg
Estimated Start Location (TERCOM): Index=(33, 37), Lat=39.933333, Lon=-73.926174, Heading=60.0 deg
Minimum Matching Error (MSE): 3.8742

Mock NMEA GPGGA sentence:
$GPGGA,215717.12,3956.0000,N,07355.5705,W,1,08,1.0,84.3,M,0.0,M,,*XX
NMEA sentence appended to nmea_tercom_output.csv

(Note: Timestamp, checksum, exact coordinates, MSE, and estimated heading will vary).

Limitations & Notes

• Basic Implementation: This is a simplified demonstration. Real-world TERCOM systems involve more sophisticated map data (e.g., higher resolution DEMs), better sensor modeling, more robust matching algorithms (e.g., handling velocity, different correlation techniques), filtering (e.g., Kalman filters) for state estimation, and potentially multi-stage searches (coarse-to-fine).
• Computational Cost: The brute-force search across all locations and headings can be computationally expensive, especially for large maps and fine heading searches. Real systems use optimized search strategies.
• Ambiguity: Areas with repetitive or flat terrain can lead to ambiguous matches and large position errors. TERCOM works best over terrain with unique features.
• Heading Sensitivity: The current implementation searches specified headings. Performance depends heavily on having the correct (or close) heading in the search list. An Inertial Navigation System (INS) is often used alongside TERCOM to provide orientation and constrain the search.
• Linear Georeferencing: Assumes a simple linear relationship between grid indices and lat/lon. This is an approximation.
• Altitude: The altitude in the NMEA sentence is derived directly from the map at the estimated start point.

Potential Extensions

• Implement different matching metrics (e.g., Normalized Cross-Correlation).
• Add Kalman filtering to smooth estimates and integrate with inertial measurements (simulated or real).
• Optimize the search algorithm (e.g., multi-resolution search, pruning unlikely areas).
• Handle map data in standard formats like GeoTIFF (using libraries like rasterio).
• Implement matching for 2D observed patches instead of 1D profiles.
• Integrate with a simulator or real sensor data feed.