简介这份资源面向嵌入式导航、自动驾驶感知与控制课程实践的学习者提供一套可直接运行的GNSS-INS组合导航MATLAB仿真方案重点解决惯性测量单元与卫星导航数据融合中姿态、速度和位置误差的动态校正问题。压缩包共3个文件以inscode工程配置、html说明页与gitignore为主整体约3KB属于轻量级代码工程结构便于快速导入与版本管理。资源围绕扩展卡尔曼滤波展开覆盖状态方程与观测方程构建、噪声协方差调参、融合结果可视化等关键环节并配有真实场景模拟数据与讲解视频帮助读者理解EKF建模思路与调参技巧。目前已有19人学习适合希望快速复现组合导航流程、开展二次开发或完成课程实验的读者参考。1. 组合导航仿真到底在解决什么问题单靠 GNSS 做定位在城市峡谷、树荫遮挡、隧道出入口这些场景里卫星信号一断位置输出就开始飘甚至直接跳变几十米。单靠 IMU 做航位推算短时间精度不错但零偏和噪声会随时间积分累积几十秒后误差就能大到没法看。GNSS 与 IMU 融合导航仿真包要解决的就是把这个「一个长期稳但短期跳、一个短期稳但长期漂」的矛盾用滤波器在仿真环境里先跑通、调好参数再上实测。这套东西适合两类人一类是做无人车、无人机、农机导航的算法工程师需要一套能反复复现、参数可调的仿真验证环境另一类是想入门组合导航的学生或转行者直接啃真实数据集成本太高先用仿真把 EKF 的预测、更新、误差状态推导跑一遍理解会扎实很多。标题里提到的 EKF 滤波实现、MATLAB 源码、实操教程本质上是把「理论公式 → 可运行代码 → 参数怎么调」这条链路补齐。仿真包的价值不在于精度多高而在于你能把每个环节拆开看知道哪一步在起作用、哪一步在引入误差。2. 仿真链路怎么搭从轨迹生成到传感器建模2.1 先定坐标系和状态量别急着写滤波组合导航翻车最多的地方不是滤波公式而是坐标系没统一。常见做法是导航系用东北天ENU机体系用右前上RFUIMU 输出的是机体系下的比力和角速度。状态量一般取 15 维误差状态位置误差3、速度误差3、姿态误差3、加速度计零偏3、陀螺零偏3。用误差状态而不是全状态是因为误差量是小量线性化更准EKF 的雅可比也好推。仿真第一步是生成一条真值轨迹。我一般用分段解析的方式先给一段匀加速直线再给一段匀速转弯最后加一段爬升。这样每段的真值位置、速度、姿态都有闭式解方便后面算误差。轨迹生成的核心是保证姿态、速度、位置三者自洽不能各生成各的。% 生成真值轨迹匀加速 转弯 爬升dt 为采样间隔 dt 0.01; T 60; t 0:dt:T; N length(t); pos zeros(3,N); vel zeros(3,N); att zeros(3,N); for k 2:N if t(k) 20 acc [0.5; 0; 0]; % 东向匀加速 omega [0; 0; 0]; elseif t(k) 40 acc [0; 0; 0]; % 匀速转弯 omega [0; 0; 5*pi/180]; % 偏航角速度 5deg/s else acc [0; 0; 0.3]; % 天向爬升 omega [0; 0; 0]; end % 机体系比力 导航系加速度投影到机体系简化忽略重力补偿细节 Cbn eul2rotm(att(:,k-1), ZYX); fb Cbn * acc; vel(:,k) vel(:,k-1) acc*dt; pos(:,k) pos(:,k-1) vel(:,k-1)*dt 0.5*acc*dt^2; att(:,k) att(:,k-1) omega*dt; end这段代码里eul2rotm的旋转顺序要和后面滤波里用的姿态定义一致ZYX 对应偏航-俯仰-横滚。acc是导航系下的加速度转到机体系得到比力fb这是 IMU 加速度计真正测量的量。参数上dt取 0.01 对应 100Hz实际 IMU 常见 100~200HzGNSS 常见 1~10Hz仿真里要按这个比例做降采样。2.2 IMU 和 GNSS 的误差模型怎么加才像真的真值轨迹有了接下来给 IMU 加零偏和噪声给 GNSS 加位置噪声和低频漂移。IMU 误差模型一般用一阶马尔可夫加白噪声零偏用随机游走噪声用高斯白噪声。GNSS 位置噪声不能只加白噪声否则滤波太容易收敛体现不出融合价值。常见做法是加一段相关时间几十秒的马尔可夫过程模拟多路径和电离层慢变。% IMU 误差零偏随机游走 白噪声 bias_a zeros(3,N); bias_g zeros(3,N); sigma_ba 0.001; sigma_bg 0.0005; % 零偏游走系数 sigma_a 0.02; sigma_g 0.002; % 白噪声标准差 for k 2:N bias_a(:,k) bias_a(:,k-1) sigma_ba*randn(3,1)*sqrt(dt); bias_g(:,k) bias_g(:,k-1) sigma_bg*randn(3,1)*sqrt(dt); end meas_a fb bias_a sigma_a*randn(3,N)/sqrt(dt); meas_g omega bias_g sigma_g*randn(3,N)/sqrt(dt); % GNSS 位置白噪声 马尔可夫慢变 sigma_gnss 2.0; tau 30; gnss_bias zeros(3,N); for k 2:N gnss_bias(:,k) exp(-dt/tau)*gnss_bias(:,k-1) 0.5*randn(3,1)*sqrt(dt); end gnss_pos pos gnss_bias sigma_gnss*randn(3,N);sigma_ba、sigma_bg决定零偏漂移快慢实际 MEMS IMU 的陀螺零偏稳定性大概在 1~10 deg/h 量级换算成 rad/s 后量级很小仿真里可以适当放大让滤波效果更明显。tau是 GNSS 误差的相关时间取 30 秒意味着误差在 30 秒内缓慢变化这样 EKF 才有机会用 IMU 的短期精度去平滑它。注意 GNSS 只给位置不给速度速度靠 IMU 积分和滤波估计这是最常见的配置。2.3 时间同步和降采样仿真里最容易忽略的一步真实系统里 IMU 和 GNSS 时间戳不可能完全对齐仿真里如果直接按同一时间轴处理滤波会「作弊」。正确做法是IMU 按 100Hz 跑预测GNSS 按 10Hz 做更新更新时刻取最近的 IMU 时刻并记录时间差。如果 GNSS 有延迟还要在滤波里做延迟补偿否则姿态会振荡。% 时间对齐GNSS 每 10 个 IMU 周期更新一次 gnss_rate 10; step round(1/(gnss_rate*dt)); for k 2:N % IMU 预测 [pos_est, vel_est, att_est, P] ekf_predict(... pos_est, vel_est, att_est, P, meas_a(:,k), meas_g(:,k), dt); % GNSS 更新 if mod(k, step) 0 [pos_est, vel_est, att_est, P] ekf_update(... pos_est, vel_est, att_est, P, gnss_pos(:,k)); end endstep控制更新频率取 10 对应 10Hz GNSS。如果实际 GNSS 是 1Hzstep取 100预测步数变多零偏估计会变慢这时候要适当调大过程噪声Q否则滤波器会过度自信。这一步是很多仿真包没讲清楚的地方也是实测和仿真差距的主要来源之一。3. EKF 预测与更新公式怎么落到 MATLAB 代码3.1 状态转移和雅可比别用数值差分糊弄EKF 的核心是状态转移矩阵 F 和观测矩阵 H。误差状态 15 维F 可以按位置、速度、姿态、零偏分块写。姿态误差的传播和比力有关速度误差的传播和姿态有关这两块是耦合的不能拆开单独积分。我一般直接写解析雅可比虽然推导麻烦但跑起来稳定数值差分在小步长下容易受浮点误差影响。function [pos, vel, att, P] ekf_predict(pos, vel, att, P, fb, omega, dt) % 姿态更新用罗德里格斯公式 theta norm(omega*dt); if theta 1e-8 axis omega*dt/theta; R rodrigues(axis, theta); else R eye(3); end Cbn eul2rotm(att, ZYX); Cbn Cbn * R; att rotm2eul(Cbn, ZYX); % 速度更新导航系加速度 姿态投影比力 重力 g [0; 0; -9.81]; acc_n Cbn * fb g; vel vel acc_n*dt; pos pos vel*dt; % 雅可比 F15x15这里只列关键块 F eye(15); F(1:3,4:6) eye(3)*dt; F(4:6,7:9) -skew(Cbn*fb)*dt; F(4:6,10:12) Cbn*dt; F(7:9,13:15) -Cbn*dt; % 过程噪声 Q Q diag([0.01*ones(1,3), 0.01*ones(1,3), ... 0.001*ones(1,3), 0.0001*ones(1,3), 0.0001*ones(1,3)])*dt; P F*P*F Q; endskew是反对称矩阵Cbn*fb是比力在导航系下的投影。F(4:6,7:9)这一块体现姿态误差对速度误差的影响符号容易写反写完最好用一段已知轨迹验证如果姿态误差为正速度误差应该往哪个方向偏。Q的对角线量级要按传感器实际噪声填填大了滤波发散填小了收敛慢。3.2 观测更新GNSS 位置怎么进滤波器GNSS 只给位置观测矩阵 H 就是 3x15前 3 列是单位阵其余为零。更新时先算新息再算增益最后修正状态和协方差。新息要卡阈值防止 GNSS 跳点把滤波器带偏。function [pos, vel, att, P] ekf_update(pos, vel, att, P, gnss_pos) H zeros(3,15); H(1:3,1:3) eye(3); R diag([4, 4, 9]); % GNSS 位置观测噪声 y gnss_pos - pos; % 新息 if norm(y) 10 % 卡阈值超过 10m 视为异常 return; end S H*P*H R; K P*H/S; dx K*y; pos pos dx(1:3); vel vel dx(4:6); att att dx(7:9); P (eye(15) - K*H)*P; endR的对角线按 GNSS 实际精度填水平 2m、垂直 3m 对应方差 4 和 9。norm(y) 10是硬阈值实际工程里更常用卡方检验但仿真阶段硬阈值够用。P更新用标准形式数值稳定性要求高的话可以换成 Joseph 形式。3.3 参数怎么调Q 和 R 的配比决定滤波性格Q 和 R 的比值决定滤波器更信 IMU 还是更信 GNSS。Q 大 R 小滤波器跟 GNSS 紧短期跳变大Q 小 R 大滤波器更平滑但 GNSS 真出问题时反应慢。我一般先按传感器手册填初值再跑一段有 GNSS 中断的轨迹看中断期间位置漂移速度如果漂太快就调小 Q 里的零偏项如果恢复太慢就调大。参数含义典型初值调整方向sigma_a加速度计白噪声0.02 m/s²越大越信 GNSSsigma_g陀螺白噪声0.002 rad/s越大姿态越跟 GNSSsigma_ba加速度零偏游走0.001越大零偏估计越快sigma_bg陀螺零偏游走0.0005同上R_posGNSS 位置方差4 / 9越大越信 IMU调参没有万能值同一套参数换条轨迹可能就不好使。建议固定一条包含直线、转弯、遮挡的轨迹每次只调一个参数记录位置 RMSE 和姿态 RMSE看趋势。4. 避坑与排查仿真跑通了但结果不对怎么办4.1 位置曲线整体偏移但形状对现象估计轨迹和真值形状一致但整体平移了几米。原因通常是 GNSS 马尔可夫偏置没被估计或者初始位置没对齐。解决检查初始状态pos是否等于真值起点检查 GNSS 偏置的tau是否过大导致滤波器把它当真实位置。把tau调到 10~30 秒并在前几秒让滤波器先收敛再统计误差。4.2 姿态角在转弯时剧烈振荡现象直线段姿态平稳一转弯横滚和俯仰就抖。原因多半是陀螺零偏没估计出来或者姿态误差的雅可比符号写反。解决先关掉 GNSS 更新纯 IMU 积分看姿态漂移是否合理再打开更新检查F(7:9,13:15)的符号。如果符号反了零偏估计会往错误方向走姿态越修越偏。4.3 滤波发散协方差 P 出现负特征值现象跑一段时间后位置误差突然变大P不再正定。原因通常是Q太小导致协方差过度收缩或者P更新用了简化形式在强非线性下失稳。解决把P更新换成 Joseph 形式适当调大Q并在每次更新后做对称化P (PP)/2。4.4 GNSS 中断后恢复慢位置要几十秒才拉回来现象模拟 10 秒 GNSS 丢失恢复后估计位置慢慢才跟上。原因R设得太大滤波器对 GNSS 信任不足或者Q里的零偏项太小零偏估计没跟上。解决把R调小 20%~30%同时把sigma_ba、sigma_bg调大让滤波器在中断期间更依赖零偏估计。4.5 仿真精度很高一上实测就崩现象仿真 RMSE 只有 0.1m实测差好几米。原因仿真里 IMU 噪声模型太理想没有考虑尺度因子、非正交误差、温度漂移GNSS 也没有考虑多路径的突发性。解决在仿真里加尺度因子误差和轴间非正交GNSS 加随机跳点让仿真环境更接近真实。仿真包的意义是验证算法框架不是替代标定。5. 进阶技巧用一致性检验判断滤波器是否可信跑通仿真只是第一步真正判断滤波器好不好不能只看 RMSE还要看新息是否白噪声、协方差是否一致。新息序列如果还有相关性说明模型没建对如果实际误差远大于协方差开方说明滤波器过度自信。我一般会算归一化新息平方NIS和归一化估计误差平方NEES前者检验观测后者检验状态。% 收集新息和协方差算 NIS 和 NEES nis zeros(1, M); nees zeros(1, M); for i 1:M y gnss_pos(:,i) - pos_est(:,i); S H*P_store(:,:,i)*H R; nis(i) y / S * y; e x_true(:,i) - x_est(:,i); nees(i) e / P_store(:,:,i) * e; end % 卡方检验NIS 均值应接近观测维度 3NEES 均值应接近状态维度 15NIS的均值如果远大于 3说明R设小了或者观测有异常NEES均值远大于 15说明Q设小了或者模型有漏项。这两个指标比 RMSE 更能暴露问题因为 RMSE 只告诉你「差多少」不告诉你「为什么差」。我自己的习惯是每次改完参数先看 NIS 和 NEES 曲线有没有明显趋势再看 RMSE。如果 NIS 一直往上飘说明滤波器在逐渐不信任观测多半是零偏估计发散的前兆。还有一个实用技巧是分段统计把轨迹按直线、转弯、遮挡分成几段分别算 RMSE。直线段主要看零偏估计转弯段主要看姿态耦合遮挡段主要看纯惯导漂移。这样调参时能快速定位是哪一块出了问题不用对着整条曲线猜。仿真包里的实操教程如果只给一条总 RMSE参考价值有限建议自己补上分段统计。最后说个血泪经验仿真里跑得再好也别直接拿参数上实测。IMU 的零偏不是常数GNSS 的噪声也不是高斯仿真包最大的价值是让你把算法逻辑和坐标系理清楚真上实测前标定和温度补偿一步都不能省。希望帮到你。本文还有配套的精品资源点击获取