基于卡尔曼滤波的GPS与INS松耦合组合导航系统

资 源 简 介

本项目主要实现基于卡尔曼滤波的GPS（全球定位系统）与INS（惯性导航系统）组合导航算法。系统功能涵盖了数据生成、捷联惯导解算、滤波器设计及误差分析全过程。具体包括：1. 轨迹发生器：模拟载体（如汽车、无人机）的运动轨迹，生成包含噪声和零偏的IMU（加速度计和陀螺仪）原始数据以及GPS位置和速度观测数据。2. 捷联惯导（SINS）机械编排：利用四元数法或方向余弦矩阵进行姿态更新，结合比力方程进行速度和位置更新，实现纯惯性导航解算。3. 组合滤波核心：设计并实现基于误差状态的扩展卡尔曼滤波器（Error-State EKF）或无迹卡尔曼滤波器（UKF），采用松耦合（Loose Coupling）架构。利用GPS量测信息构建观测方程，对INS推算的误差状态（位置误差、速度误差、姿态误差及传感器零偏）进行估计和闭环修正，从而抑制INS随时间发散的累积误差。4. 结果可视化与评估：输出并绘制真实轨迹、纯惯导轨迹与组合导航轨迹的对比图，以及各状态量的误差收敛曲线，直观展示算法对导航精度的提升效果。

详 情 说 明

GPS/INS 组合导航系统 (基于卡尔曼滤波)

项目介绍

本项目实现了一个基于 MATLAB 的 GPS（全球定位系统）与 INS（惯性导航系统）松耦合组合导航系统。核心算法采用基于误差状态的扩展卡尔曼滤波器（Error-State EKF）。程序不仅仅是解算器，还包含了一个完整的仿真环境，能够生成包含特定机动动作（加速、转弯、爬升）的轨迹数据以及添加了噪声和零偏的 IMU 和 GPS 传感器数据，用于验证导航算法的性能。

功能特性

• 全流程仿真：内置轨迹发生器，无需外部数据集即可运行。可模拟静止、直线加速、匀速、转弯、爬升等多种运动状态。
• 传感器建模：模拟了陀螺仪和加速度计的随机游走（零偏）和白噪声，以及 GPS 的位置和速度测量噪声。
• SINS 机械编排：实现了基于四元数的捷联惯导解算算法，包括姿态、速度和位置的更新。
• 松耦合 EKF 融合：

* 设计了 15 维状态向量的卡尔曼滤波器。 * 实现 INS 机械编排（高频）与 GPS 量测更新（低频）的多速率融合。 * 包含闭环（Closed-loop）反馈校正机制，实时修正导航状态并估计传感器零偏。

• 误差分析与可视化：提供轨迹对比（真实值 vs 惯导解算 vs 组合导航）及各状态量的误差收敛曲线。

系统要求

• MATLAB R2016a 或更高版本。
• 无需额外的工具箱（核心运算基于基础矩阵操作）。

使用方法

直接运行主函数 main 即可开始仿真。程序将自动执行以下步骤：

1. 生成仿真轨迹和传感器数据。
2. 执行组合导航解算。
3. 弹出窗口显示轨迹对比图和误差分析图表。

详细功能与实现逻辑

本项目的主程序严格按照以下逻辑流程实现，代码内部结构清晰分明：

1. 全局常数与仿真参数配置

程序首先定义了 WGS-84 坐标系下的地球参数（长半轴、偏心率、自转角速度、重力加速度）。 仿真参数设定如下：

• 仿真时长：600秒。
• IMU 更新频率：100Hz。
• GPS 更新频率：1Hz。
• 定义了陀螺仪和加速度计的零偏不稳定性及随机噪声标准差。
• 定义了 GPS 的位置和速度量测噪声。

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

利用内置的生成器函数创建参考轨迹。轨迹设计包含以下阶段：

• 静止对准阶段：前10秒。
• 加速阶段：向北加速。
• 匀速巡航：保持速度飞行。
• 转弯机动：向东转弯（改变偏航角）。
• 爬升机动：改变俯仰角并增加高度。
• 平飞与减速。

程序根据运动学方程反推出理想的 IMU 比力和角速度，随后叠加设定好的零偏和白噪声，生成仿真 IMU 数据。同时生成带有噪声的 GPS 定位和测速数据。

3. 系统初始化

在开始导航解算前，程序模拟了初始对准过程：

• 状态初始化：在真实状态基础上人为添加了微小的初始姿态误差、速度误差，模拟非完美的初始对准。
• 协方差矩阵 (P) 初始化：构建 15x15 的协方差矩阵，对角线元素分别对应姿态、速度、位置、陀螺零偏、加计零偏的初始不确定性。
• 过程噪声 (Q) 与量测噪声 (R)：根据传感器参数构建相应的噪声矩阵。

4. 主循环：SINS 解算与 EKF 融合

这是系统的核心部分，针对每一个 IMU 采样时刻进行处理：

• IMU 数据预处理：获取当前时刻的陀螺仪和加速度计数据，并减去当前估计的零偏（est_bg, est_ba）。
• SINS 机械编排：

* 利用修正后的数据进行纯惯性导航解算。 * 使用四元数更新姿态。 * 根据比力方程更新速度，并考虑地球自转和哥氏力影响。 * 更新经纬高位置。

• EKF 时间更新（预测）：

* 计算系统状态转移矩阵 F。 * 由于是连续时间系统，通过 $I + F cdot dt$ 进行离散化得到 Phi。 * 更新状态协方差矩阵 $P = Phi P Phi^T + Q cdot dt$。

• EKF 量测更新（GPS 到来时）：

* 判断当前时间是否与 GPS 时间戳对齐。 * 构造残差：计算 INS 推算的位置/速度与 GPS 观测值之间的差值。 * 量测矩阵 (H)：构建线性化观测矩阵，建立误差状态与观测残差的映射关系。 * 卡尔曼增益 (K)：计算最优增益。 * 状态校正：计算 15 维误差状态量 dx。

• 反馈校正 (Feedback)：

* 姿态修正：利用计算出的姿态误差角修正四元数。 * 位速修正：直接从当前 INS 状态中减去速度和位置误差。 * 零偏修正：将估计出的传感器零偏误差累加到当前的零偏估计值中。 * 重置：完成闭环修正后，将误差状态量视为已补偿，无需清零 P 矩阵，但逻辑上的误差状态归零，等待下一次累积。

5. 结果存储与可视化

循环结束后，程序调用绘图函数展示：

• 三维轨迹图：直观对比参考真值、纯惯导（未修正，会发散）和组合导航（修正后）的路径。
• 误差曲线：绘制位置、速度、姿态在该 600 秒内的误差变化，以及程序对传感器零偏的在线估计收敛情况。

关键算法说明

• 状态向量 (15维)：

* $phi$ (3维): 姿态误差 (Roll, Pitch, Yaw) * $delta v$ (3维): 速度误差 (北、东、地) * $delta p$ (3维): 位置误差 (纬度、经度、高度) * $epsilon$ (3维): 陀螺仪零偏误差 * $nabla$ (3维): 加速度计零偏误差

• 闭环反馈 (Closed-Loop)：系统采用闭环架构。这意味着滤波器估计出的误差不仅用于输出显示，还直接反馈到系统状态中进行修正。这种方法可以防止线性化误差随着时间推移通过积分无限增大，从而保持 EKF 的线性假设有效。
• 姿态四元数更新：采用了小角度近似的方法，将滤波估计出的姿态误差角向量转换为由 [1; phi/2] 构成的微小旋转四元数，左乘原姿态四元数完成修正。