基于EKF的GPS与INS组合导航高精度定位系统

资 源 简 介

本项目利用MATLAB开发了一套针对导航应用的高精度扩展卡尔曼滤波（EKF）算法，主要解决非线性动力学系统下的状态估计问题。在该应用中，核心功能是实现惯性导航系统（INS）与全球定位系统（GPS）的数据融合。系统通过INS获取高频、短时间内高精度的相对运动信息，同时利用GPS提供的低频、无累积误差的绝对位置信息来校正惯性传感器的漂移。 在实现方法上，项目建立了包含位置、速度、姿态以及传感器偏置在内的状态空间模型。由于导航中的运动方程和观测方程（如经纬度转化为地心坐标系）具有强烈的非线性，算法在每个时间步通

详 情 说 明

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

项目介绍

本项目实现了一个基于扩展卡尔曼滤波（EKF）的高精度导航定位仿真系统。系统核心逻辑是通过融合惯性导航系统（INS）与全球定位系统（GPS）的数据，实现对运动物体轨迹和状态的精确估计。该系统充分发挥了IMU高采集频率和GPS无累积误差的各自优势，在非线性动力学模型下提供稳定的导航参数输出。

功能特性

1. 多源数据融合：同时处理100Hz的IMU高频数据和1Hz的GPS低频数据。
2. 16维状态估计：能够实时解算包括三维位置、三维速度、四元数姿态（4维）、陀螺仪三轴偏置及加速度计三轴偏置在内的完整导航状态。
3. 15维误差状态卡尔曼滤波：采用间接法滤波框架，通过估计位置、速度、姿态误差及传感器漂移来修正名义状态。
4. 非线性系统线性化：利用雅可比矩阵对复杂的动力学方程和观测方程进行一阶泰勒展开，处理坐标系转换及旋转运动带来的非线性问题。
5. 传感器误差建模：内置了详细的传感器噪声模型，包括陀螺仪和加速度计的高斯白噪声、零偏不稳定性及注入常值偏置的补偿机制。

系统要求

1. 软件环境：MATLAB R2016b 或更高版本。
2. 工具箱：无需特殊工具箱支持，完全由底层数学逻辑实现。
3. 资源占用：标准个人计算机即可满足计算需求，系统运行效率高，接近实时解算水平。

使用方法

1. 在MATLAB环境中通过命令行调用主程序入口。
2. 程序将自动执行为期120秒的轨迹仿真，模拟物体从直线加速到转弯及最后减速的运动过程。
3. 仿真运行过程中，后台会同步进行IMU预积分和GPS校正计算。
4. 运行结束后，控制台将输出平均位置均方根误差（RMSE），并自动弹出可视化分析窗口。

核心实现逻辑与功能分析

1. 轨迹与传感器数据生成 程序首先模拟了一个真实的物理动态过程。通过设定加速度和角速度，生成包括直线加速、水平转弯（40-80s）和减速三个阶段的参考轨迹。同时，在真实的运动参数基础上注入了随机噪声和常值零偏（Bias），生成模拟的IMU原始输出，并按1Hz频率抽取生成带噪声的GPS参考坐标。

2. EKF 预测步（时间更新） 在每一个IMU采样周期（0.01s）：

• 状态推算：利用四元数法进行姿态更新，避免了欧拉角带来的奇异性问题。通过旋转矩阵将机体系加速度转换至导航系，并结重力补偿进行速度和位置的积分。
• 协方差预测：构建15x15维的状态转移矩阵F，描述了位置、速度、姿态误差与传感器偏置之间的相互耦合关系。根据过程噪声阵Q和时间步长同步更新估计协方差矩阵P。

3. EKF 更新步（量测更新） 每隔1秒（GPS采样时刻）：

• 观测模型：建立以GPS位置和速度为输入的6维观测向量。
• 增益计算：计算卡尔曼增益K，将观测坐标与INS预测位置之间的残差权重映射到误差状态中。
• 闭环修正：利用估计出的15维误差状态对名义状态进行修正。位置、速度和偏置采用加法修正，姿态则通过增量四元数进行乘法修正，并执行四元数归一化以确保数学严谨性。

4. 关键辅助算法

• 坐标变换：实现了方向余弦矩阵（DCM）、四元数、欧拉角三者之间的标准转换函数，确保在不同坐标系下的矢量投影准确。
• 反对称运算：专门编写了反对称矩阵（Skew-symmetric）生成函数，用于处理姿态解算中的叉乘运算。
• 姿态追踪：通过四元数乘法逻辑，实现了高频动态下的姿态连续性维护。

可视化与性能评估

系统提供四种维度的可视化分析：

• 三维轨迹图：直观对比参考轨迹与EKF估计轨迹的重合度。
• 误差曲线：实时显示X, Y, Z三个方向的位置估计误差，并标注总体的RMSE。
• 姿态追踪：针对航向角（Yaw）进行独立对比，展示系统对动态转弯过程的捕捉能力。
• 协方差收敛：通过半对数坐标显示P矩阵对角线元素的变化，验证滤波器在引入GPS观测后定位不确定性的迅速收敛过程。