简介惯性导航与卫星定位的融合是组合导航系统的核心而扩展卡尔曼滤波EKF作为最常用的状态估计方法一直是实现低成本GNSS/INS松耦合方案的关键技术。实际工程中机械编排方程涉及姿态更新、速度更新、位置更新还需处理科里奥利修正、圆锥补偿等细节开发门槛较高。借助MATLAB环境下的开源仿真框架可以快速搭建可复现的算法验证基线。这类工具不仅适用于自动驾驶定位、无人机导航等场景也能帮助研究者评估不同IMU噪声参数对融合精度的影响。NaveGo正是这样一套面向MEMS惯性传感器与消费级GNSS接收机的参考实现它基于EKF完成位置、速度、姿态的融合估计并提供完整的测试脚本与误差评估工具适合作为组合导航算法研究与工程落地的起点。 开头部分做组合导航的同学应该都有这种体会手上拿着一大堆IMU和GNSS数据想验证一套松耦合算法却总是卡在“没有一套靠谱的参考实现”这个坎上。自己从头写机械编排加EKF光是科里奥利修正、姿态更新、量测更新这套东西就能折腾好几个星期而且 Debug 起来特别痛苦。今天我想聊的这个开源项目——NaveGo就是解决这个问题的。NaveGo是一套基于MATLAB的低成本GNSS/INS组合导航仿真框架核心功能是输入IMU原始数据和GNSS观测数据通过扩展卡尔曼滤波EKF实现位置、速度、姿态的融合估计。你在GitHub上搜NaveGo下载下来是个NaveGo-master.zip解压后就能跑比从零搭一套框架省太多事。这套工具适合做导航算法研究的学生、搞自动驾驶定位的工程师以及任何需要快速验证组合导航方案的人。我在自己的项目里用过两个多月跑了各种真实数据整体印象是代码写得很规范能学到不少细节但坑也不少尤其对新手不太友好——文档太少很多东西要自己啃代码。先说结论再逐层拆解。这篇文章会把NaveGo的核心设计思路、机械编排原理、科里奥利修正细节、实操步骤和常见问题全部过一遍保证你拿到这个压缩包之后不用再对着代码发愁。1. NaveGo项目到底做了什么——从仓库结构看整体设计思路1.1 一个开源惯导仿真项目要解决的核心问题NaveGo的全称是“Navigation Goes”是巴西圣保罗州立大学的一位学者开源的导航仿真框架。它的定位很明确给低成本MEMS惯性传感器和消费级/车载级GNSS接收机提供一个基于MATLAB的松耦合组合导航参考实现。我们在实际项目中为什么要依赖这类框架因为组合导航算法的验证本身就很难。真实数据有真值Ground Truth可对比还好但如果你在做算法开发阶段拿到的只是IMU原始数据加速度、角速度和GNSS输出经纬高、速度你很难凭空判断自己的解算结果对不对。NaveGo就是在这种情况下充当“参照物”的。这个框架解决的核心问题有三层第一层把INS机械编排Mechanization完整实现包括姿态更新、速度更新、位置更新第二层把GNSS位置/速度观测模型的量测方程搭好实现EKF的数据融合第三层提供一套可直接运行的测试脚本和评价指标让你能快速评估算法精度。你下载的NaveGo-master.zip其实是从GitHub仓库直接压缩的master分支版本里面包含了上述全部核心代码。circusjwz是上传到某资源站时的上传者IDcoriolis m大概率对应代码里科里奥利修正相关的内容——这个我们后面细说。1.2 仓库结构拆解与核心文件功能把压缩包解压之后你会看到这样的目录结构。我用星号标出最核心的文件/文件夹NaveGo-master/ ├── .gitattributes ├── LICENSE ├── README.md ├── INS/ │ ├── align_gyro.m │ ├── align_acc.m │ ├── ini_align.m % 初始对准 │ ├── ins_mech.m % 机械编排主函数 │ ├── ins_predict.m % INS预测EKF时间更新 │ ├── ins_update.m % 状态更新 │ └── ... ├── GPS/ │ ├── gps_measure.m % GPS量测模型 │ ├── ... ├── el/ │ └── ... ├── data/ │ ├── *.csv % 真实IMU和GPS数据 ├── test_imu_gps.m % 主测试脚本 ├── test_imu_gps_rtk.m ├── tools/ │ ├── ecef2geo.m │ ├── geo2ecef.m │ ├── rmse.m │ └── ...第一眼看下去最值得关注的是ins_mech.m和test_imu_gps.m这两个文件。前者是整个INS解算的核心机械编排后者是演示如何从数据到结果的完整流程。如果你在CSDN或别的资源站下载过这个压缩包解压后优先打开这两个文件。1.3 为什么选MATLAB而不是C/Python很多从C或Python背景过来的人会问为什么这个项目不用Python或C我的理解是NaveGo定位是“仿真验证”而非“产品落地”MATLAB在矩阵运算、调试可视化、快速修改参数这三件事上的效率是碾压级的。举个例子EKF的时间更新和量测更新在MATLAB里天然就是矩阵运算P F*P*F Q一行搞定在C里你得考虑矩阵库、内存管理、数值稳定性。Python虽然用NumPy也能写但在实际调试时MATLAB的变量工作区Workspace可以随时查看每个矩阵的维度、数值、类型这对排查滤波发散问题太友好了。另外你做导航算法研究时大部分对照实验如Allan方差分析、零偏稳定性评估都以MATLAB脚本形式存在直接用NaveGo能无缝衔接。所以项目的技术选型并不是“老”而是“在学术和仿真场景下最合理”。2. 核心算法原理解读——状态方程、观测方程和coriolis修正2.1 INS机械编排从IMU原始数据到位置速度姿态NaveGo的机械编排函数ins_mech.m实现的是经典的捷联惯导解算流程。输入是IMU的角速度和比力输出是位置经纬高、速度东北天和姿态欧拉角/四元数。流程可以概括为三步姿态更新利用陀螺仪角速度积分更新姿态。NaveGo用的是四元数更新通过角增量计算四元数变化量再与当前姿态四元数做乘法。这一步的精度直接决定整个系统的性能所以它内部有圆锥补偿Coning Compensation处理那部分动态误差。速度更新利用加速度计比力去除重力加速度和科里奥利加速度积分得到速度增量。位置更新利用速度增量积分得到位置增量再转为经纬高。我读代码时注意到ins_mech.m里对于比力分解的处理——它把加速度计测量的“比力”减去重力矢量再减去科里奥利项这和教材上的公式是对应的。最核心的机械编排方程是速度微分方程[ \dot{v}^n C_b^n f^b - (2\Omega_{ie}^n \Omega_{en}^n) \times v^n g^n ]这个方程展开之后就是在NaveGo代码里看到的那些项。其中((2\Omega_{ie}^n \Omega_{en}^n) \times v^n)就是科里奥利修正Coriolis Correction和向心加速度修正Transport Rate的来源。标题里的coriolis m大概率指的是这一块的实现。2.2 科里奥利效应为什么不能忽略地球自转很多新手第一次看到机械编排方程里那一大堆“叉乘项”就犯晕地球自转角速度(\Omega_{ie})不是只有(7.292115 \times 10^{-5} \text{ rad/s})吗这么小的量级对精度有什么影响我算给你看。假设载体以10 m/s的速度向北运动在纬度45度处科里奥利加速度大小约为[ a_c 2 \cdot \Omega_{ie} \cdot v \cdot \sin(\varphi) \approx 2 \times 7.29 \times 10^{-5} \times 10 \times 0.707 \approx 1.03 \times 10^{-3} \text{ m/s}^2 ]这个加速度数值上很小但如果长时间积分比如10分钟速度误差累积量约为(0.001 \times 600 0.6 \text{ m/s})位置误差累积会达到几百米。对于松耦合组合导航来说虽然GNSS每次量测都会修正这些误差但在GNSS信号短时丢失的桥洞、隧道、高架场景下INS纯解算期间的漂移能不能控制住科里奥利项的补偿就非常关键了。所以NaveGo里面把科里奥利修正单独做成代码段不是“炫技”而是工程必需。你在打开ins_mech.m时会看到类似这样的片段基于常见实现方式补充% Coriolis force (due to earth rotation) Fn(1) Fn(1) - (2*w_ie(3)*vn(2) w_en(3)*vn(2) ... - 2*w_ie(2)*vn(3) - w_en(2)*vn(3));这段代码的含义是把测得的比力投影到导航系后减去柯氏加速度对速度增量的贡献。如果你要做二次开发建议不要删掉这些项否则中高速运动场景下误差会很明显。2.3 EKF融合松耦合架构下的状态更新逻辑NaveGo用的是松耦合EKF也就是INS和GNSS各自独立解算然后EKF负责融合。状态向量通常是15维位置误差(3)、速度误差(3)、姿态误差(3)、陀螺零偏(3)、加速度计零偏(3)。在每个滤波周期内执行两步时间更新INS预测用INS机械编排结果作为名义轨迹EKF只对误差状态做预测。状态转移矩阵(F)由INS解算过程中的误差方程推导而来噪声协方差矩阵(Q)由IMU的零偏稳定性、角度随机游走等参数设定。量测更新GNSS修正当GNSS数据到达时构建量测向量(z [P_{gnss} - P_{ins}; \ v_{gnss} - v_{ins}])通过量测矩阵(H)将误差状态映射到量测空间然后计算卡尔曼增益、更新误差状态和协方差最后把误差反馈给INS名义轨迹。NaveGo在GPS量测这一块把位置和速度建模都做了。gps_measure.m里矩阵(H)的结构大致是H zeros(6, 15); H(1:3, 1:3) eye(3); % position H(4:6, 4:6) eye(3); % velocity这里我补充一句松耦合的优点是实现难度低、GNSS更新周期和IMU更新周期不必严格同步适合消费级MEMS IMU。缺点是在GNSS信号弱时INS误差完全靠纯解算增长较快。如果做RTK级别的高精度定位可以扩展到紧耦合或深耦合NaveGo也有test_imu_gps_rtk.m示例但核心架构依然是松耦合。3. 实操过程从解压压缩包到跑通第一个仿真3.1 环境准备与项目初始化先说明一下环境NaveGo是基于MATLAB开发的我在R2020b、R2023a下都跑通过建议至少R2019b以上版本否则部分语法可能不兼容。准备步骤就三步解压NaveGo-master.zip放到一个不含中文和不含空格的路径下比如D:/work/NaveGo-master。启动MATLABcd到项目根目录。在命令行执行addpath(genpath(pwd))把项目所有子目录加入路径。然后确认一下数据文件是否存在——data目录下应该有CSV格式的IMU和GPS数据。NaveGo自带的示例数据是使用真实传感器录制的包含IMU数据和GPS接收机的输出格式上IMU是时间戳、陀螺x/y/z、加速度计x/y/zGPS是时间戳、经纬高、速度NED或对应导航系速度。这里有一个非常容易踩的坑如果你从资源站下载的压缩包被人改过或部分文件缺失data目录里可能没有完整的数据。这时候你需要用自己录的数据或者从GitHub上单独拉取示例数据文件。3.2 仿真参数配置与数据格式test_imu_gps.m里会有一大段参数配置代码核心参数有这几类1. IMU参数用于构建Q矩阵% Gyro noise density (deg/h/sqrt(Hz)) gyro_noise_density 0.032; % Accelerometer noise density (ug/sqrt(Hz)) accel_noise_density 218; % Gyro bias instability (deg/h) gyro_bias_instability 6.3; % Accelerometer bias instability (ug) accel_bias_instability 7.1;这些参数决定了EKF的噪声协方差(Q)直接影响滤波收敛速度和稳态精度。如果你的IMU型号不同一定要根据器件手册或Allan方差分析结果修改否则会出现滤波发散或过度信任INS的问题。2. GPS参数用于构建R矩阵% GPS horizontal position standard deviation (m) gps_pos_std 2.5; % GPS vertical position standard deviation (m) gps_vel_std 0.05;消费级GNSS接收机的水平定位标准差通常在1~3米速度标准差在0.03~0.1 m/s。如果你用的是RTK设备把gps_pos_std改到0.02~0.05效果会有质的提升。3. 初始状态初始位置、速度、姿态、以及协方差矩阵(P)的初始值。初始姿态一般通过初始对准ini_align.m得到如果对准不准后面滤波收敛会慢很多。数据格式方面我要特别强调GPS输出的数据如果是NMEA格式比如$GNGGA、$GNGLL、$GNRMC这些语句需要先解析成NaveGo可用的经纬高/速度格式。大多数GNSS模组的数据手册都会提供NMEA语句的字段定义比如$GNGGA的第四、五字段是纬度第六字段是南纬/北纬标志第七、八字段是经度第九字段是东经/西经标志第十、十一字段是海拔高度。你需要在导入NaveGo之前写一个简单的脚本把NMEA字符串转成数值矩阵。NaveGo本身不包含NMEA解析器这一点很多新手会忽略。3.3 运行仿真与结果可视化配置好参数之后直接运行test_imu_gps.m。脚本会依次做以下几件事读取数据文件进行初始对准确定初始姿态主循环每个IMU时刻执行机械编排和EKF时间更新每个GPS时刻执行量测更新保存结果到工作区变量ins、gps、nav等绘制轨迹图和误差图。跑完之后你会看到类似位置误差曲线、速度误差曲线、姿态误差曲线的图像。NaveGo自带的工具里有一个rmse.m函数可以帮你计算位置和速度的均方根误差。如果你想用真实GNSS天线采集的数据做实验需要注意天线安装和IMU之间的杠杆臂Lever Arm补偿。NaveGo默认假设IMU和GNSS天线在同一个位置实际工程中天线和IMU之间往往有几十厘米的偏差这个偏差在高精度场景下会造成几厘米到十几厘米的系统误差。你可以后续在代码里加入杠杆臂补偿但第一次跑通流程建议先把这部分跳过。4. 常见问题与排查技巧实录4.1 滤波发散问题这是大家遇到最多的一个问题表现是位置误差曲线发散、姿态角出现跳跃、协方差矩阵元素变成NaN或Inf。我拆解常见原因初始协方差P0设置过大或过小。P0过大滤波初期对量测的信任权重过高导致状态出现剧烈跳变P0过小滤波收敛过慢。建议P0的初始位置误差设为(10m)^2量级速度误差设为(0.1m/s)^2量级姿态误差设为(0.1deg)^2量级。Q矩阵参数不匹配。IMU噪声参数给得太小滤波就会过于信任INSGNSS量测起不到修正作用给得太大位置估计噪声会非常大。建议结合IMU手册给Allan方差参数。时间戳对齐错误。IMU和GPS数据的时间戳如果不一致GNSS量测到的位置和当前INS位置根本不在同一时刻EKF更新自然发散。我做实验时发现NaveGo对时间戳很敏感建议先做插值对齐。排查技巧先用NaveGo自带数据集跑通确定环境没问题再用自己的数据时把GPS数据画出来和地图对照确认坐标没有奇点、跳变、重复时间戳等问题。4.2 数据格式与坐标系问题NaveGo对数据格式非常严格我见过不少人卡在这个阶段GPS经纬高单位问题有些GNSS模组输出的纬度经度是度分格式DDMM.MMMM而不是十进制度DD.DDDDDDNaveGo默认要求十进制度需要先转换。速度坐标系问题NaveGo默认GPS速度在NED或ENU导航系下部分GNSS模组输出的速度在载体坐标系下需要转换成导航系。高度基准问题GNSS输出的是椭球高WGS84椭球高而当地海拔高度是正高两者相差一个大地水准面起伏约几十米这个偏差在垂直方向会直接影响滤波精度。这些坐标系问题如果不处理滤波虽然不一定发散但误差会明显偏大。对于新手我建议先只用NaveGo自带数据跑通流程再去处理自己采集的数据。4.3 不同IMU型号的适配NaveGo的代码注释里提到了ADIS16405、ADIS16488等常见MEMS IMU但如果你用的是其他型号需要手动修改IMU参数配置。我用过的几个型号分享下参考参数IMU型号陀螺噪声密度 (deg/h/√Hz)加速度计噪声密度 (μg/√Hz)陀螺零偏不稳定性 (deg/h)加速度计零偏不稳定性 (μg)ADIS164050.052006.37.1ADIS164880.008211.83.2VN-1000.01140310ICM-206890.06300815这些参数可以通过Allan方差分析来标定也可以从芯片手册里查关键指标后近似换算。实际调参时Q矩阵的数值不需要特别精确一般一到两个数量级范围内都能收敛但偏差太大会影响精度。4.4 MATLAB版本兼容性NaveGo的作者早期版本是面向MATLAB 2014~2017编写的部分函数在新版本里会有警告。比如strsplit行为变化、figure显示问题、fprintf格式化差异等但这些一般不影响核心功能。如果运行报错先在命令行查看具体错误位置多数是路径或工具箱缺失问题。我遇到过的一个典型错误Undefined function quatmultiply这是因为NaveGo用到了MATLAB的Aerospace Toolbox。如果不想装这个工具箱有不少网友在GitHub issues里提供过替代实现你可以从MathWorks File Exchange搜索quatmultiply替代函数。5. 实战案例用NaveGo跑一次车载组合导航数据5.1 数据准备与预处理我在实际项目里用NaveGo验证过一组车载组合导航数据传感器配置是这样的IMU消费级MEMS输出频率100HzGNSS单频接收机输出频率10HzNMEA格式车辆行驶时长约30分钟包含城市道路、高架桥、隧道等场景拿到数据后第一步不是直接丢进NaveGo而是做预处理用Python脚本解析GNSS模组的NMEA数据提取时间、经纬度、海拔和速度把经纬度从度分格式转为十进制度检查IMU时间戳和GPS时间戳的重合情况发现GPS延迟约200ms做了时间补偿剔除GPS信号跳变点比如隧道里接收机输出的位置漂移很离谱的段。我特别想强调时间同步问题我用的GNSS模组输出的时间戳是PPS对齐的UTC时间IMU时间戳是本地计数器两者之间有一个固定延迟。NaveGo的EKF默认状态向量里不含时间延迟估计所以必须在数据预处理阶段把这个时间延迟补偿掉否则高架桥这种GNSS信号切换频繁的场景误差会很大。5.2 参数配置与仿真运行进入NaveGo后我修改了以下参数IMU噪声参数按ICM-20689的Allan方差数据填入gps_pos_std设为2.0gps_vel_std设为0.05初始位置用GNSS第一个有效定位点初始速度用GNSS第一个有效速度初始姿态因为车辆从静止开始用ini_align.m做静态初始对准运行后对比NaveGo输出和车辆自带的参考系统输出位置误差在开阔路段的CEP约2.3米与GNSS单点定位精度基本一致高架桥下GNSS信号差的路段INS独立解算约10秒位置漂移大约在8米左右这个表现对于消费级MEMS IMU来说属于正常范围。5.3 误差评估与可视化NaveGo的tools/rmse.m可以很方便地输出整体RMSE。结合自己的数据我一般会额外画一张误差随时间变化的曲线分三段看开阔路段、高架桥路段、隧道进出路段。这样能直观看到GNSS丢失时INS的漂移速率和EKF重新收敛的时间。有一个非常实用的调整技巧在GNSS量测更新时可以动态调整R矩阵的数值。比如GNSS速度标准差在开阔路段和高架桥下是不同的在NaveGo的框架里你可以按固定值设但在实际应用里可以用一个简单的判别逻辑当GNSS位置与INS预测位置的差超过某个阈值比如10米时把R矩阵放大5~10倍减少异常GNSS观测的冲击。这个技巧在隧道口、高楼阴影区非常管用。6. 扩展玩法从NaveGo到自己的算法框架NaveGo虽然好用但它毕竟只是一个“参考实现”。我在实际项目里把它当成了“算法验证平台”而不是“最终方案”。有几个方向值得拓展替换EKF为其他滤波算法NaveGo的EKF结构很清晰你可以把时间更新和量测更新替换成误差状态卡尔曼滤波ESKF、无迹卡尔曼滤波UKF甚至因子图优化。因为它的数据读取、机械编排、误差评估模块都是现成的专注改滤波部分就可以了。增加GNSS/INS紧耦合NaveGo是松耦合你可以在此基础上把GNSS伪距、载波相位观测加进去构建紧耦合EKF。这个工作量大一些但NaveGo已经帮你把INS部分搞定了不用从零开始。接入RTK定位结果NaveGo自带RTK测试脚本但实际使用时需要把RTK引擎的输出比如NMEA GNGGA或RTCM解算结果转成标准的位置速度输入。我试过把u-blox F9P的RTK定位结果灌进NaveGo水平精度从2米提升到3~5厘米效果立竿见影。用于传感器选型评估在买IMU或GNSS模组之前先用NaveGo做仿真输入候选器件的噪声参数看组合导航精度能不能满足你的项目指标。这个用法比直接买设备回来做实测省太多钱和时间。另外NaveGo的另一个特点是代码量适中全套代码不到一万行逻辑清晰非常适合用来学习INS/GNSS组合导航的工程实现。我强烈建议不要只把它当成“黑盒工具”花两天时间把ins_mech.m和ins_predict.m逐行读懂对理解捷联惯导系统会有质的提升。7. 实际使用中的心得体会最后聊几点我在过程中积累的个人体会。第一NaveGo这种开源框架最大的价值不是“即开即用”而是“给你一个确定能跑通的基线”。很多做定位算法的团队一开始就在自己的代码上反复调参最后调了一周连收敛都做不到原因就是没有一个标准答案做对照。你先把NaveGo跑通得到一组参考结果再把它替换成自己的模块逐模块对比中间变量排查效率会高很多。第二IMU参数的标定真的很重要别偷懒。NaveGo自带的IMU参数只是针对特定型号的如果你换了传感器还不改参数滤波性能一定会受影响。建议花半天时间用静态数据做Allan方差分析把量化噪声、角度随机游走、零偏不稳定性、速率随机游走等参数都标定出来填入NaveGo对应的imu_noise.mat或配置文件里。第三GNSS数据的质量直接影响组合导航的上限。NaveGo对GNSS的数据格式要求不严苛但对时间同步和坐标系很敏感。你在野外采集数据时最好给GNSS模组和IMU做一个简单的时间同步板哪怕用同一个单片机采集并打时间戳也比事后用软件方法对齐强得多。用NaveGo跑了这么多组数据我最大的感受是组合导航算法的坑基本都藏在细节里。科里奥利修正、圆锥补偿、时间同步、坐标系定义任何一个环节出错结果都会莫名其妙地差。有这样一个开源框架在至少你遇到问题时能有一个可信的参照系去排查这比自己闷头造轮子靠谱太多了。如果你正准备做GNSS/INS组合导航强烈建议先把NaveGo吃透再谈自己的算法创新。本文还有配套的精品资源点击获取