简介面向惯性导航与组合导航开发者的一套MATLAB开源程序基于NaveGo项目实现GPS与IMU数据融合采用扩展卡尔曼滤波EKF对IMU的累积误差进行实时校正适用于无人系统、车载导航、水下定位等需要连续位置与姿态信息的场景也适合作为研究生课题的参考基线。压缩包共66个文件以56个M文件为主覆盖EKF核心实现、IMU/GPS动态模型、Allan方差分析、RINEX/RTK数据读取、数据预处理与误差后处理等完整流程另有5个MAT数据文件、说明文档、许可文件等整体约50.4MB。已有8546人学习代码结构层次清晰既支持纯仿真数据验证也能处理真实传感器数据便于反复拆解和组合模块。通过阅读和调试这套代码可以深入理解GPS/IMU融合中的滤波建模、参数整定和误差抑制技巧并直接复用于自研导航算法节省从零搭建原型的时间。 你有没有注意过一个细节手机导航进隧道或者穿过高架桥底之后定位点并不会卡在原地而是会沿着道路继续平滑往前走。很多人以为这是地图软件的惯性预测其实背后真正干活的是惯性导航系统——准确说是GPS和IMU的组合导航。我前段时间刚整理完一套基于MATLAB的惯导开源程序核心就是GPS和IMU的数据融合代码量不大但把惯导解算、坐标转换、卡尔曼滤波、误差评估这条完整的链路都串起来了。这篇文章不写泛泛的原理直接把程序结构、核心代码、以及我调参时踩过的坑掰开讲清楚想拿来做课程设计、毕设起步或者入门组合导航的朋友可以直接照着抄。为什么一定要把GPS和IMU放在一起用而不是单靠其中一个因为这两个传感器就像性格完全相反的搭档各有各的致命短板只有搭在一起才能互补。这套开源程序的核心思路就是围绕这个互补性设计的用IMU的高频输出做连续姿态和轨迹推算用GPS的低频绝对定位去定期修正IMU积累的漂移中间用卡尔曼滤波器把两个来源的数据合理地拧成一股绳。1. 为什么车载导航离不开GPS和IMU的搭伙过日子我刚开始接触惯导时一直有个疑惑既然GPS能直接给出经纬度坐标那直接用不就行了为什么还要费劲做惯导融合直到自己在城市道路上做了几次实测才彻底搞明白这个问题。GPS和IMU各自的局限其实非常明显而它们又恰好能弥补对方的短板。1.1 GPS的慢性子和IMU的急性子GPS的优点是能给出绝对位置长时间运行不会累积误差因为每一帧都独立测距解算。但它的短板也很突出第一输出频率低消费级GPS接收机一般只有5到20Hz高动态场景下两个定位点之间的轨迹只能靠猜第二非常依赖卫星信号隧道、地下车库、高架桥底、茂密树荫下定位误差会瞬间从米级漂到几十米甚至直接丢星第三GPS的观测噪声不是均匀的城市峡谷里的多径效应会让定位结果在某些路段出现几米到十几米的跳变。IMU的优缺点是反过来的。它全靠自身测量角速度和加速度不依赖任何外部信号输出频率可以做到100Hz以上短时间内的相对位移推算非常平滑动态响应也快。缺点则在于积分漂移陀螺仪的微小零偏会通过姿态积分不断累积加速度计的微小偏差会通过二次积分直接变成位置误差。理论上讲一个普通的MEMS惯性器件静止几分钟后推算出的位置可能就漂了几百米这在纯惯导模式下是没法用的。这两者放在一起看就很清楚了GPS像一个记性很好但反应慢半拍的慢性子IMU像一个反应极快但记性越来越差的急性子。慢性子负责时不时纠正大方向急性子负责在两三次纠正之间把每一帧运动轨迹补出来。这就是组合导航最朴素的动机。1.2 松耦合融合链路为什么开源程序选它数据融合按架构一般分松耦合和紧耦合两类。松耦合是先把GPS和IMU各自解算成导航结果然后把这两个结果送进滤波器做位置、速度层面的融合紧耦合则是把GPS的原始伪距、载波相位观测直接放进滤波器和IMU原始数据一起联合解算。这套MATLAB开源程序用的是松耦合。原因很直接紧耦合状态维度大、观测模型复杂、初始化和故障检测都要额外处理对入门学习和原型验证来说性价比不高。松耦合的模块边界非常清晰IMU姿态解算可以单独调试GPS坐标转换可以单独验证最后融合环节只关心两个输入排查问题的时候非常方便。在MATLAB这种以快速验证为主要目标的开发环境里松耦合几乎是唯一合理的选择。整个融合链路可以概括成四步第一步IMU高频输出经过姿态解算把载体坐标系下的角速度和加速度转换到导航坐标系第二步用加速度在导航坐标系下做积分推算出速度和位置增量第三步GPS输出的经纬度坐标被转换到以起始点为原点的ENU东-北-天直角坐标系下第四步两者在卡尔曼滤波器里做融合用GPS观测结果修正IMU推算出来的状态。2. 开源程序的总体框架与初始化流程这套程序不是我在一个文件里堆了几百行代码那种做法而是按功能拆成了多个独立的脚本和函数结构非常标准。整个代码的脉络就是数据进、轨迹出中间每一步都用单独的模块处理。2.1 程序文件结构与职责划分拿到程序后先不要急着运行花几分钟把文件结构看清楚后面调参会顺畅很多。文件/函数名职责输入输出main.m主脚本控制整个流程原始数据文件路径、参数配置融合轨迹、误差曲线、图表load_data.m读取IMU和GPS原始数据统一时间轴数据文件夹路径结构体格式的IMU数据、GPS数据imu_attitude_update.m四元数姿态解算与加速度去重力上一时刻状态、IMU原始输出、dt当前时刻姿态四元数、导航系加速度gps_to_enu.m把GPS经纬度转换为ENU局部坐标经纬度序列、参考原点ENU坐标系下的位置序列kalman_predict.m卡尔曼滤波预测步当前状态向量、状态协方差、IMU数据、dt预测后的状态向量与协方差kalman_update.m卡尔曼滤波更新步预测状态、GPS观测量、观测噪声矩阵修正后的状态向量与协方差evaluate_trajectory.m与真值或参考轨迹做误差统计融合轨迹、参考轨迹RMSE、最大误差等指标visualize_results.m绘制轨迹对比与误差曲线融合结果、GPS轨迹、IMU推算轨迹图表窗口main.m是整个程序的入口它做的事情按顺序展开就是设置参数、加载数据、初始对准、进入主循环、输出评估、画图。主循环里每一步先调kalman_predict再判断当前时刻是否有GPS观测如果有就调kalman_update没有就直接用预测结果输出。这套流程和实际工程里嵌入式组合导航的框架几乎一致只是换成了MATLAB的语法。2.2 开场10秒的初始对准为什么不能跳程序在进入主循环之前会有一段静止数据通常取前10秒这段数据不要随便删。惯导系统有一个铁律姿态解算是相对推算必须要知道初始姿态才能算后面每一帧的旋转加速度积分算速度和位置也一样必须要知道初始速度通常是零和初始位置。如果初始姿态差了几度位置漂移会随着时间二次方放大跑完整个轨迹误差直接起飞。初始对准在程序里做的事可以拆成三步。第一步取静止段的加速度计三轴输出求平均再利用重力矢量方向反推初始横滚角和俯仰角。第二步如果数据里有磁力计用磁场方向获取初始航向角如果没有磁力计也可以用GPS运动方向作为航向参考但静止状态下GPS没有方向输出这一点需要注意。第三步对静止段的陀螺仪和加速度计输出求平均作为传感器零偏的初值在后续滤波中这些零偏还会被状态估计器进一步修正。我记得最初用零偏没有校准的数据跑程序时前几秒轨迹看起来还是直的两分钟之后轨迹就开始画圈速度的大小也在乱跳。后来把初始对准代码补上问题立刻消失。如果你跑别人工程时发现轨迹一开始就偏得很离谱先检查这一步有没有处理正确。3. 融合主循环卡尔曼滤波的预测-更新代码拆解主循环是整个程序的心脏卡尔曼滤波又是主循环的心脏。很多教程一上来就抛出一堆矩阵公式把人看懵其实从代码角度理解卡尔曼滤波要容易得多预测步就是你相信IMU的推算结果把状态往前推一步更新步就是你用GPS的新观测来纠正预测结果两者之间的权重由协方差矩阵自动决定。3.1 状态向量与运动模型的选择这套程序的状态向量是15维包括3个位置分量、3个速度分量、4个姿态四元数、3个陀螺仪零偏、3个加速度计零偏。选择四元数而不是欧拉角是因为四元数没有万向锁问题也不需要像方向余弦矩阵那样维护9个冗余参数在工程上计算效率和数值稳定性都更好。状态向量定义如下% 状态向量结构 % x(1:3) - ENU坐标系下的位置 [px, py, pz] % x(4:6) - ENU坐标系下的速度 [vx, vy, vz] % x(7:10) - 姿态四元数 [qw, qx, qy, qz] % x(11:13) - 陀螺仪零偏 [bgx, bgy, bgz] % x(14:16) - 加速度计零偏 [bax, bay, baz]这里有个细节把IMU零偏放进状态向量意味着滤波器可以在线估计并修正零偏而不只是把它当成固定常量减掉。这一步对长时间导航非常关键因为MEMS陀螺仪的零偏会随温度和时间缓慢变化如果不在线估计姿态漂移会越来越严重。状态运动模型对应的是连续时间运动学方程在代码里做了离散化。核心的预测逻辑分成姿态、速度、位置三块递推。姿态更新用四元数微分方程的离散形式速度更新把导航系加速度减去重力矢量后乘以步长位置更新用速度和加速度的二阶近似。3.2 预测步的核心代码逻辑下面这段是程序里kalman_predict.m的核心代码做了简化便于阅读。这里的关键操作顺序是先用加速度计输出和当前姿态计算导航系下的加速度并扣除重力再更新速度最后用速度更新位置。顺序不能乱因为位置更新需要用到刚刚更新完的速度。function [x_pred, P_pred] kalman_predict(x, P, imu_data, dt) % imu_data: [acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z] acc_body imu_data(1:3) - x(14:16); gyro_body imu_data(4:6) - x(11:13); % 1. 姿态四元数更新 q x(7:10); omega [0, -gyro_body(1), -gyro_body(2), -gyro_body(3); gyro_body(1), 0, gyro_body(3), -gyro_body(2); gyro_body(2), -gyro_body(3), 0, gyro_body(1); gyro_body(3), gyro_body(2), -gyro_body(1), 0]; dq 0.5 * dt * omega * q; q_new q dq; q_new q_new / norm(q_new); x(7:10) q_new; % 2. 比力转换到导航系后减去重力 acc_nav quatrotate(quatconj(q_new), acc_body); acc_nav acc_nav - [0; 0; 9.80665]; % 3. 速度更新 x(4:6) x(4:6) acc_nav * dt; % 4. 位置更新 x(1:3) x(1:3) x(4:6) * dt 0.5 * acc_nav * dt^2; % 5. 状态协方差递推一阶线性近似 F compute_jacobian(x, imu_data, dt); Q diag([0.01, 0.01, 0.01, ...]); % 过程噪声矩阵 P_pred F * P * F Q; end这里要提醒一个非常容易忽略的点四元数更新完以后一定要做归一化。四元数的模长应该恒为1但由于数值积分的误差模长会缓慢偏离如果不强制归一化姿态矩阵会逐渐退化最终导致速度、位置的解算完全失真。我见过不止一次因为忘写归一化导致程序结果慢慢漂移到离谱的情况。状态转移矩阵F是用雅可比矩阵做了一阶线性化。严格做法是用误差状态模型推导解析表达式但在入门程序里直接对非线性函数做数值差分也能跑出不错的效果代价是计算时间稍长。理解了这一层再看后续Q矩阵的调参就有依据了Q矩阵本质上描述的是你对IMU推算模型精度的信任程度。3.3 更新步GPS观测如何拉回漂移GPS观测进入滤波器的方式很直观。程序把GPS给出的是经纬度先转成ENU坐标那么观测方程就是状态向量里的位置分量加上观测噪声观测矩阵H非常简单就是一个选择位置状态的稀疏矩阵。function [x_upd, P_upd] kalman_update(x_pred, P_pred, gps_enu, R) % gps_enu: GPS观测的ENU位置 [x; y; z] H zeros(3, 15); H(1,1) 1; H(2,2) 1; H(3,3) 1; z_pred H * x_pred; y gps_enu - z_pred; % 新息残差 S H * P_pred * H R; % 新息协方差 K P_pred * H * inv(S); % 卡尔曼增益 x_upd x_pred K * y; P_upd (eye(15) - K * H) * P_pred; end卡尔曼增益K在这里起的作用就是自动调节权重当GPS噪声小R矩阵小时K变大滤波结果更相信GPS当IMU过程噪声小Q矩阵小时P预测值小K变小滤波结果更相信IMU推算。这个自适应调节的过程完全不需要人工干预是卡尔曼滤波最优雅的地方。有一点值得展开新息y这个值本身携带了有效信息。正常情况下如果滤波器收敛且传感器不出异常新息应该是一个均值为零的高斯序列。如果你在新息序列里看到明显的非零均值或周期性波动说明某一环节出了问题比如时间没有对齐或物体运动模型不匹配。我习惯把新息单独画出来看分布如果明显偏离零均值基本可以断定观测模型或者坐标转换哪里写错了。4. 最容易翻车的几个细节和调参经验程序跑通只是第一步真正让结果变靠谱的是调参和排错。这个程序我在实验室里跑了很多轮也和同学在不同数据集上验证过总结出几个高频翻车点基本都是新手必踩的坑。4.1 单位、坐标系、时间戳三座大山这三个问题看起来简单但在组合导航里是实实在在的坑每一个都能让结果错得莫名其妙。第一个是单位。IMU原始数据里加速度计的单位可能是m/s^2也可能是g重力加速度倍数陀螺仪的单位可能是rad/s也可能是deg/s。如果只按照度/s的数据算姿态更新速度会快57倍以上轨迹直接炸掉。我在程序开头加了一个单位检查函数读取数据后先按列输出最大最小值和数量级人工确认一遍再继续跑。0.1到10量级的加速度大概率是m/s^20到360量级的角速度大概率是deg/s用这个经验能快速判断。第二个是坐标系。GPS经纬度转换到ENU时参考原点的选择会影响所有转换后坐标的数值大小。程序里统一取第一帧GPS经纬度作为参考原点这样整个轨迹都在一个相对坐标下显示便于和IMU推算结果比较。另外IMU输出的加速度方向是否和ENU坐标系定义一致也需要确认。不同传感器的安装方向和轴系定义五花八门有的把Z轴朝上有的朝下有的把X轴当前进方向。如果轴系定义不对融合结果会出现左右颠倒或者上下颠倒。第三个是时间戳。IMU输出频率100HzGPS输出频率10Hz两者的时间戳不是一一对应的。程序里的做法是维护一个循环时钟每来一帧IMU就做一次预测同时判断当前时刻是否已经超过下一帧GPS的时间戳如果超过了就用最邻近的GPS观测做一次更新。这里的核心逻辑是IMU驱动主循环GPS插队更新不是简单地把两组数据按序号配对。4.2 Q矩阵和R矩阵的初值怎么给调参是整个程序里最需要经验的部分。我建议第一次跑程序时不要凭感觉乱试而是从传感器数据手册出发推导初值然后再做微调。R矩阵表示GPS观测噪声的协方差可以从GPS接收机的标称定位精度直接给。消费级GPS水平定位精度大概在2.5米到5米之间那么R矩阵的对角元素可以设成(5)^225Z方向因为GPS高度精度差通常设成(10)^2100。如果用的是RTK级别的GPS厘米级精度R可以设到(0.1)^20.01。Q矩阵表示IMU过程噪声理论上要用Allan方差分析得到角度随机游走和速度随机游走参数但对初学者来说一个更简单可行的方式是从一个初值开始观察滤波结果的轨迹平滑度和误差大小再逐步调整。我的经验是如果融合轨迹抖动很厉害、高频噪声很明显说明Q相对于R太小滤波器过于相信IMU、被IMU噪声带偏了试着把Q调大如果轨迹过于平滑、GPS的修正总是跟不上转弯处存在明显滞后说明Q相对于R太大滤波器过分忽略GPS试着把Q调小。这个大小失衡的判断标准比对着公式调参数直观得多。4.3 异常GPS跳变怎么处理GPS在真实环境里不一定总是干净的。高架桥下可能瞬间跳到马路对面隧道出口可能一下子跳到几十米外。如果不做任何处理这些异常观测会被卡尔曼滤波直接吞进去结果是融合轨迹里突然出现一个尖锐的脉冲要好长一段时间才能被后续观测拉回来严重时会直接导致滤波器发散。我在程序里加了一个简单的异常值剔除逻辑计算新息的马氏距离即y * inv(S) * y如果这个值大于某个阈值比如卡方分布的0.99分位数就认为这次GPS观测是异常值跳过本次更新。阈值取9.21比较好这是自由度3的卡方分布在0.99分位数下的值。这个逻辑代码量很小但对实际路测数据的融合效果提升非常明显强烈建议保留。mahal_dist y * inv(S) * y; if mahal_dist 9.21 % 判定为异常观测跳过更新 x_upd x_pred; P_upd P_pred; else % 正常更新 x_upd x_pred K * y; P_upd (eye(15) - K * H) * P_pred; end5. 把开源程序接进你自己的数据需要改哪几处程序自带的示例数据跑起来之后很多人想问这个程序能不能直接用我自己的数据答案是可以但有两个地方需要根据你自己的数据格式做适配。5.1 数据格式整理与参数适配第一步是改laod_data.m里的读取逻辑。现在的实现是按固定的列顺序读取文本文件IMU数据一般是时间戳 加速度计三轴 陀螺仪三轴GPS数据一般是时间戳 经度 纬度 高度。你自己的数据列顺序可能不一样单位也不一样所以第一步就是把读取后的数据统一成程序内部约定的单位并在主脚本里确认开头的配置参数。第二步是确认IMU的输出频率和GPS的输出频率在main.m里把dt_imu和dt_gps设成实际值。这两个频率参数直接影响滤波器的离散化步长和观测时机判断设错了轨迹的形状一定会出问题。我自己用过一个标称100Hz但实际抖动在95到105Hz之间的IMU简单固定步长处理最后轨迹有明显的时间漂移后来改了按实际时间戳差分解算问题就消失了。如果你手里的传感器时间戳比较准建议用实际时间戳做差分不要写死固定步长。第三步是根据IMU的噪声水平调整初始P矩阵和Q矩阵。初始P矩阵描述的是对初始状态的不确定度第一帧时状态不确定度很大一般把位置、速度、姿态对应的P对角线设成一个较大值把零偏对应的P设成中等值。程序里给的默认参数普遍偏高如果你的传感器比较差可以统一乘以一个比例因子观察滤波收敛速度再做微调。5.2 没有真值的时候怎么判断融合效果开源数据集通常带真值轨迹可以直接算RMSE但自己采集数据时往往没有真值怎么判断融合效果好不好我的经验是看三个特征。第一融合轨迹的平滑性GPS原始轨迹通常有高频抖动融合后的轨迹应该明显更平滑但不能平滑成经过大幅低通滤波的样子要能保留转弯、变速等真实动态信息。第二速度估计的合理性把融合输出的速度画出来看有没有出现剧烈的正负跳变。舒适的车辆行驶加速度一般不超过3m/s^2如果你发现速度剧烈震荡大概率是Q和R的比例不对或者IMU数据里有异常毛刺。第三闭合回路的终点误差如果真的做了一个闭合路线起点和终点应该重合终点和起点之间的误差基本可以代表整条轨迹的累积精度。我自己后来还习惯做一个实验把GPS数据倒数第二段故意挖掉十几秒看融合轨迹在这段GPS缺失的情况下还能不能维持正确的道路形状。如果IMU零偏估计准确、卡尔曼参数合理这段推算轨迹通常能保持几十秒的可用精度如果这段轨迹直接飞掉说明IMU零偏估计还没收敛或者Q矩阵偏大。这个实验做一次你对这套代码的理解会比看十遍公式都深。程序目前其实还可以往两个方向扩展一个是把GPS速度观测也加进滤波器这对低速、急刹车场景的融合精度提升很明显另一个是用误差状态卡尔曼滤波ESKF重写核心逻辑把姿态误差参数化在大初始误差角下比现在这种直接线性化四元数的做法稳定得多。我已经在分支上验证过ESKF版本的可行性后面整理好代码会再更新一版。本文还有配套的精品资源点击获取