基于卡尔曼滤波的INS/GPS组合导航仿真平台

资 源 简 介

本项目是专门为导航工程初学者设计的教学级仿真系统，旨在解决惯性导航系统（INS）随时间发散的累积误差问题。系统通过MATLAB环境实现了完整的导航解算逻辑，包括轨迹发生器、IMU传感器模拟、捷联惯导力学方程解算以及卡尔曼滤波融合模块。其核心功能是利用GPS提供的低频、高精度位置和速度信息，对高频、自主性强的INS数据进行实时修正。本项目采用了经典的15维误差状态卡尔曼滤波器，涵盖了载体的位置误差、速度误差、姿态误差以及加速度计和陀螺仪的随机零偏，能够有效估计并在反馈调节中消除传感器漂移的影响。该系统不仅展

详 情 说 明

基于卡尔曼滤波的INS/GPS组合导航仿真平台

本项目是一个专为导航工程初学者设计的教学级仿真系统，旨在通过MATLAB环境深入演示惯性导航系统（INS）与全球定位系统（GPS）的融合过程。该系统不仅实现了完整的导航解算逻辑，还通过15维误差状态卡尔曼滤波器（ESKF）有效地解决了惯性传感器随时间发散的累积误差问题。

项目核心功能特性

系统提供了一套完整的“轨迹生成-传感器模拟-导航解算-误差修正-结果可视化”闭环仿真流程。主要特性包括：

1. 15维误差状态建模：涵盖了载体的三轴姿态误差、三轴速度误差、三轴位置误差、陀螺仪随机零偏以及加速度计随机零偏，能够全面捕捉系统误差特性。
2. 完整的INS解算链条：实现了基于四元数的姿态更新算法、速度更新以及基于地球曲率半径变化的位置更新。
3. 松组合导航架构：利用低频（1Hz）的高精度GPS位置与速度信息，对高频（100Hz）的INS解算结果进行实时反馈修正。
4. 传感器误差模拟：支持向理想运动数据中注入固定零偏、高斯白噪声以及随机游走误差，模拟真实的传感器工作环境。
5. 运动轨迹模拟：支持载体进行直线匀速运动及转弯动作，模拟复杂的动态运行场景。

实现逻辑与算法细节

系统的主程序逻辑严格遵循导航系统的物理特性和数学模型，具体实现步骤如下：

一、参数初始化与环境建模 设置WGS-84地球模型参数（长半轴、扁率、自转角速度等），定义仿真总时长为600秒，IMU采样率为100Hz，GPS更新率为1Hz。同时设定传感器的噪声标准差和初始状态。

二、轨迹发生器逻辑 生成参考真相（Ground Truth）。载体初始以20m/s的速度由北向南运动，在100至200秒期间进行90度的水平转弯。系统根据运动学方程实时推算每一时刻的理想姿态（欧拉角）、速度（NED坐标系）和地理位置（纬度、经度、高度）。

三、IMU传感器数据生成 根据理想运动轨迹反算载体在机体系下的理想比力与理想角速度。在理想值基础上叠加预设的常值零偏（Bias）和随时间变化的随机噪声，模拟产生实际陀螺仪和加速度计的输出数据。

四、捷联惯导（SINS）解算

1. 姿态更新：使用陀螺仪数据通过四元数法进行姿态递推，并考虑了小角度下的更新补偿。
2. 速度更新：将加计输出的比力通过旋转矩阵转换至导航坐标系（NED），补偿重力加速度后进行速度积分。
3. 位置更新：考虑地球经纬度方向的子午圈和卯酉圈曲率半径，将速度映射为经纬度和高度的变化量。

五、误差状态卡尔曼滤波（ESKF）过程

1. 状态预测：构建15维状态转移矩阵F，描述误差随时间的传播过程。在每一个IMU采样周期，更新误差状态协方差矩阵P。
2. 观测更新：当GPS信号到达时，计算INS预测位置、速度与GPS测量值之间的残差。
3. 增益计算：根据预测误差协方差P和观测噪声矩阵R计算卡尔曼增益K。
4. 反馈修正：利用滤波器输出的误差估计值，直接对INS的四元数姿态、速度向量和位置坐标进行修正，并将对应的误差状态清零，完成闭环反馈。

关键函数说明

系统内置了多个数学工具函数以支撑核心算法：

1. 坐标变换系列：包括欧拉角转旋转矩阵、欧拉角转四元数、四元数转旋转矩阵、旋转矩阵转四元数以及四元数转欧拉角。这些函数确保了在不同坐标系和表示方法之间的准确切换。
2. 地球模型计算：通过输入纬度，计算当前地理位置对应的子午圈半径（RM）和卯酉圈半径（RN），这是位置解算的数学基础。
3. 数学运算工具：包括生成反对称矩阵的函数，用于处理矢量叉乘运算，在构建状态转移矩阵和处理姿态误差项时至关重要。

系统要求与使用方法

系统要求：

1. 运行环境：MATLAB R2016b 或更高版本。
2. 依赖模块：仅需MATLAB基础数学库与绘图工具箱，无需额外专业工具箱。

使用方法：

1. 打开MATLAB，将工作目录指向项目根目录。
2. 在命令行窗口直接输入主程序函数名并回车。
3. 程序将自动开始仿真计算，屏幕下方会显示当前计算的时间进度。
4. 仿真完成后，系统会自动弹出四个图形窗口，分别展示：三维运动轨迹对比图、位置与速度误差曲线图、姿态估计误差图以及IMU传感器零偏的估计收敛轨迹。

结果说明

通过本平台的仿真，用户可以直观地观察到：在没有GPS修正时，INS的误差会随时间呈平方或立方级增长；而引入GPS进行组合导航后，系统的位置误差能稳定在米级，速度误差趋于稳定，且滤波器能够准确地估计并分离出IMU的常值零偏。