基于粒子滤波的地形辅助导航算法仿真系统

资 源 简 介

本项目基于MATLAB软件平台，详细设计并实现了基于粒子滤波（Particle Filter, PF）的地形辅助导航（Terrain Aided Navigation, TAN）算法。项目旨在模拟飞行器在缺乏卫星导航（GPS）信号的复杂环境下，利用地形匹配技术修正惯性导航系统（INS）的累积误差。主要功能模块包括：1. 仿真环境构建：加载数字高程模型（DEM）地图数据，规划飞行轨迹并生成包含经纬度、高度及姿态的飞行真值。2. 传感器模拟：建立惯性导航系统误差模型以生成含漂移的推算位置，并模拟雷达高度计在不同地形下的对地高度测量数据（包含高斯白噪声）。3. 核心算法实现：应用序贯蒙特卡洛方法（粒子滤波），具体包含粒子群的初始化分布、基于运动学方程的状态预测、利用DEM数据与实测高程差计算粒子权重的更新步骤、以及防止粒子退化的重采样策略。4. 性能评估：将粒子滤波估计的轨迹与真实轨迹及纯INS轨迹进行对比，量化分析算法在不同地形特征（平坦、起伏、粗糙）下的收敛速度与定位精度。

详 情 说 明

基于粒子滤波的地形辅助导航仿真系统

项目简介

本项目是一个基于MATLAB平台开发的地形辅助导航（Terrain Aided Navigation, TAN）仿真系统。该系统旨在模拟飞行器在无法获取GPS信号的环境下，通过惯性导航系统（INS）与雷达高度计配合，利用数字高程模型（DEM）进行地形匹配，从而显著修正INS的累积误差，实现高精度的自主定位。核心算法采用序贯蒙特卡洛方法（粒子滤波）。

功能特性

• 合成地形环境构建：利用MATLAB内置函数生成具有起伏特征的虚拟数字高程模型（DEM），模拟真实地形环境。
• 多传感器仿真：

* INS仿真：模拟惯性导航系统的加速度计、陀螺仪测量过程，并引入加速度噪声及速度漂移，生成包含累积误差的推算轨迹。 * 雷达高度计仿真：模拟机载雷达对地高度测量，包含基于地形真值和高斯白噪声的测量模型。

• 粒子滤波定位算法：实现了完整的SIR（Sampling Importance Resampling）粒子滤波流程，包括状态预测、利用地形高程差进行权重更新、以及低方差重采样策略。
• 全方位可视化分析：提供三维轨迹对比、二维平面粒子收敛过程、位置误差收敛曲线以及地形轮廓匹配情况的多视图展示。
• 性能量化评估：自动计算并对比纯INS导航与粒子滤波修正后的均方根误差（RMSE），直观展示精度提升效果。

系统要求

• MATLAB R2016a 或更高版本
• 无需额外的工具箱（核心逻辑使用基础数学库及插值函数，如interp2）

使用方法

1. 确保MATLAB当前工作目录为项目根目录。
2. 直接运行主脚本（原main.m）即可启动仿真。
3. 程序将自动执行以下步骤：

* 初始化参数与构建地形。 * 生成飞行真值轨迹。 * 生成传感器观测数据。 * 运行粒子滤波算法进行逐时刻位置修正。 * 输出进度日志与最终误差统计。 * 弹出包含四个子图的综合结果分析窗口。

算法实现细节与逻辑

本项目代码完全在一个主函数文件中实现，逻辑结构清晰，具体实现细节如下：

1. 仿真参数与环境初始化

• 时间设定：仿真总时长为100秒，采样间隔1秒。
• 粒子群配置：由500个粒子组成，重采样阈值设为粒子总数的一半。
• 地形生成：使用MATLAB的peaks函数生成$2000 times 2000$米的基础地形数据，并将其高度线性缩放至0-300米范围，分辨率为10米/网格。
• 不确定性建模：定义了雷达测量噪声（5m）、INS加速度噪声（0.5m/s²）、速度漂移（0.1m/s）以及初始位置误差（50m）。

2. 轨迹与传感器数据生成

• 真实轨迹（Ground Truth）：模拟飞行器从地图左下区域出发，通过正弦加速度控制产生转弯机动。飞行高度设定为气压高度，包含一个缓慢变化的正弦波动（600m基准）。
• INS推算：在真实加速度基础上加入高斯白噪声，并累积随机游走漂移，通过积分获得具有累积误差的位置估计（x_ins）。
• 雷达测量：测量方程定义为h_radar = 飞行绝对高度 - 地形高度 + 噪声。程序在每一步通过双线性插值获取当前真实位置下的地形高度来生成测量值。

3. 粒子滤波核心逻辑

算法采用标准的“预测-更新-重采样”循环：

• 初始化：在INS初始位置周围生成服从高斯分布的粒子群，初始权重均等。
• 预测步骤 (Prediction)：

* 利用INS的速度信息作为控制输入驱动粒子运动。 * 为防止粒子贫化，在位置和速度更新中加入了过程噪声。

• 更新步骤 (Update)：

* 批量地形查询：同时查询所有粒子当前位置对应的DEM地形高度。 * 似然计算：计算预测测量值（气压高度 - 粒子处地形高度）与实际雷达测量值的残差。假设测量噪声服从高斯分布，根据残差计算粒子权重（似然度）。 * 权重归一化：将所有粒子权重归一化，若权重发生数值退化则重置。

• 状态估计：计算所有粒子的加权平均值作为当前时刻的系统位置估计（x_est）。
• 重采样 (Resampling)：

* 计算有效粒子数（$N_{eff}$）。 * 当$N_{eff}$低于阈值时，触发低方差重采样（Low Variance Resampling）算法。该算法保留高权重粒子，淘汰低权重粒子，能够有效缓解粒子退化问题。

4. 关键辅助函数

• 地形高度查询：封装了interp2函数，支持单点或批量坐标的地形高度线性插值。包含了边界检查逻辑，防止粒子跑出地图范围导致程序报错。
• 低方差重采样：实现了标准的随机通用采样（Stochastic Universal Sampling）逻辑，通过单一随机数和累积权重进行系统性重采样，计算复杂度低且采样方差小。

结果分析与可视化

仿真结束后，控制台将输出纯INS导航与粒子滤波辅助导航的RMSE（均方根误差）对比及精度提升百分比。同时生成包含以下内容的图形窗口：

1. 三维飞行轨迹对比：在3D地形图上同时绘制真实轨迹（绿色）、INS推算轨迹（红色虚线）和粒子滤波修正轨迹（蓝色），直观展示高程方向和平面方向的修正效果。
2. 2D轨迹与粒子散点：以等高线图为背景，每隔20步绘制一次粒子群的分布散点（品红色），展示粒子从初始散布到收敛于真实位置的过程。
3. 水平位置误差收敛曲线：对比随时间变化的INS误差（发散）与PF误差（收敛），验证算法的长期稳定性。
4. 地形剖面匹配效果：绘制真实地形轮廓与粒子滤波估计位置处的地形轮廓，两者的一致性直接反映了地形匹配的成功程度。