基于EKF的GPS与惯性组合导航仿真系统

资 源 简 介

该项目实现了全球定位系统（GPS）与捷联式惯性导航系统（SINS）的松组合导航仿真程序。系统核心功能是利用惯性测量单元（IMU）提供的高频加速度与角速度数据进行惯性解算，通过机械编排算法实时计算飞行器或车辆的三维位置、速度和姿态信息。为了抑制惯导系统随时间不断累积的发散误差，程序引入了GPS提供的绝对位置和速度观测值作为校正信息。系统采用扩展卡尔曼滤波（EKF）作为信息融合算法，将惯导的误差状态（包括位置误差、速度误差、姿态误差以及加速度计和陀螺仪的零偏）定义为滤波状态量，通过误差状态空间模型进行在线评估

详 情 说 明

基于扩展卡尔曼滤波的GPS与惯性导航系统(GPS/INS)组合导航仿真系统

项目介绍

本项目是一个基于扩展卡尔曼滤波（EKF）的SINS/GPS松组合导航仿真系统。系统模拟了飞行器在三维空间中的运动过程，通过融合高频的惯性测量单元（IMU）数据与低频的全球定位系统（GPS）数据，实现高精度的位置、速度和姿态估计。该系统重点在于利用误差状态空间模型，通过闭环反馈机制修正惯导系统的累积误差，并实时估计传感器的零偏（Bias）。

功能特性

• 多维运动轨迹生成：内置预设的水平圆周转弯运动轨迹，支持三维坐标、速度及姿态参考值的生成。
• 传感器噪声模拟：考虑到实际传感器的物理特性，系统模拟了陀螺仪和加速度计的常值偏置（Bias）、随机游走（ARW/VRW）噪声，以及GPS的位置和速度观测噪声。
• 捷联惯导解算（SINS）：通过四元数法进行姿态更新，并采用经典的机械编排算法完成速度与位置的积分计算。
• 15维扩展卡尔曼滤波（EKF）：滤波器涵盖了位置误差、速度误差、姿态误差、陀螺仪零偏及加速度计零偏共15个状态量。
• 闭环校正机制：每当GPS观测数据到来时，系统计算估计误差并立即反馈至导航状态中，同时复位滤波器的误差状态。
• 性能量化分析：自动计算三维位置和速度的均方根误差（RMSE），并生成多维度可视化图表。

系统要求

• 软件环境：MATLAB R2016b 或更高版本。
• 核心模块：仅需MATLAB基础环境，不依赖额外的工具箱。

实现逻辑与功能细节

1. 仿真场景与参数初始化

程序首先定义了仿真总时长（600秒）、IMU采样频率（100Hz）和GPS采样频率（1Hz）。同时设定了地球物理常数（如长半径、自转角速度、重力加速度）以及传感器的误差标准差，包括陀螺仪 0.1 deg/hr 的偏置和加速度计 100 ug 的偏置。

2. 轨迹发生器实现

系统在北-东-地（NED）坐标系下生成理想轨迹。模拟了一个定高100米、恒定速率50m/s的连续转弯运动。基于运动学关系，程序根据预设轨迹推导出了理想的角速度和比力（加速度）数据，作为传感器的激励源。

3. 传感器数据仿真

在理想IMU数据基础上，叠加了常值偏置和基于高斯白噪声产生的随机游走误差。GPS观测数据则通过在真实轨迹上添加位置5米、速度0.1米/秒的标准差噪声来生成。

4. 捷联惯导机械编排

• 姿态更新：通过旋转矢量法处理角速度数据，利用四元数进行姿态集成，避免了欧拉角带来的奇异性问题。
• 速度与位置更新：利用姿态阵（Cbn）将机体系下的加速度补偿重力后转至导航系，通过一次和二次积分得到速度与位置。

5. EKF滤波器核心算法

• 状态向量设计：$X = [delta P^n, delta V^n, phi^n, epsilon, nabla]^T$，即位置、速度、姿态的误差，以及传感器零偏。
• 预测步：构造15x15的系统状态转移矩阵F，考虑了姿态变动对速度误差的影响，并基于当前IMU输出对比力建立误差动力学模型。
• 更新步：当仿真时钟到达GPS频率周期时，提取GPS观测值。观测矩阵H为6x15，代表位置和速度的直接观测。通过计算卡尔曼增益并处理残差，得到最优误差估计。
• 闭环反馈：估计出的误差量用于补偿当前的导航状态（位置、速度、四元数、零偏估值），随后将滤波器中的状态量清零。

关键算法说明

• 四元数乘法与转换：系统内部定义了四元数与旋转矩阵、欧拉角相互转换的函数，支持高频姿态解算。
• 反对称矩阵（Skew-symmetric）：用于向量叉乘的矩阵化处理，是构造EKF状态转移矩阵F的关键。
• 误差补偿逻辑：导航解算过程中，先减去当前估计的传感器偏置（Bias），再进行积分，这种前向反馈显著提高了导航精度。

结果可视化与评估

程序运行结束后会生成四个图表：

• 3D轨迹对照图：直观展示真实轨迹、GPS观测点与组合导航解算轨迹的重合程度。
• 导航误差曲线：实时显示北、东、下三个方向的位置误差（米）与速度误差（米/秒）。
• 姿态误差曲线：计算并显示俯仰、横滚、航向角的估计偏差（度）。
• 偏置估计收敛图：动态展示滤波器对加速度计和陀螺仪零偏的在线学习能力。

最终控制台将输出系统性能评估报告，涵盖总时长、位置均方根误差（RMSE）、速度均方根误差以及传感器偏置的最终估计值。