Particle Filter Implementation for Terrain-Aided Navigation Algorithm

In this paper, we implement a particle filter-based terrain-aided navigation algorithm using MATLAB software. Our implementation begins with collecting sensor data (such as IMU measurements and altimeter readings) and preprocessing it to extract essential navigation information. The core algorithm involves initializing a particle set representing possible vehicle states, followed by recursive prediction and update steps. During prediction, particles are propagated using motion models, while the update step incorporates terrain elevation data through likelihood evaluation against digital elevation maps (DEMs). Key MATLAB functions include particle resampling techniques (e.g., systematic resampling) to mitigate degeneracy and importance weighting based on terrain correlation metrics. This algorithm is applicable not only to ground vehicles but also to UAVs and other autonomous navigation systems. Through this research, we aim to contribute to the advancement of future navigation technologies by demonstrating robust probabilistic localization in GPS-denied environments.