简介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做预测时逐帧读IMUGPS只在它自己的时间戳上触发量测更新也就是说两路数据不需要等间隔对齐但时钟基准必须一致。常见坑是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是100HzGPS是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(end1) 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都会强制走一遍完整链路先静态校验再杆臂补偿最后才看轨迹这套顺序帮我省了很多死磕时间。希望帮到你。本文还有配套的精品资源点击获取