简介：NaveGo是一套面向低成本惯性测量单元（IMU）与全球定位系统（GPS）组合导航的开源框架，主要服务于高校学生、研究者与嵌入式导航开发者，用来系统学习并实践卡尔曼滤波、无迹卡尔曼滤波等传感器融合算法。压缩包共77个文件，以63个Matlab脚本为核心，覆盖姿态更新、速度/位置解算、IMU与GPS数据生成、传感器误差建模、Allan方差分析与RMSE精度评估等模块；另附8个.mat数据文件、仿真说明PDF、README及许可文件，整体约54.66MB。借助文档与示例脚本，可以从传感器仿真和误差标定出发，逐步完成惯导机械编排、GNSS/INS松组合解算及精度评估，理解组合导航如何弥补GPS在信号遮挡时的定位不足。同时代码结构清晰、易于修改，可进一步扩展磁力计、光流等传感器，适合课堂教学与毕业设计参考。当前已有2247人学习该资源，对于希望从理论走向动手验证的导航方向读者尤其合适，也便于在此基础上做算法对比与二次开发。
1. NaveGo这个开源组合导航框架：解决的是INS/GPS融合定位落地问题
NaveGo这个开源组合导航框架，解决的就是无人车和移动机器人里INS/GPS融合定位的落地问题。陀螺和加速度计会漂，GPS会掉线，两者一融合才能稳定输出姿态和位置，但自己从零搭一套松组合卡尔曼滤波，光处理时间同步、杆臂、坐标系转换就要花一两周。NaveGo用MATLAB写好了从IMU/GPS数据输入到误差状态EKF滤波输出的完整链路，还自带传感器模拟器，没实车也能先调通算法。它适合做课程设计、毕设验证、算法对照，也适合想快速落地组合导航demo的从业者。我用它跑了几组数据，发现只要输入格式对齐，十分钟就能出轨迹和误差曲线。
2. 框架与数据流：从原始IMU到导航解的三个关键设计
2.1 输入数据格式：IMU和GPS矩阵里到底放什么
NaveGo的数据接口很朴素，就是二维矩阵。每一行代表一个采样时刻，列顺序由包内函数注释约定，但最常见的写法是：IMU矩阵[t, wx, wy, wz, fx, fy, fz]，t是秒，角速度单位是 rad/s，加速度比力单位是 m/s^2，注意比力包含了重力分量。GPS矩阵[t, lat, lon, h, vn, ve, vd]，经纬度有的接口用度、有的用弧度，速度一般用北东地（NED）分量，单位 m/s。
时间轴是整个组合导航的命门。NaveGo做预测时逐帧读IMU，GPS只在它自己的时间戳上触发量测更新，也就是说两路数据不需要等间隔对齐，但时钟基准必须一致。常见坑是IMU记录的是开机以来的毫秒数，GPS是UTC秒数，两者差一个固定偏移或缩放，跑出来直接发散。我通常在预处理阶段先把所有时间戳换算成从0开始的秒数，并做单调性检查。
% 假设原始CSV列：t_ms, gx_deg, gy_deg, gz_deg, ax, ay, az raw = readmatrix('imu_raw.csv'); t_sec = raw(:,1) ./ 1000; % 毫秒转秒 imu = zeros(size(raw,1), 7); imu(:,1) = t_sec; imu(:,2:4) = deg2rad(raw(:,2:4)); % 陀螺仪deg/s转rad/s imu(:,5:7) = raw(:,5:7); % 加速度计保持m/s^2 % 检查时间是否单调递增 assert(all(diff(t_sec) > 0), '时间戳必须严格单调递增');这里把时间转成秒、角速度转成弧度，是几乎所有导航库的统一要求。deg2rad最容易漏，很多原始数据直接给 deg/s，不转换的话姿态会以每秒57倍的速度疯转。加速度计列一般不需要转，除非你拿到的是重力归一化后的值。
GPS矩阵的经纬度，最稳的做法是预处理时全部转成弧度，因为滤波内部做航向和位置差分时，用弧度才能和地球模型参数统一。速度分量建议直接用北东地（NED），如果原始给的是ECEF，先转换。还有一个细节：GPS的高度通常是椭球高，而IMU积分得到的是相对于初始点的增量高度，两者混用时会在垂直通道产生常值偏差。NaveGo的处理方式一般是从初始位置开始差分，所以输入时要把GPS高度也减去起始高度，或者让滤波器自己估计一个高度偏移。
2.2 融合逻辑：误差状态卡尔曼滤波的预测与更新
NaveGo的融合核是误差状态EKF。为什么不用普通EKF直接对姿态、位置、速度做估计？因为姿态是个流形，直接给四元数加高斯噪声会破坏归一化约束，数值上很容易漂掉。误差状态模型把姿态误差放到三维局部切空间里，线性化精度更高，这也是商用组合导航的主流做法。
状态量一般包括位置误差、速度误差、姿态误差、陀螺零偏、加速度计零偏。IMU每次采样触发预测：用上一帧的姿态把比力转到导航系，积分出速度和位置，同时累积零偏估计。GPS每来一帧就做量测更新：用预测位置和GPS位置之差构造新息，修正状态量和协方差矩阵。整个流程就是标准的“预测+更新”循环，NaveGo把每个环节拆成可替换的函数，所以你可以只换掉GPS观测模型，把位置更新改成伪距更新，就能往紧组合方向改。
这里要注意异步更新带来的观感问题。如果IMU是100Hz，GPS是1Hz，那一秒里有99次纯预测和1次预测+更新。纯预测阶段协方差会一直膨胀，GPS更新后收缩，这是正常现象。看误差曲线时不要期待它一条直线，只要均值稳定、没有周期性的发散节点，就算正常。我见过有人看到协方差锯齿就疯狂调小Q，结果滤波器变迟钝，真实误差反而变大。
2.3 自带模拟器：怎么生成可信的IMU/GPS测试数据
NaveGo附带模拟器，可以按你设定的轨迹生成带噪的IMU和GPS原始数据。这对我这种没有实车的人特别友好，先在仿真里把滤波器参数调到合理区间，再换真机数据去验证。模拟器里需要配置：运动轨迹类型（直线、圆周、加减速）、IMU采样率、GPS采样率、陀螺噪声密度、加速度计噪声密度，以及初始位置和初始姿态。
% 模拟器配置 imu_freq = 100; % IMU采样率 Hz gps_freq = 1; % GPS采样率 Hz sim_time = 300; % 仿真时长 s % 陀螺仪噪声：车规级约0.3 deg/sqrt(h) gyro_noise_density = 0.3; % 加速度计噪声：约0.03 mg/sqrt(Hz) accel_noise_density = 0.03;噪声密度换算成协方差时，要先乘以采样率的平方根，再考虑带宽。很多人在模拟器里填完参数就直接跑，出的轨迹比真机还平滑，那是因为噪声密度单位搞错，弄成了噪声方差，滤波器把传感器想象得过于完美。模拟器最大的价值是参考真值：有了它，你就能直接量化滤波器每米的误差，而不是对着曲线猜“看起来收敛了”。你还可以人为在某一帧给GPS位置加几十米跳变，测试滤波器对新息异常的抗性，这比拿真车磕碰测试成本低太多。
3. 快速跑通：把自带Demo跑出轨迹的完整步骤与参数解读
3.1 环境准备：目录结构和路径设置
NaveGo是用MATLAB写的，不需要编译，但环境变量必须配好。解压后，我习惯把整个目录加到MATLAB搜索路径里，包括所有子目录，否则调用函数时一旦位置不对就报“未定义函数”。
addpath(genpath('NaveGo')); % 添加全部子目录到搜索路径 savepath; % 保存路径，下次自动加载genpath('NaveGo')会递归添加路径下所有子目录，避免漏掉某个工具函数。然后运行示例脚本，如果报错说某个脚本或函数未定义，说明路径没加好或目录名不对。另一个常见环境问题是MATLAB版本太旧，示例脚本里用了新版语法，比如arguments块或tiledlayout，老版本会直接报错。先跑自带测试脚本确认无报错，再往下做。
3.2 修改运行参数：采样率、时长、初始位置、噪声等级
跑通示例最简单的方式是打开自带demo脚本，里面默认参数能直接出结果。但默认参数不一定适合你的场景，需要改四个关键值：仿真时长、初始经纬度、IMU采样率、GPS采样率。仿真时长决定数据量，初始经纬度决定地球模型参数和重力计算起点，采样率决定预测与更新的节奏。
% 修改示例参数 sim_duration = 600; % 从300秒改成600秒 imu_fs = 100; % IMU 100Hz gps_fs = 1; % GPS 1Hz init_lat = deg2rad(27.98); % 某测点的纬度，注意转弧度 init_lon = deg2rad(86.92); % 某测点的经度，注意转弧度注意两点。第一，示例脚本里经纬度可能默认是度数，如果直接给弧度值而不改换算，位置会差到离谱。第二，GPS采样率不建议超过IMU频率的十分之一，否则每个更新周期内预测步数太少，滤波器修正效果变差，协方差会来回震荡。我一般用100Hz IMU配1Hz GPS，或者200Hz配5Hz，这样的节奏下松组合最稳。
3.3 看结果：轨迹图、误差曲线和收敛判断
运行完示例，MATLAB会弹出几个图。第一个是平面轨迹图，看整体路径是否和预设轨迹一致；第二个是位置误差图，横轴时间，纵轴北、东、地三个方向的位置误差；第三个是速度误差图，第四个是姿态误差图。很多人只关心轨迹像不像，却忽略误差曲线，这不对——轨迹再像，也可能整体偏了一个常数，误差曲线才能看出收敛质量。
判断收敛有两个硬指标：位置误差曲线在运行中段以后明显下降且保持平稳；速度误差在零值附近震荡，振幅小于0.1m/s。组合导航有典型收敛过程：前几十秒初始对准和零偏估计不到位，误差较大，之后随GPS观测进来逐渐收敛。如果误差从开始就在一两米内抖动，说明初始对准已经不错；如果误差几百米且不收敛，基本都是输入数据格式或参数配置问题，别继续调滤波器，先回头检查矩阵。
我还会看新息序列。新息是GPS量测值与滤波预测值之差，如果新息均值明显偏离零，说明观测模型里某个坐标转换或杆臂补偿没做对。这一步比盯着误差曲线更容易早发现问题，因为新息反映的是单步观测质量，而误差曲线是多个误差源的叠加结果。
4. 接入自己的数据：IMU/GPS文件转换、杆臂补偿与调参顺序
4.1 把CSV转成NaveGo矩阵：单位统一是第一步
真实设备采集的数据五花八门，最常见的是CSV文本。NaveGo只认矩阵，所以转换是第一步。前面提过，陀螺要转成 rad/s，加速度计保持 m/s^2，时间戳统一成秒。GPS部分还要注意速度单位，有的设备给 km/h，有的给 m/s，量纲不一致会导致量测更新错乱。
GPS数据通常比IMU稀疏，且采样时间不一定严格整秒，所以需要重采样到固定频率。我一般只保留距离整秒最近的观测帧，不插值。经纬度在距离较短时虽然可以线性插值，但低端GPS的原始时间抖动大，插值反而引入伪观测。更稳的做法是只在内存里保留离整秒最近的帧，丢掉其他帧。
% 读取原始GPS gps_raw = readmatrix('gps_raw.csv'); % 统一单位为：秒、弧度、弧度、米、m/s t_gps = (gps_raw(:,1) - gps_raw(1,1)) / 1000; lat = deg2rad(gps_raw(:,2)); lon = deg2rad(gps_raw(:,3)); alt = gps_raw(:,4); vn = gps_raw(:,5); ve = gps_raw(:,6); vd = gps_raw(:,7); % 按最近邻取整秒帧 sec_idx = round(t_gps); [~, unique_idx] = unique(sec_idx); gps = [sec_idx(unique_idx), lat(unique_idx), lon(unique_idx), alt(unique_idx), vn(unique_idx), ve(unique_idx), vd(unique_idx)]; % 删除重复时间戳 gps(diff(gps(:,1)) == 0, :) = [];这里用unique去重，再用diff删除重复行。原因是低端GPS接收机可能在同一秒输出多帧，而一次量测更新只应消费一帧。不处理的话，同一时刻的多次观测会让新息权重重复累加，相当于人为增大GPS置信度。
4.2 杆臂和时间同步：组合导航最容易出错的两个环节
杆臂是GPS天线相位中心和IMU测量中心之间的空间向量，在车上是几十厘米到一两米的事。忽略它，位置观测和IMU推算的位置就不是同一个点，转弯时位置误差会周期性起伏。处理方式是把杆臂从机体坐标系转换到导航坐标系，再修正GPS量测值。
% 杆臂在机体坐标系下的分量：前右下（米） lever_arm_b = [0.5; 0.2; -0.1]; % 用当前姿态四元数转方向余弦矩阵 C_b_n = quat2dcm(att_est); lever_arm_n = C_b_n * lever_arm_b; % GPS量测位置修正：gps_pos_meas = gps_raw_pos - lever_arm_n注意减号方向取决于杆臂定义。我习惯定义为“从IMU指向天线”，在滤波里用GPS测量减去这个向量。如果你发现直线正常、转弯误差冲出几个米，多半是杆臂方向符号反了。杆臂的精确值要用卷尺量IMU和天线在车体坐标系下的三维距离，别用目测估计。
时间同步同样隐蔽。有些GPS模块的PPS和串口输出时刻之间有固定延迟，可能达到几十毫秒，高速运动下会造成位置偏差。如果IMU和GPS各自有独立时钟，跑十分钟后还会产生毫秒级漂移。实用做法是预处理时把两路时间戳画在同一坐标系，看是否有固定偏移，偏移稳定就直接加减常数。如果漂移明显，就得用外部参考时钟重写时间戳，或者把GPS时间戳附近的IMU数据做插值对齐。
4.3 调参顺序：先噪声后模型，别一上来就调滤波增益
拿到真实数据后，调参跑不掉。我见过很多人直接改过程噪声Q和量测噪声R，一会儿大一会儿小，最后精度越调越差。NaveGo里Q和R的物理意义分别是IMU和GPS的噪声水平，应该由器件手册或实验标定得到，不是凭手感。
我的调参顺序是：先用静态数据标定陀螺和加速度计的噪声密度，可以用Allan方差或直接看功率谱密度；然后把噪声密度换算成连续性协方差填进Q。R用GPS静止时的位置标准差来填。如果轨迹上还有高频抖动，再去微调Q里零偏随机游走系数，而不是大幅动R。整个过程中用一个对比脚本记录不同参数下的RMS位置误差，用数值说话。
% 参数对比：多组R值设置 r_list = [1.0, 0.5, 0.1]; % 位置量测噪声标准差（米） for r = r_list filter.R_pos_std = r; run_filter_and_log(); % 封装好的运行函数 rms_err(end+1) = compute_rms_error(); % 与参考真值比 end这样能直观看到：R过大则滤波迟钝，轨迹滞后；R过小则GPS噪声被过度信任，轨迹出现锯齿。先找到合理区间，再谈进一步优化。真实数据里R往往和器件标称值差两三倍，原因是多路径误差没有建模，所以可以先从标称值放大两三倍开始试。
5. 避坑指南：跑NaveGo时最常见的5个翻车点
5.1 时间戳单位混用导致滤波器发散
现象：北向位置误差像开闸放水，几十秒内冲到几百米，而且只增不减。
原因：IMU时间戳是设备开机以来的毫秒整数，GPS时间戳是UTC秒，预处理只把其中一个除以1000，另一个没动，导致两路时间轴比例尺不一样。滤波器每更新一次，相当于用错误间隔的IMU数据做了一轮积分，误差自然滚雪球。
解决：在数据装载入口强制检查时间戳范围，如果最大值大于1e5，基本可以断定是毫秒，先除以1000再做单调性校验。我还会画一张时间对齐散点图，如果IMU和GPS时间差呈现线性趋势，直接说明时钟基准不一致。从那以后我每次处理新数据都会先做这一步，等于给时间轴上了保险。
5.2 经纬度单位搞错导致位置跳变
现象：轨迹在东向、北向方向乱跑，甚至出现坐标负值或超过360度。
原因：NaveGo某些接口期望经纬度是弧度，但设备输出是度数，预处理没有转，导致经度方向等效相差57倍。滤波器把57倍的位置误差当成新息，修正方向自然错乱。
解决：统一在预处理阶段转成弧度，并且不只在调用处转换。因为滤波器内部多处会用经纬度计算地球半径、重力和子午圈曲率半径，内部一致性必须保证。我习惯在矩阵构建完成后打印max(abs(rad2deg(lat)))，如果看到大于360，说明某个环节把弧度当度写了，马上就能定位。
5.3 初始对准不充分导致航向漂移
现象：前几十秒速度误差和位置误差都很大，随时间推移位置误差收敛，但航向仍缓慢漂移，直线段出现横向误差。
原因：组合导航的初始航向角没有可靠来源，加速度计只能确定水平姿态，航向需要磁力计或车辆运动激励。静止初始化如果只持续10秒，航向可观测性不足，陀螺零偏也没估计好。
解决：静止初始化至少保持60秒以上，让滤波器充分估计零偏。如果场景不允许长时间静止，就加一段“走直线启动”，用速度方向约束航向。NaveGo自带示例通常会有一段静止段，别删掉，它是收敛质量的地基。
5.4 GPS观测野值把滤波器拉偏
现象：误差曲线在其他时刻正常，但在某个时间点突然跳到几十米，再花几十秒慢慢回来，轨迹上出现一个明显鼓包。
原因：城市峡谷、多路径或接收机内部异常导致GPS位置跳变，新息幅值远超正常。标准卡尔曼滤波对野值没有抗性，它会把野值当真实观测，强行把状态拉过去。
解决：量测更新前做粗差检测，判断新息是否落在合理范围内。NaveGo默认没有这个保护，需要在调用量测更新前自己包一层。
% 新息粗差检测 innovation = z - hx; % 观测残差 S = H * P * H' + R; % 新息协方差 if innovation' / S * innovation < chi2_95 % 5.991 对应二维观测 % 正常更新 else % 拒测，只预测不更新 endchi2_95取值5.991对应2自由度观测（东、北位置），如果检测的是三维速度或完整三维位置，阈值要改成7.815。这个门槛卡得太严会丢正常观测，卡得太松又拦不住大野值，我一般取3到5倍新息标准差。
5.5 杆臂未补偿导致转弯误差耦合
现象：车辆直线行驶位置误差很小，转弯过程中北向、东向误差出现几十厘米到一两米的波浪，转弯结束后恢复。
原因：GPS天线装在车顶，IMU装在车体内，位置差在转弯时会产生一个与角速度有关的虚拟速度差。不补偿杆臂，滤波器会认为两个传感器在同一个点，转弯时产生额外新息。
解决：量测更新前把杆臂向量旋转到导航系并修正GPS位置，前面4.2节已经写了具体代码。更稳的做法是在滤波器状态中显式加入杆臂的在线估计，但NaveGo默认不支持，只能预修正。我用低精度IMU做实验时，杆臂误差是转弯误差的最大来源，比陀螺零偏还影响明显。所以每次换车都必须重新量杆臂，不能复用上一版的参数。
6. 进阶技巧：用真实低精度IMU跑出可用结果的三个校验方法
6.1 静态零速校验：先确认静止不发散
拿到一块低精度IMU，先用NaveGo跑一段静止数据，这是最便宜的自检方法。把IMU水平放稳，采两分钟数据，输入滤波器。如果输出速度在噪声带内震荡，没有趋势性漂移，位置误差能保持几米以内，说明基础模型和参数没问题。如果静止也发散，先查零偏是否被正确估计，再回头查单位。
6.2 轨迹导出：把滤波结果转成KML/CSV回放
NaveGo的结果通常存在结构体里，包含时间、经纬度、高度、速度、姿态。要可视化，就把经纬度和高度导出成CSV，再用MATLAB的geoplot或外部工具转成KML。经验是导出的经纬度必须和输入保持同一个单位约定，否则地图上直接跑到海里。我有一回在导出时忘了把弧度转回度，卫星图上一整条轨迹落在太平洋中间，查了好久才发现是单位问题。
6.3 松组合到紧组合：什么时候才值得换
如果只有GPS位置和速度，NaveGo的松组合已经能解决90%的问题。需要紧组合的场景一般是城市峡谷GPS频繁遮挡，只有少量卫星可锁定，伪距观测断续且不完整。这时松组合的“有位置才更新”模式会大量时间空转，紧组合直接用伪距残差更新，能撑得更久。但紧组合需要原始观测、卫星星历和更复杂的测量模型，调试量成倍增加。我建议先用松组合把零偏、杆臂、时间戳这些基础问题解决，再考虑改紧组合，否则会同时面对两套模型的bug。
从那以后，我每次跑NaveGo都会强制走一遍完整链路：先静态校验，再杆臂补偿，最后才看轨迹，这套顺序帮我省了很多死磕时间。希望帮到你。
本文还有配套的精品资源，点击获取
