【滤波跟踪】基于拓展卡尔曼滤波EKF的INS_GNSS松耦合系统，用于建模和集成使用多个传感器的动态系统附matlab代码

原创于 2025-11-11 21:29:43 发布 · 895 阅读

12 ·

CC 4.0 BY-SA版权

文章标签：

#matlab #开发语言

✅作者简介：热爱科研的Matlab仿真开发者，擅长数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。

🍎 往期回顾关注个人主页：Matlab科研工作室

🍊个人信条：格物致知,完整Matlab代码获取及仿真咨询内容私信。

🔥 内容介绍

这个代码实现了一个基础的惯性导航系统（INS）和全球导航卫星系统（GNSS）的松散耦合组合导航方案。其核心目标是：

利用 EKF
将低成本 IMU（惯性测量单元，提供角速率和加速度）的高频数据与 GNSS 的低频（但绝对）位置和速度测量数据进行融合。
估计并补偿
IMU 的主要误差源，如陀螺零偏和加速度计零偏。
输出
比纯 INS 或纯 GNSS 更平滑、更精确的导航结果（位置、速度、姿态）。

代码的主要特点是：

松耦合
：融合的是 GNSS 解算后的位置和速度，而不是原始的伪距或载波相位观测值。
基于 EKF
：使用扩展卡尔曼滤波来处理系统的非线性特性（主要来自姿态和位置的更新）。
状态估计
：除了导航参数（位置、速度、姿态），还估计了 IMU 的零偏，这是提高长期导航精度的关键。
可视化
：提供了丰富的绘图功能，包括轨迹、姿态、速度、误差以及一个简单的 3D 飞行器动画。

数据加载
：支持加载模拟数据或真实数据。
状态初始化
：
- 状态向量
  包含 21 个元素：3 个位置误差、3 个速度误差、3 个姿态误差（用四元数的三个分量表示）、3 个陀螺零偏、3 个加速度计零偏、3 个陀螺比例因子、3 个加速度计比例因子。
- 误差协方差矩阵 P
  被初始化为一个对角矩阵，对角线上的元素代表了对每个初始状态误差的不确定度的估计。

2.2. 主处理循环

INS 机械编排（Mechanization）- 预测步骤的基础：
- 姿态更新
  ：使用四元数微分方程，根据 IMU 的角速率（减去估计的零偏）更新载体的姿态。
- 速度更新
  ：将 IMU 的加速度（减去估计的零偏）从载体坐标系转换到导航坐标系，然后加上重力、科里奥利力等项，更新速度。
- 位置更新
  ：根据更新后的速度，更新位置（经纬度和高度）。
- 这些计算都是开环的，仅依赖于 IMU 数据，因此误差会随时间累积。
EKF 预测（Prediction）：
- 状态转移矩阵 F
  ：描述了系统状态误差如何随时间演化。它是通过对非线性的 INS 方程进行线性化得到的。
- 过程噪声协方差 Q
  ：描述了由于 IMU 噪声（如随机游走）和未建模误差导致的状态不确定性的增长。
- 在预测步骤中，EKF 根据F和Q来预测下一时刻的误差协方差矩阵 P。
EKF 更新（Update）- 当有 GNSS 观测时：
- 代码设定为每 sampling_freq 个 IMU 周期进行一次 GNSS 更新。
- 观测方程
  ：z = H * x + v，其中 z 是 GNSS 的位置和速度观测值与 INS 预测值之间的差值（即innovation）。H是观测矩阵，它将状态误差映射到观测误差。
- 卡尔曼增益 K
  ：它决定了观测信息在修正状态估计时的权重。K 越大，说明观测信息越可信。
- 状态修正
  ：state_correction_vector = K * z。这是 EKF 的核心输出，它告诉我们需要如何调整（修正）我们当前对 INS 状态（位置、速度、姿态、零偏等）的估计，以使其与 GNSS 观测值更好地吻合。
- correct_states;
  ：这是一个关键的子函数（虽然代码中未列出其实现），它负责实际应用这个修正向量到 INS 的状态中。例如，它会用修正量去调整位置、速度、姿态四元数以及估计的陀螺和加速度计零偏。
- 协方差更新
  ：P = (I - K*H) * P，更新后的P反映了融合后状态估计的不确定性。