三维的组合导航。 ins和卫星的组合导航算法，基于卡尔曼滤波和eskf滤波的都有。 MATLAB源码，有kf和eskf的对比，也有单独的误差，或者输入滤波之后的位移速度等导航参数。 这段程序主要是一个卡尔曼滤波器，用于估计运动物体的位置和速度。它涉及到数据的读取、处理和滤波过程。 首先，程序通过`importdata`函数读取名为`ceshi.txt`的数据文件，并将数据存储在变量`data`中。 接下来，程序从`data`中提取出所需的数据，并将其存储在不同的变量中，如`gvx`、`gvy`、`gvz`等。这些变量代表了运动物体的速度和位置信息。 然后，程序对数据进行处理，计算出一些相关的量，如位移量和时间和。这些处理包括对速度和加速度进行积分，计算位移和时间和。 接下来，程序初始化一些变量，并创建一些空矩阵用于存储测量值和协方差矩阵。 然后，程序进入一个循环，从第二个数据点开始，计算一系列的矩阵和向量，包括转移矩阵、过程噪声协方差、观测矩阵等。然后，程序使用离散卡尔曼滤波公式对状态进行更新和校正，得到估计的位置和速度。 在循环的过程中，程序还计算了卡尔曼滤波器的误差，并将结果存储在一些变量中。 最后，程序绘制了一些图形来展示卡尔曼滤波器的结果，包括估计的位置和速度与测量值的比较，以及误差的分析。 此外，程序还定义了一个名为`kalman`的子函数，用于实现卡尔曼滤波的具体计算过程。该函数接受时间、加速度和GPS测量值作为输入，并返回估计的速度和位置。 总的来说，这段程序主要是为了通过卡尔曼滤波器来估计运动物体的位置和速度。它涉及到数据的读取、处理、滤波过程以及结果的分析和可视化。在实际应用中，这种方法可以用于航空航天、导航和自动驾驶等领域，以提高位置和速度的精确度和稳定性。
在现代高精度定位与导航系统中，惯性导航系统（INS）与全球卫星导航系统（GNSS）的融合已成为主流技术路径。本文基于实际代码实现，深入剖析一种采用扩展卡尔曼滤波（Extended Kalman Filter, EKF）的三维组合导航算法架构，重点说明其数据融合逻辑、状态建模策略与误差校正机制。
系统架构与数据来源
该实现以MATLAB为开发平台，整合了两类传感器原始数据：
- GNSS模块（ATGM332D）：提供三维位置（经度、纬度、高度）与速度信息；
- 惯性测量单元（MPU6050）：输出三轴加速度、姿态角（俯仰、滚转、偏航）及时间戳。
系统通过将GNSS的绝对位置/速度与IMU的相对运动信息进行融合，旨在克服单一传感器的固有缺陷——IMU存在积分漂移，而GNSS在动态或遮蔽环境中更新率低、噪声大。
导航状态建模
算法采用15维状态向量，涵盖以下物理量：
- 位置与速度（6维）：三维位移与三维速度；
- 姿态角误差（3维）：俯仰、滚转、偏航的小角度误差（隐含在误差状态中）；
- 陀螺与加速度计零偏/误差（6维）：用于补偿IMU传感器的系统性偏差。
这种误差状态卡尔曼滤波（Error-State Kalman Filter, ESKF）结构，将真实状态与估计状态之间的微小偏差作为滤波器状态，既保留了非线性系统的近似处理能力，又避免了直接对姿态角进行线性化带来的奇异问题。
时间对齐与运动积分
在每一采样时刻，系统首先基于IMU加速度数据进行两次数值积分：
- 由加速度积分得到速度；
- 由速度再次积分得到位置。
此“纯惯导”轨迹作为预测值，但会随时间迅速发散。与此同时，GNSS提供的绝对位置与速度作为外部观测输入，构成量测向量。值得注意的是，代码中对GNSS原始经纬度进行了以初始点为参考的差分处理，转化为局部坐标系下的相对位移，便于与惯导积分结果对齐。
扩展卡尔曼滤波核心流程
算法在每一时间步执行标准EKF五步迭代：
- 状态预测：利用当前状态与IMU输入，通过状态转移矩阵 $F$ 推演下一时刻状态；
- 协方差预测：结合过程噪声协方差 $Q$，传播状态不确定性；
- 卡尔曼增益计算：基于预测协方差与观测噪声协方差 $R$，动态调整滤波权重；
- 状态更新：利用GNSS与惯导之间的残差（即量测新息），校正预测状态；
- 协方差更新：降低更新后状态的不确定性。
其中，状态转移矩阵 $F$ 融合了地球曲率修正（通过椭球参数 $a$、$b$）、姿态矩阵 $R_b$ 以及IMU误差动力学模型，体现了对三维运动学的物理建模深度。
对比验证与性能评估
为验证EKF融合效果，系统同时实现了一个简化的标准卡尔曼滤波器（KF），仅针对单轴位置与速度进行融合（状态维数为2）。通过并行运行KF与EKF，程序输出两类结果：
- KF结果：基于线性假设，适用于小范围、低动态场景；
- EKF（ESKF）结果：显式处理姿态与传感器误差，适用于三维复杂运动。
最终通过绘制位移、速度曲线及计算定位误差范数（如水平位置误差 $\sqrt{(x{\text{est}} - x{\text{gps}})^2 + (y{\text{est}} - y{\text{gps}})^2}$），直观展示EKF在抑制惯导漂移、提升动态响应精度方面的优势。
总结
该实现不仅完成了INS/GNSS紧耦合组合导航的基本功能，更通过15维误差状态设计、姿态矩阵实时更新与协方差动态调整，构建了一个具备工程实用价值的三维导航滤波框架。尽管代码在某些细节（如GNSS高度处理复用纬度变量）存在待优化之处，但其整体架构清晰、逻辑严谨，为后续引入更复杂的传感器模型（如磁力计辅助航向、零速修正等）奠定了良好基础。对于从事自主导航、无人机或智能驾驶定位模块开发的工程师而言，此实现提供了极具参考价值的技术范本。
