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

资 源 简 介

该项目是一个专门针对卫星导航与惯性导航集成技术设计的仿真系统。其核心功能在于模拟和实现GPS与INS的组合导航定位，通过融合两种传感器的优势，有效解决INS随时间偏移严重以及GPS信号更新频率低、易受遮挡的问题。系统内部集成了高精度的轨迹发生器，能够真实还原飞行器或移动载体在三维空间中的复杂运动路径，包括匀速、加速、转弯及爬升等运动状态。系统建立了包含位置、速度、姿态以及惯性器件（加速度计和陀螺仪）零偏在内的多维系统状态模型。在信息融合层面，程序重点实现了基于伪距和伪距率的组合观测模式，利用扩展卡尔曼滤波

详 情 说 明

GPS/INS组合导航仿真系统

项目介绍

本项目是一个基于线性化卡尔曼滤波（EKF）技术的卫星导航（GPS）与惯性导航（INS）紧组合仿真系统。该系统旨在模拟载体在三维空间中的运动，并通过融合高频、短期精度高的惯导数据与低频、无随时间漂移的GPS数据，实现高精度的定位、定速和测姿。系统能够对惯性器件的零偏进行在线估计与补偿，显著提升了导航系统的鲁棒性。

功能特性

• 高度真实的轨迹发生器：支持模拟复杂的运动状态，包括静止、匀速运动、加速运动以及水平转弯，真实反映航天器或地面载体的动态特性。
• 完整的SINS机械编排：实现了基于四元数的姿态更新算法，并包含精细的地球模型补偿，如哥氏加速度、重力随高度/纬度变化以及地球自转修正。
• 15维扩展卡尔曼滤波器：状态向量涵盖姿态误差、速度误差、位置误差、陀螺仪零偏和加速度计零偏，提供全面的误差估计。
• 闭环反馈机制：通过滤波器估算的误差实时修正惯导系统的状态，并定期重置状态向量，确保滤波过程的线性化条件始终成立。
• 误差注入与仿真：支持对陀螺仪和加速度计注入常值偏差和随机游走噪声，模拟真实传感器环境。
• 多维度结果评估：自动生成三维轨迹对比图、位置/速度误差曲线以及姿态误差分析图，辅助算法验证。

系统要求

• MATLAB R2020b 或更高版本。
• 无需額外工具箱，核心算法基于MATLAB原生矩阵运算实现。

实现逻辑与功能详解

1. 参数设置与环境初始化

系统首先定义WGS-84地球参数（长半轴、扁率、自转角速度），并配置仿真时间（600秒）与采样频率（IMU 100Hz，GPS 1Hz）。随后设定惯性传感器的物理误差模型，包括陀螺仪频率随机游走、加速度计零偏等噪声参数。

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

通过理想运动模型生成参考轨迹，具体逻辑为：

• 0-100秒：匀速运动。
• 100-200秒：在北向与东向施加加速度，执行加速运动。
• 200-400秒：施加角速度，执行水平转弯运动。

在生成轨迹的同时，反算出理想的加速度计比力输出和陀螺仪角速度输出，并叠加预设的零偏和随机噪声，生成“观测级”IMU数据。

3. 接续惯性导航（SINS）编排

作为系统核心，程序每隔10毫秒进行一次惯导解算：

• 姿态更新：考虑地球自转（wie）和载体运动引起的航迹坐标系平移，利用四元数微分方程进行姿态演变，随后转换为欧拉角和旋转矩阵。
• 速度更新：在地理坐标系（n系）下进行，计算中严格补偿了哥氏加速度以及随纬度、高度变化的重力加速度。
• 位置更新：利用更新后的速度，结合当前的子午圈和卯酉圈半径计算纬度、经度和高度的变化。

4. 扩展卡尔曼滤波（EKF）

• 状态预测：根据系统误差微分方程构建15x15的转移矩阵F。F矩阵捕捉了姿态、速度、位置误差之间的耦合关系。
• 量测更新：当仿真时间到达1秒整数倍时（GPS更新点），计算惯导推算位置/速度与GPS观测值（含噪声）之间的差值，构成6维观测向量。
• 反馈校正：EKF估算出当前的系统误差项，立即对惯导的四元数、速度、位置进行“减法”或“旋转修正”，并将滤波器内部的前9维状态清零（闭环模式），确保误差不会累积。

关键算法细节分析

姿态表达与修正

项目采用了四元数（Quaternion）作为主要的姿态描述方式，有效避免了欧拉角模型在90度位置的奇异点问题。在接收到滤波器反馈的姿态误差角（phi）后，系统通过构建等效旋转矢量并将其转化为增量四元数，对主航向四元数进行乘法修正。

地球物理模型

系统内置了子午圈（RM）和卯酉圈（RN）半径的实时计算函数。在速度和位置更新中，这些半径值随着纬度的变化动态调整，确保了长距离、长航时导航仿真的地理准确性。

状态转移矩阵 (F)

程序的关键环节在于构建F矩阵。该矩阵描述了：

• 姿态误差随速度误差和位置的变化。
• 加速度计测量误差对速度准确性的影响。
• 陀螺仪零偏对姿态更新的直接干扰。
• 惯性器件残留零偏的随机常值特性。

数据可视化与分析

程序运行结束后，会生成三个窗口：

• 三维空间运动轨迹：直观展示参考轨迹与组合导航解算轨迹的重合度。
• 位置与速度误差：以时间轴展示北向、东向、地向的残差，证明GPS修正的有效性。
• 姿态误差：通过角分（arcmin）单位展示横滚、俯仰、偏航角的估计偏差，反映INS/GPS融合对姿态精度的提升。

使用方法

1. 启动MATLAB环境。
2. 将包含该项目的文件夹设为当前工作路径。
3. 在命令行窗口直接输入主程序名并回车。
4. 程序将自动进行60000次迭代解算，并最终弹出三张性能分析图表。
5. 如需修改传感器规格，可在初始化代码段调整相关的偏置（bias）和噪声（noise）标称值。