基于鲁棒卡尔曼滤波的GPS/INS组合导航仿真系统

资 源 简 介

本系统实现了一种基于鲁棒控制理论改进的卡尔曼滤波算法，专为解决GPS/INS组合导航系统中出现的非高斯干扰及量测异常值问题。项目首先在初始对准阶段引入鲁棒滤波，通过抗抑制算法降低干扰噪声对初始姿态确定精度的影响，缩短对准时间并提高航向角与旋转角的估计可靠性。 在组合导航运行阶段，该系统同步运行标准卡尔曼滤波（KF）与鲁棒卡尔曼滤波（RKF），对惯性导航系统（INS）提供的角速度、加速度信息与全球定位系统（GPS）提供的定位、测速信息进行深度融合。算法核心通过引入Huber代价函数或三段论阈值判别，实时动态

详 情 说 明

基于鲁棒卡尔曼滤波的GPS/INS组合导航与初始对准仿真系统

本系统是一个基于MATLAB开发的组合导航仿真平台，旨在对比标准卡尔曼滤波（KF）与鲁棒卡尔曼滤波（RKF）在应对GPS量测异常值（Outliers）时的性能差异。通过构建15维误差状态空间模型，系统实现了从IMU原始数据生成、INS机械编排到多传感器深度融合的全过程模拟。

功能特性

1. 复杂环境模拟 系统能够模拟载体在600秒时长内的运动轨迹，包括直线行驶和圆周转弯机动。同时，系统通过在特定时间段人为注入量测跳变（经纬度及速度大幅度偏移），模拟了实际工程中常见的GPS多路径干扰或卫星失锁导致的非高斯噪声环境。

2. 双路径滤波对比 系统同步运行两条滤波分支：一种是传统的标准卡尔曼滤波，另一种是基于Huber代价函数的鲁棒卡尔曼滤波。用户可以直观对比出在干扰发生时，标准算法的剧烈震荡与鲁棒算法的平稳过渡。

3. 高维误差状态估计 系统建立了包含姿态误差、速度误差、位置误差、陀螺仪零偏及加速度计零偏在内的15维状态向量，实现了对INS系统误差的实时估计与反馈补偿。

4. 自动化性能评估 仿真结束后，系统会自动统计并打印忽略初始对准期后的东向与北向位置均方根误差（RMSE），并生成多维度的图形化分析结果。

---

系统逻辑与实现细节

1. 传感器数据生成逻辑 系统首先根据预设的机动方案生成理想的姿态、速度和位置真值。随后，在真值基础上叠加了传感器误差：

• 陀螺仪与加速度计：包含常值零偏（Bias）和随机游走（Random Walk）噪声。
• GPS数据：在低频采样（1Hz）的基础上，通过在200s和450s附近区域加入脉冲式干扰，生成含有异常值的量测序列。

2. INS机械编排实现 系统通过欧拉积分法实现了惯性导航的解算。利用IMU采集的角速度和加速度信息，不断更新载体的姿势阵（Cnb）、速度以及经纬度坐标。该模块作为滤波器的预测部分，提供了载体在GPS更新间隙期间的高频状态推算。

3. 鲁棒卡尔曼滤波（RKF）算法核心 该模块是系统的核心创新点，其实现步骤如下：

• 残差标准化：计算观测值与预测值之间的创新向量（残差），并利用观测协方差矩阵和预测误差协方差矩阵进行标准化处理。
• 三段论阈值判别：引入Huber权重因子。当标准化残差小于预设阈值k0（2.0）时，视为正常数据，权重为1；当残差在k0与k1（4.5）之间时，线性削减其权重；当残差超过k1时，判定为异常值，赋予极小权重（1e-6）。
• 动态噪声调整：通过权重因子实时修正量测噪声协方差矩阵R，从而抑制异常观测对状态更新的负面影响。

4. 误差补偿机制 滤波器输出的15维状态增量被用于修正导航参数。姿态修正通过反对称阵（skew-symmetric matrix）作用于当前姿态阵实现，速度与位置则通过增量相加的方式进行直接补偿。

---

关键函数与算法分析

姿态阵转换算法 系统实现了从俯仰、横滚、偏航（ZYX顺序）到旋转矩阵的转换逻辑。通过构建三个维度的坐标旋转矩阵并进行乘积运算，准确描述了载体坐标系（b系）与导航坐标系（n系）的相对关系。

反对称矩阵构造 为了处理三维空间的交叉乘积运算（如加速度在姿态误差下的投影），系统将三维向量转化为反对称矩阵，这是建立INS误差传播方程（如速度误差导数方程）的基础。

运动学建模简化 在仿真实现中，系统重点关注滤波算法的鲁棒性，因此对地球模型进行了适度简化，忽略了科氏加速度和地球自转角速度的补偿，使算法的核心逻辑更加聚焦于抗干扰性能。

---

使用方法

1. 启动MATLAB软件。
2. 将包含相关脚本的文件夹设置为当前工作目录。
3. 运行主仿真函数。
4. 仿真完成后，系统将自动弹出5个图形窗口，展示轨迹对比、位置误差、速度误差、姿态误差以及RMSE统计表。

系统要求

• 软件环境：MATLAB R2016b 及以上版本。
• 硬件要求：标准PC即可，无需特殊算力支持。
• 依赖工具箱：无需额外第三方工具箱，仅使用MATLAB自带的基础函数。