三维组合导航系统采用惯性导航系统（INS）与全球卫星导航系统（GNSS）融合技术，利用扩展卡尔曼滤波（EKF）与标准卡尔曼滤波（KF）进行状态估计。本文基于实际代码实现，深入剖析一种采用扩展卡尔曼滤波的三维组合导航算法架构，重点说明其数据融合逻辑、状态建模策略与误差校正机制。
系统架构与数据来源
该实现以 MATLAB 为开发平台，整合了两类传感器原始数据：
- GNSS 模块（ATGM332D）：提供三维位置（经度、纬度、高度）与速度信息；
- 惯性测量单元（MPU6050）：输出三轴加速度、姿态角（俯仰、滚转、偏航）及时间戳。
系统通过将 GNSS 的绝对位置/速度与 IMU 的相对运动信息进行融合，旨在克服单一传感器的固有缺陷——IMU 存在积分漂移，而 GNSS 在动态或遮蔽环境中更新率低、噪声大。
导航状态建模
算法采用 15 维状态向量，涵盖以下物理量：
- 位置与速度（6 维）：三维位移与三维速度；
- 姿态角误差（3 维）：俯仰、滚转、偏航的小角度误差（隐含在误差状态中）；
- 陀螺与加速度计零偏/误差（6 维）：用于补偿 IMU 传感器的系统性偏差。
这种误差状态卡尔曼滤波（Error-State Kalman Filter, ESKF）结构，将真实状态与估计状态之间的微小偏差作为滤波器状态，既保留了非线性系统的近似处理能力，又避免了直接对姿态角进行线性化带来的奇异问题。
时间对齐与运动积分
在每一采样时刻，系统首先基于 IMU 加速度数据进行两次数值积分：
- 由加速度积分得到速度；
- 由速度再次积分得到位置。
此'纯惯导'轨迹作为预测值，但会随时间迅速发散。与此同时，GNSS 提供的绝对位置与速度作为外部观测输入，构成量测向量。值得注意的是，代码中对 GNSS 原始经纬度进行了以初始点为参考的差分处理，转化为局部坐标系下的相对位移，便于与惯导积分结果对齐。
扩展卡尔曼滤波核心流程
算法在每一时间步执行标准 EKF 五步迭代：
- 状态预测：利用当前状态与 IMU 输入，通过状态转移矩阵 $F$ 推演下一时刻状态；
- 协方差预测：结合过程噪声协方差 $Q$，传播状态不确定性；
- 卡尔曼增益计算：基于预测协方差与观测噪声协方差 $R$，动态调整滤波权重；
- 状态更新：利用 GNSS 与惯导之间的残差（即量测新息），校正预测状态；
- 协方差更新：降低更新后状态的不确定性。
其中，状态转移矩阵 $F$ 融合了地球曲率修正（通过椭球参数 $a$、$b$）、姿态矩阵 $R_b$ 以及 IMU 误差动力学模型，体现了对三维运动学的物理建模深度。
对比验证与性能评估
为验证 EKF 融合效果，系统同时实现了一个简化的标准卡尔曼滤波器（KF），仅针对单轴位置与速度进行融合（状态维数为 2）。通过并行运行 KF 与 EKF，程序输出两类结果：
- KF 结果：基于线性假设，适用于小范围、低动态场景；
- EKF（ESKF）结果：显式处理姿态与传感器误差，适用于三维复杂运动。
最终通过绘制位移、速度曲线及计算定位误差范数（如水平位置误差 $\sqrt{(x_{\text{est}} - x_{\text{gps}})^2 + (y_{\text{est}} - y_{\text{gps}})^2}$），直观展示 EKF 在抑制惯导漂移、提升动态响应精度方面的优势。
总结
该实现不仅完成了 INS/GNSS 紧耦合组合导航的基本功能，更通过 15 维误差状态设计、姿态矩阵实时更新与协方差动态调整，构建了一个具备工程实用价值的三维导航滤波框架。尽管代码在某些细节（如 GNSS 高度处理复用纬度变量）存在待优化之处，但其整体架构清晰、逻辑严谨，为后续引入更复杂的传感器模型（如磁力计辅助航向、零速修正等）奠定了良好基础。对于从事自主导航、无人机或智能驾驶定位模块开发的工程师而言，此实现提供了极具参考价值的技术范本。
