简介这份资源面向惯性导航与组合导航方向的学习者与工程人员提供一套基于扩展卡尔曼滤波EKF的INS组合导航MATLAB实现代码可用于理解姿态、速度与位置估计的完整流程并作为算法验证与课程设计的参考基础。压缩包共19个文件约21KB以cpp与h源码为主辅以Makefile构建脚本、README说明文档及gitignore配置涵盖陀螺仪、加速度计、磁力计、GPS等传感器模块与EKF核心、GPS滤波等实现结构紧凑便于按模块阅读。目前已有468人学习下载说明其在导航算法入门与实践中具有一定参考价值。读者可从中获取传感器建模、状态预测与观测更新、多源数据融合及仿真结果展示等关键环节的代码思路适合作为组合导航系统设计与滤波算法改进的起点。1. 拆开 INS_EKF-master一份能跑的组合导航代码到底长什么样很多人第一次接触组合导航都是从“INS 加 GPS 用 EKF 融合”这句话开始的但真到动手时才发现传感器数据怎么进、状态量怎么排、观测矩阵怎么填每一步都能卡住人。INS_EKF-master.zip 就是一份把这条链路完整落地的代码包里面既有 GYRO、ACCELEROMETER、MAGNETOMETER 的驱动层也有 EKF、GPS_Filter 的算法层还有 main.cpp 和 Makefile 负责把整条流程串起来。它适合正在做惯性导航、组合导航课程设计或工程原型的工程师也适合想从零理解 EKF 在导航中怎么落地的人。你拿到的不只是几个公式而是一套能编译、能跑、能改参数看结果的骨架。2. 从传感器到状态量INS_EKF 的数据流与 EKF 选型理由2.1 为什么组合导航绕不开 EKF惯性导航靠加速度计和陀螺仪积分出位置、速度、姿态短时精度高但零偏和噪声会随时间累积几分钟后位置就能飘出几十米。GPS 长期稳定但更新率低、易受遮挡动态场景下还会跳变。把两者融合本质是用 GPS 的长期观测去修正 INS 的累积误差而 EKF 就是干这件事的常用工具。EKF 之所以在组合导航里出现频率这么高是因为 INS 的误差传播方程和 GPS 观测方程都带有非线性但非线性程度不算剧烈。EKF 的做法是在当前估计点附近做一阶泰勒展开把非线性模型线性化然后套用标准卡尔曼滤波的预测和更新框架。相比粒子滤波EKF 计算量小、实时性好相比无迹卡尔曼滤波EKF 实现更直接代码量可控。对于 INS_EKF-master 这种以教学和原型验证为目标的代码包EKF 是性价比最高的选择。代码包里 EKF.cpp 和 EKF.h 承担核心滤波GPS_Filter.cpp 负责 GPS 观测的预处理和融合GYRO、ACCELEROMETER、MAGNETOMETER 三组文件分别对应陀螺、加速度计和磁力计的读取与标定。main.cpp 把传感器数据、GPS 数据和 EKF 串起来Makefile 负责编译。整个结构清晰没有过度封装适合逐行读。2.2 状态量怎么排15 维还是 9 维打开 EKF.h第一件事是看状态向量怎么定义。常见做法是 15 维误差状态位置误差3、速度误差3、姿态误差3、陀螺零偏3、加速度计零偏3。也有简化成 9 维的只保留位置、速度、姿态误差把零偏当作随机游走或直接忽略。INS_EKF-master 里具体用哪种需要看 EKF.h 的成员变量和 EKF.cpp 的协方差矩阵维度。如果你拿到的代码是 15 维预测步骤里状态转移矩阵 F 会包含姿态对速度的耦合项、零偏对姿态和速度的耦合项。这些项在静态或低动态场景下影响不大但在转弯、加减速时直接决定滤波收不收敛。我一般会先确认 F 矩阵的构造是否完整再去看 Q 矩阵的量级是否和传感器噪声匹配。// EKF.h 中状态量定义的典型写法以 15 维为例 const int STATE_DIM 15; // 状态顺序位置(0-2) 速度(3-5) 姿态(6-8) 陀螺零偏(9-11) 加计零偏(12-14) Eigen::Matrixdouble, STATE_DIM, 1 x; // 误差状态 Eigen::Matrixdouble, STATE_DIM, STATE_DIM P; // 协方差 Eigen::Matrixdouble, STATE_DIM, STATE_DIM Q; // 过程噪声这段定义决定了后面所有矩阵的维度。如果你改状态量个数P、Q、F、H 全部要跟着改漏一个就会编译报错或运行出 NaN。参数上Q 的对角线通常按传感器噪声密度乘以采样时间构造陀螺零偏的过程噪声一般取 1e-8 到 1e-6 量级加计零偏类似。具体值要看 IMU 手册没有手册就用静态数据标定。2.3 预测与更新EKF.cpp 里的两个核心函数EKF 的预测步骤做两件事用状态转移矩阵把误差状态推一步同时把协方差 P 推一步。更新步骤用 GPS 的位置和速度观测构造观测矩阵 H计算卡尔曼增益 K修正状态和协方差。INS_EKF-master 的 EKF.cpp 里通常能看到 predict() 和 update() 两个函数或者合并在一个 step() 里。// EKF.cpp 预测步骤的典型结构 void EKF::predict(const IMUData imu, double dt) { // 1. 用陀螺和加计更新名义状态位置、速度、姿态 nominal_state_.integrate(imu, dt); // 2. 构造状态转移矩阵 F Eigen::Matrixdouble, STATE_DIM, STATE_DIM F Eigen::Matrixdouble, STATE_DIM, STATE_DIM::Identity(); F.block3,3(0,3) Eigen::Matrix3d::Identity() * dt; // 位置对速度 F.block3,3(3,6) -skew(nominal_state_.accel) * dt; // 速度对姿态 // 3. 协方差预测 P F * P * F.transpose() Q * dt; }这里 F 的构造是 EKF 里最容易翻车的地方。速度对姿态的耦合项用加速度的反对称矩阵符号错了滤波会发散。姿态误差的定义方式左乘还是右乘也影响 F 的写法代码里一般会在 README 或注释里说明。如果你发现跑起来姿态越修越偏先检查这个符号。更新步骤里GPS 观测通常是位置和速度H 矩阵就是简单的选择矩阵把状态里的位置和速度挑出来。如果 GPS_Filter.cpp 里还做了观测异常检测比如卡方检验或新息阈值那说明作者考虑过 GPS 跳变的情况。这部分逻辑值得单独读因为实际跑车或飞行时GPS 跳变是导致滤波崩溃的头号原因。3. 编译与跑通Makefile、main.cpp 和传感器数据接入3.1 先看 Makefile 再动手编译拿到代码包不要急着 make。先打开 Makefile 看三件事编译器是 g 还是 clangC 标准是 C11 还是 C14有没有链接 Eigen 或其他数学库。INS_EKF-master 里如果用了 EigenMakefile 里会有 -I/usr/include/eigen3 之类的路径。如果你的机器上 Eigen 装在别处改这一行就行。# 典型的编译命令 make clean make -j4 # 如果报错找不到 Eigen手动指定路径 g -stdc11 -I/usr/include/eigen3 main.cpp EKF.cpp GPS.cpp GYRO.cpp \ ACCELEROMETER.cpp MAGNETOMETER.cpp GPS_Filter.cpp Captor.cpp -o ins_ekf编译通过后先别接真实传感器。main.cpp 里一般会有一个仿真模式或离线数据回放模式用预先录制的 IMU 和 GPS 数据跑一遍看输出是否合理。如果 main.cpp 只支持实时传感器输入那就需要自己造一组静态数据陀螺和加计输出零均值噪声GPS 固定在一个点。跑几分钟看位置估计是否收敛到 GPS 附近姿态是否稳定。这一步能排除大部分初始化问题。3.2 传感器数据接入的常见做法GYRO.cpp、ACCELEROMETER.cpp、MAGNETOMETER.cpp 这三个文件负责从硬件读取原始数据。常见做法是通过串口或 I2C 读取然后做单位转换和标定。陀螺输出通常是 rad/s 或 deg/s加计是 m/s² 或 g磁力计是 uT 或 Gauss。代码里一般会有 scale factor 和 bias 两个参数标定就是确定这两个值。如果你没有真实 IMU可以用手机或开源飞控录一段数据存成 CSV然后改 Captor.cpp 里的读取逻辑从文件读而不是从串口读。Captor.h 和 Captor.cpp 是传感器抽象层改这里对上层 EKF 没有影响。我一般会保留一个 data/ 目录放静态、动态、GPS 遮挡三种场景的数据每次改完滤波参数都跑一遍对比。GPS.cpp 和 GPS_Filter.cpp 处理 GPS 数据。GPS 输出通常是 NMEA 语句或二进制协议需要解析出经纬度、高度、速度、航向。GPS_Filter.cpp 里可能做了坐标转换把经纬度转成局部 ENU 坐标再送给 EKF。这一步的坑在于坐标系定义是前右下还是北东地原点选在哪里都会影响 H 矩阵和最终结果。代码里如果有注释说明坐标系一定要先读。3.3 跑通后的第一轮验证跑通之后先看三个量位置误差、速度误差、姿态误差。如果代码里有输出日志用 Python 或 MATLAB 画出来。位置误差应该在 GPS 精度范围内波动速度误差在 0.1 m/s 量级姿态误差在 1 度以内。如果姿态误差持续增大检查陀螺零偏估计是否收敛如果位置误差有规律地振荡检查 Q 和 R 的比例。# 用 Python 快速画误差曲线 import pandas as pd import matplotlib.pyplot as plt df pd.read_csv(ekf_output.csv) plt.plot(df[time], df[pos_err], labelpos err) plt.plot(df[time], df[vel_err], labelvel err) plt.legend() plt.show()这一步不需要高深工具能看出趋势就行。如果曲线发散先降 Q 或升 R让滤波更信任 GPS。如果曲线滞后升 Q 或降 R。调参没有万能公式但方向是明确的。4. 避坑与排查INS_EKF 跑不起来时先查这五件事4.1 现象编译报错 “Eigen/Core: No such file or directory”原因Eigen 头文件路径没配或者根本没装 Eigen。解决Ubuntu 下sudo apt install libeigen3-dev然后确认 Makefile 里的 -I 路径是/usr/include/eigen3。如果用的是 macOSbrew 装完 Eigen 后路径可能是/opt/homebrew/include/eigen3改 Makefile 即可。4.2 现象程序跑起来输出全是 NaN原因协方差矩阵 P 失去正定性或者某个矩阵除零。常见触发点是 dt 为 0、传感器数据里有 NaN、Q 或 R 设成了 0。解决在 predict 和 update 入口加断言检查 dt 0、输入数据有限、Q 和 R 的对角线大于 0。如果 P 已经发散可以在每次更新后做一次对称化P (P P.transpose()) / 2。4.3 现象姿态估计越跑越偏GPS 修不回来原因姿态误差的符号定义和 F 矩阵不匹配或者陀螺零偏没有估计。解决先确认代码里姿态误差是左乘还是右乘再检查 F 矩阵里速度对姿态的耦合项符号。如果零偏没估计把状态量扩到 15 维给陀螺零偏一个小的过程噪声让它慢慢收敛。4.4 现象GPS 跳变时位置估计被带飞原因没有做观测异常检测GPS 的离群点直接被 EKF 吸收。解决在 GPS_Filter.cpp 里加新息卡方检验或者简单点算新息范数超过阈值就跳过这次更新。阈值一般取 3 到 5 倍 GPS 标准差。代码包里如果有 GPS_Filter大概率已经留了接口改一个 if 就行。4.5 现象静态下位置缓慢漂移原因加速度计零偏没标定或者 Q 里加计零偏的过程噪声太大。解决静态采集几分钟加计数据算均值作为零偏初值写入代码。同时把加计零偏的过程噪声调小让滤波在静态时更信任零偏估计。如果还漂检查重力补偿是否正确姿态旋转矩阵有没有把重力方向搞反。5. 进阶用法用仿真数据验证 EKF 收敛性并调参跑通真实数据后我建议回头做一轮纯仿真验证。做法是自己生成一条已知轨迹比如匀速直线加转弯用真实 IMU 噪声模型生成陀螺和加计输出再生成对应时刻的 GPS 观测加入高斯噪声。然后把这份仿真数据喂给 INS_EKF-master看估计轨迹和真值的偏差。这一步能排除传感器标定和安装误差的干扰直接看 EKF 本身是否收敛。// 仿真数据生成的核心逻辑伪代码 for (double t 0; t T; t dt) { // 真值轨迹 true_pos trajectory(t); true_vel derivative(trajectory, t); true_att attitude(t); // 生成 IMU 观测真值加零偏加噪声 gyro_meas true_omega gyro_bias gyro_noise(); accel_meas true_accel accel_bias accel_noise(); // 生成 GPS 观测真值加噪声低频输出 if (fmod(t, gps_dt) dt) { gps_pos true_pos gps_noise(); gps_vel true_vel gps_noise(); } // 送入 EKF ekf.predict(gyro_meas, accel_meas, dt); if (gps_ready) ekf.update(gps_pos, gps_vel); }仿真跑完后重点看三个指标位置误差的均方根、速度误差的均方根、姿态误差的均方根。如果位置 RMSE 在 1 米以内、速度 RMSE 在 0.1 m/s 以内、姿态 RMSE 在 0.5 度以内说明 EKF 参数基本合理。如果某一项明显偏大就针对那一项调 Q 和 R。比如位置误差大先升 GPS 的 R 还是降 Q我的经验是先确认 GPS 噪声模型是否准确如果 GPS 本身噪声就大升 R 会让滤波更平滑但滞后如果 IMU 噪声大升 Q 会让滤波更信任 GPS。两者要配合调不能只动一个。调参时我习惯用表格记录每次改动的参数和对应的 RMSE避免来回试。下面是一个典型的调参记录表轮次Q_posQ_velQ_attR_gps_posR_gps_vel位置 RMSE速度 RMSE姿态 RMSE11e-41e-31e-61.00.12.3 m0.25 m/s1.2 deg21e-51e-41e-71.00.11.1 m0.12 m/s0.8 deg31e-51e-41e-72.00.20.9 m0.10 m/s0.7 deg从表里能看出降 Q 和升 R 都让误差变小但升 R 太多会导致动态响应变慢。实际工程里要在精度和平滑度之间取平衡。仿真验证的好处是你可以反复跑不用心疼硬件。还有一个技巧把 EKF 的协方差 P 的对角线画出来。如果 P 收敛到一个稳定值说明滤波进入稳态如果 P 一直增大说明过程噪声太大或观测太少如果 P 震荡说明 Q 和 R 比例不合适。这个曲线比误差曲线更早暴露问题。从那以后我每次拿到新的组合导航代码都强制先跑仿真再上实车仿真里把 Q 和 R 调到一个合理范围实车只做微调。这样能省下大量在车上反复重启的时间。希望帮到你。本文还有配套的精品资源点击获取