★ FEATURED ARTICLE 简介《GNSS 与惯性及多传感器组合导航系统原理第二版》配套光盘中的MATLAB程序围绕全球导航卫星系统、惯性导航及多传感器融合展开适合导航工程专业学生、组合导航算法研究人员和嵌入式开发者对照教材进行仿真验证。压缩包大小约13.56MB现有1865人浏览学习。程序代码覆盖GNSS信号捕获、跟踪与解码涉及PRN码生成、载波相位同步和多普勒频移计算惯性导航部分提供加速度计与陀螺仪的数据建模、位置速度姿态积分及卡尔曼滤波实现。多传感器融合模块则演示通过扩展卡尔曼滤波EKF将GNSS、INS、磁力计和高度计数据统一处理同时包含城市峡谷、卫星遮挡等典型场景仿真以及误差源量化分析与滤波器参数调优示例。读者运行这些MATLAB程序可将抽象公式转化为可视化结果深入理解组合导航系统的设计原理、误差特性和算法调试方法为后续工程开发或论文研究提供直接参考。1. 组合导航仿真从哪里开始先把这套盘程序跑通做 GNSS 惯性导航的同学多半经历过这个画面自己写的 15 维卡尔曼滤波调了三天协方差矩阵跑完一看轨迹已经飘到隔壁城市去了而《GNSS 与惯性及多传感器组合导航系统原理第二版》随书整理出来的这套光盘资源半小时就能把松组合链路跑通。它是原书团队写的 MATLAB 仿真程序集从捷联惯导解算、GNSS 定位解算到松组合、紧组合滤波都配好了可执行脚本和仿真数据。适合两类人一类是拿这本书当教材、想把公式变成能跑代码的学生另一类是做实机组合导航的工程师先在仿真环境里把链路验证通再接真实传感器。如果你想完整复现这本书的仿真环境这份配套光盘内容值得先收下来跑一遍它能替你省掉最痛苦的调协方差环节让你把精力放在理解状态方程和量测方程上。2. 资源结构与技术选型这套 MATLAB 程序的工程地图组合导航程序最怕什么最怕一拿到手就点运行报错后在一个不认识的工程里瞎找文件。这套光盘是典型的教科书配套风格按仿真数据、核心函数、主脚本、文档四层分没有引入大型工程框架也没有图形界面代码之间的依赖全靠文件名和注释约束。所以第一次打开工程的人第一件事应该是先看 README.txt 和目录结构而不是急着点运行。2.1 技术选型为什么清一色 MATLAB做组合导航仿真MATLAB 几乎是事实标准原因不难理解卡尔曼滤波本质就是矩阵运算教材里写的是状态方程和量测方程用 MATLAB 的矩阵语法平移过去几乎不用改结构另外画轨迹对比图、画误差曲线几条 plot 语句就能出图。这套光盘里全部是 .m 脚本和 .mat 数据文件常见做法是不依赖特定工具箱基础 MATLAB 就能跑也没有 Simulink 模型。这意味着你在个人电脑上装一个相对较新的版本就可以不需要申请额外授权对学生很友好。它的代价也很直观没有单元测试函数与数据之间靠文件名约定一旦有人把某个函数改名调用链就断。所以拿到资源后我会先把整个目录过一遍确认哪些是数据、哪些是主脚本、哪些是辅助函数避免后面跑的时候缺这个缺那个。2.2 目录结构先定位数据、主脚本和核心函数典型的随书光盘会把代码按下面的方式摆放不同印次略有出入但大致是这种分层GNSS_INS_2nd/ ├── README.txt ├── data/ │ ├── imu_data.mat # 三轴陀螺、三轴加速度的仿真测量 │ ├── gnss_data.mat # GNSS 位置速度解算结果 │ └── ref_traj.mat # 参考轨迹真值 ├── src/ │ ├── initial_alignment.m # 初始对准 │ ├── ins_mechanization.m # 捷联惯导机械编排 │ ├── kf_error_state.m # 15 维误差状态卡尔曼滤波 │ ├── loose_coupling.m # 松组合量测模型 │ ├── tight_coupling.m # 紧组合量测模型 │ └── plot_results.m # 绘图 ├── main_loose_coupling.m # 松组合主脚本 ├── main_tight_coupling.m # 紧组合主脚本 └── doc/ └── programming_notes.pdf读目录时有几个信号值得注意。data 里单独放了参考轨迹说明作者提供了真值你可以做误差评估而不是只看轨迹好不好看。src 里把 ins_mechanization 和 kf_error_state 分开说明惯导递推和误差估计是两条独立流程这也是组合导航工程最常见的代码组织方式。主脚本只有两个分别对应松组合和紧组合意味着改动量测模型时主体递推代码可以完全不动。2.3 数据约定坐标系、单位、时间同步这一小节是整个资源里最该先读的部分。组合导航跑飞的原因里坐标系和单位的坑占了一大半。常见做法是这套数据统一用 NED 导航坐标系北东地和前右下载体坐标系姿态角用横滚、俯仰、航向的欧拉角定义。动手前先把这三个约定抄在纸上数据变量含义单位说明imu_acc三轴加速度测量m/s²含零偏与随机噪声imu_gyro三轴角速度测量rad/s含零偏与随机噪声gnss_posGNSS 经纬高deg, deg, m纬度在前经度在后gnss_velGNSS 速度 NEDm/s北、东、地三分量ref_traj参考轨迹经纬高与姿态deg, deg, m, deg用于误差评估拿到数据后第一步不是跑主脚本而是先读数据、检查采样周期和时间戳确认 IMU 和 GNSS 的采样频率关系。一般 IMU 频率是 100 HzGNSS 是 1 Hz两个信号更新率差两个数量级量测更新的时机必须卡准否则后面卡尔曼滤波一定会乱。% 读取配套数据先确认时间轴对不对 imu_data load(data/imu_data.mat); gnss_data load(data/gnss_data.mat); dt_imu median(diff(imu_data.t)); % 稳健估计 IMU 采样周期 dt_gnss median(diff(gnss_data.t)); % 稳健估计 GNSS 采样周期 fprintf(IMU dt %.4f s, GNSS dt %.2f s\n, dt_imu, dt_gnss); assert(abs(dt_imu - 0.01) 1e-4, IMU 时间轴有异常间隔);这段代码用 median 而不是 mean 来估计采样周期是为了避免个别野值时间戳把平均值带偏。assert 的作用是提前发现时间轴断裂如果 IMU 间隔不是 10 ms说明数据文件在导出或压缩过程中出了问题后面的时间同步就没意义了。我一般会在跑任何主脚本前先把这一段单独执行一遍能排除掉很多玄学故障。3. 核心模块拆解从惯导递推到卡尔曼滤波的完整链路这套程序的骨架是一条清晰的链路先用参考轨迹生成带噪声的 IMU 测量再在每个采样周期里做机械编排递推然后按 GNSS 更新频率触发卡尔曼滤波的量测更新。看懂这条链路比看懂任何单个函数都重要因为你改任何一处都得知道它在这条链路里的位置。3.1 轨迹发生器与 IMU 数据模拟仿真数据的来路要验证组合导航算法先要有一条干净的参考轨迹反推出 IMU 应该测到的比力和角速度再叠加零偏和随机游走噪声。这就是 data 文件夹里仿真数据的来路。常见做法是差分近似加噪声代码逻辑大致如下function [acc_meas, gyro_meas] imu_sim_from_traj(ref, dt) % 由参考轨迹反推理想 IMU 测量 % ref: pos(经纬高), vel(NED), att(欧拉角) n size(ref.pos, 1); gyro_meas zeros(n, 3); acc_meas zeros(n, 3); for k 2:n d_att ref.att(k, :) - ref.att(k-1, :); gyro_meas(k, :) d_att / dt; % 角速度差分近似 f_n (ref.vel(k, :) - ref.vel(k-1, :)) / dt; % 导航系加速度 f_n(3) f_n(3) 9.8; % 比力 加速度 - 重力 acc_meas(k, :) f_n * rotz(ref.att(k,3)); % 转到载体坐标系 end endrotz 是绕 z 轴旋转指定角度的 3x3 旋转矩阵程序里要么自带实现要么在机械编排里已经有一个方向余弦矩阵可以直接借用。这段代码的细节不用细抠理解思路即可参考轨迹提供真值IMU 数据是加了噪声的测量滤波的作用就是从带噪声的测量里把真值估计出来。这也解释了为什么调参时先看 Q、R 的量级它们和仿真时叠加的噪声强度是匹配的。有一点要注意如果轨迹里有大幅转弯差分求角速度前要先对航向角做 unwrap 处理否则角速度会跳出 180 度变成一个大尖峰。仿真数据一般不踩这个坑接真实数据时就很容易翻车。3.2 捷联惯导机械编排姿态、速度、位置的三级更新机械编排是每个 IMU 采样周期内都要执行的核心循环姿态更新在其中最关键function q attitude_update(q, gyro, dt) % 等效旋转矢量 四元数乘法完成姿态递推 omega norm(gyro); % 角速度幅值rad/s if omega 1e-10 axis gyro / omega; % 旋转轴载体坐标系下 half_theta omega * dt / 2; % 半旋转角 q_delta [cos(half_theta); axis*sin(half_theta)]; % 姿态增量四元数 else q_delta [1; 0; 0; 0]; % 角速度为 0姿态不变 end q q_mul(q, q_delta); % 四元数右乘载体系增量 end这里是组合导航代码里最容易写反的一处。角速度是陀螺测到的载体系相对惯性系的旋转增量四元数要右乘写成左乘姿态就会朝错误方向积分反映到轨迹上就是持续转弯。q_mul 是程序自带的四元数乘法等价于 Hamilton 积不想依赖工具箱的话手写也就三行。如果将来换用输出增量角度的光纤陀螺直接把 gyro*dt 替换成 delta_theta 即可函数骨架不用动。速度与位置更新的典型写法是function [vel, pos] pos_vel_update(vel, pos, acc_ned, dt) vel vel acc_ned * dt; % 比力方程简化版 Rm 6378137; % 地球平均半径 lat pos(1) vel(1) * dt / Rm; % 纬度增量rad lon pos(2) vel(2) * dt / (Rm * cos(pos(1))); % 经度增量rad pos [lat; lon; pos(3) vel(3)*dt]; % 经、纬、高递推 end注意 lat 和 lon 在递推过程中其实都是弧度到最后才统一转成度。这里用平均地球半径做了简化教材正文里会引入椭球模型和两个曲率半径但代码里为了清晰常常用常量。实际改造时如果跑高精度场景要换成子午圈曲率半径和卯酉圈曲率半径这两组公式在书里都能找到。3.3 15 维误差状态卡尔曼滤波预测与更新的骨架组合导航的精髓在这里。状态向量是位置误差、速度误差、姿态误差、陀螺零偏、加计零偏一共 15 维。程序里矩阵规模不大直接在 .m 文件里写死状态分组维度物理含义δr3位置误差NED 系δv3速度误差NED 系φ3姿态误差角b_g3陀螺零偏b_a3加速度计零偏滤波预测与更新的代码几乎是教科书原样function [X, P] kf_predict(X, P, Phi, Q) X Phi * X; % 状态预测 P Phi * P * Phi Q; % 协方差预测 end function [X, P] kf_update(X, P, H, Z, R) S H * P * H R; % 新息协方差 K P * H / S; % 滤波增益用除法而非求逆 X X K * (Z - H * X); % 量测更新 P (eye(size(P)) - K * H) * P; % 协方差更新用 size 兼容扩展 end预测里 Phi 矩阵由惯性误差方程离散化得到Q 由陀螺和加计的噪声密度构建。更新里 Z 的量测残差是 GNSS 与惯导的位置速度差值。H 矩阵把误差状态映射到量测空间松组合时它只有位置和速度对应的行其余行是零。K P * H / S 这句在 MATLAB 里用右除而不是显式求逆数值上更稳。R 设太大会让增益趋近于零滤波完全依赖惯导递推时间一长漂移就回来R 设太小又会把 GNSS 噪声全部放进估计结果轨迹出现锯齿。3.4 松组合与紧组合量测设计的两种思路这套光盘给两个主脚本分别对应两组量测设计。松组合把 GNSS 解算出的位置速度当作量测简单直观适合先跑通再深入紧组合直接用伪距和伪距率原始观测需要在代码里对量测方程做雅可比线性化程序厚很多但可见星少于四颗时仍能维持估计。对比项松组合紧组合量测来源GNSS 输出的位置速度GNSS 伪距、伪距率H 矩阵构建固定选择行直观逐星计算几何方向矢量对可见星数量要求至少 4 颗才能解算1 颗以上可观测实现难度低高程序厚度一个量测函数一个含循环的测量模型函数建议先跑松组合确认机械编排和滤波骨架没问题再切紧组合。两套代码共用同一套机械编排和数据文件切换成本主要在量测模型文件里这也是这套资源设计得比较舒服的地方。4. 运行与验证半小时跑出第一张轨迹对比图跑这套程序不需要复杂的编译环境MATLAB 装好、路径设对就能出结果。下面按我从零开始跑通的步骤写照着做基本不会卡壳。4.1 环境准备与路径设置建议用 R2016 之后的版本早期版本的字符串处理和绘图函数与新版本有差异而新版本偶尔会废弃一些老式绘图接口遇到语法错误先查那一行是不是老式写法。设置工作目录时执行# 在 MATLAB 命令行里进入资源解压目录 cd /path/to/GNSS_INS_2nd addpath(genpath(pwd));addpath(genpath(pwd)) 会把 data、src 都加入搜索路径之后在任何目录都能调用 main 脚本和 src 里的函数。如果不想每次都设置可以在 MATLAB 的 startup.m 里加一行但这个资源本身不要求。这一步做完后可以用 which 命令验证某个函数是否被正确找到比如 which ins_mechanization返回路径正确就说明环境就绪。4.2 运行松组合主脚本松组合是理解整套程序的最佳入口直接执行% 松组合主脚本完成初始对准、机械编排、滤波与绘图 main_loose_coupling运行后命令窗口一般会打印对准完成、开始组合导航、GNSS 量测更新这几类过程信息。脚本结束后工作区会出现 pos_ins、pos_gnss、pos_est、att_est 等变量。如果报错说找不到函数第一反而不是改代码回到 addpath 那一步看看路径是否覆盖到 src 目录。如果报错发生在某个具体函数内部先把函数调用行前的断点打上一步步看输入矩阵的维度。4.3 结果验证轨迹贴合与误差曲线程序运行完第一张图通常是轨迹对比图。验证可以分两步先目测轨迹是否贴合再做定量误差评估figure; plot3(pos_gnss(:,2), pos_gnss(:,1), pos_gnss(:,3), LineWidth, 1); hold on; plot3(pos_est(:,2), pos_est(:,1), pos_est(:,3), LineWidth, 1); plot3(ref_traj.pos(:,2), ref_traj.pos(:,1), ref_traj.pos(:,3), k--); legend(GNSS 单独解算, 组合导航, 参考轨迹); xlabel(经度); ylabel(纬度); zlabel(高度);正常结果是 GNSS 点有一定噪声散布组合导航轨迹比 GNSS 平滑并且更贴近黑色虚线参考轨迹。如果组合导航轨迹离参考轨迹比 GNSS 还远先别急着调滤波参数回头查量测单位或坐标系这在后面避坑章节会专门讲。误差量级的常识判断在仿真数据下位置误差通常米级速度误差 0.1 m/s 级姿态误差角在角分级如果位置误差到百米级基本可以断定是单位问题不是调参问题。注意绘图时如果觉得轨迹太挤可以先用 axis equal 强制等比缩放避免高度轴被拉伸造成误判。4.4 常用参数改哪里跑通之后必然想改参数看效果。常见的改动点集中在下面几个位置参数所在位置作用调参经验Q 过程噪声kf_error_state.m 或主脚本描述惯性误差增长量级与陀螺、加计噪声密度对齐R 量测噪声loose_coupling.m描述 GNSS 观测噪声过小导致轨迹抖动过大导致漂移对准时长initial_alignment.m初始姿态精度静态段越长初始姿态越准IMU/GNSS 频率数据生成脚本采样率时间不同步会直接发散我会建议每次只改一个参数记录前后误差曲线而不是同时动 Q 和 R否则你根本分不清轨迹变化是谁引起的。第一次调参就用默认值跑通然后单独把 R 放大一个量级看轨迹如何变钝再单独把 Q 缩小一个量级看漂移如何回来这样能很快建立起对滤波行为的直觉。5. 避坑排查五个让新手下车的常见问题这一章是我自己跑这类程序时踩过的坑按现象、原因、解决的顺序记录遇到类似问题可以直接对号入座。5.1 轨迹持续绕圈漂移现象组合导航轨迹慢慢转圈逐渐远离参考轨迹而且半径越来越大。原因陀螺零偏没有被估计或者滤波状态里根本没设零偏这一项姿态误差随时间累积惯导递推的方向越来越偏。解决先确认对准阶段是否有足够长的静态段让水平姿态收敛再检查滤波状态向量里是否包含陀螺零偏以及 Q 矩阵对应零偏的位置有没有给非零过程噪声。如果零偏是常数没有过程噪声它也能被估计出来但如果零偏受温度缓慢漂移不给过程噪声就会跟不上。5.2 GNSS 更新瞬间轨迹跳变现象轨迹在每个 GNSS 更新点出现锯齿状跳动而且跳幅越来越大。原因R 矩阵对角线设置过小滤波过度信任 GNSS 量测每次更新都把观测噪声全部吸收进状态。解决先把 R 量测噪声扩大一个量级再看轨迹是否变平滑同时打印新息序列 inspect 一下如果新息均值明显大于 R 对角线的开方说明噪声模型低估了实际观测噪声。这个坑在仿真数据里尤其常见因为仿真噪声通常是白噪声R 稍微调大一点就能压住。5.3 纬度经度递推越来越离谱现象纯惯导模式下跑几分钟经纬度数值明显偏离而且偏得毫无规律。原因单位混用最常见的做法是把弧度当成度直接累加或者反过来把度当成弧度送进递推公式纬度的增量会被放大约 57 倍。解决在递推函数内部统一用弧度clamp 到合理范围内只在画图和输出时转成度。我一般会在递推函数里加一行断言检查单步经纬度变化量不超过一个阈值比如 1e-3 弧度级别超了就立刻停下来定位。5.4 默认初始航向不准导致整条轨迹旋转现象轨迹形状正确姿态角变化也对但整条轨迹绕起点旋转了一个固定角度。原因初始对准阶段只做了水平对准没有磁力计或外部航向参考航向初值误差大相当于整个导航系都偏了。解决在仿真环境里用轨迹发生器给出的真值航向作为初值或者把对准脚本的第一个采样点姿态强制设为参考轨迹的姿态。接入实机数据时再考虑磁力计辅助或 GNSS 测速推算航向但那是另一个层面的问题。5.5 换了新版 MATLAB 之后程序反而跑不动现象代码在别人老版本上能跑到了你的新版本直接提示语法错误或函数被移除。原因MATLAB 新版本会废弃一些旧绘图函数与字符处理接口教材配套代码如果年代较早很容易撞上这类不兼容。解决先看警告信息定位到具体函数把老式绘图接口替换成新版本接口核心数值计算函数几乎不受影响不要因为绘图函数报警就怀疑卡尔曼滤波写错了白白在数值代码里找半天。如果某个老函数确实找不到可以用 which 确认它是否已被移除再搜替代函数。当轨迹不对的时候我的固定排查顺序是先查坐标系与单位再查时间同步然后查初值对准最后才怀疑滤波发散。这个顺序能避免你在错误的环节上浪费两天。6. 二次开发方向把配套程序改成你自己的导航方案跑通只是开始真正有价值的是把它改成你自己的惯性导航方案。下面三个改造方向按投入产出比排序适合从这套资源出发做二次开发。6.1 把散在脚本里的参数收敛成配置结构体这套代码的参数散落在主脚本和各函数顶部改起来要全局搜索。我会先建一个 params.m% params.m把零偏、噪声、频率统一成结构体 p.imu_freq 100; p.gnss_freq 1; p.gyro_bias [0.01; -0.02; 0.005]; % rad/s p.acc_bias [0.01; 0.01; 0.02]; % m/s^2 p.R_pos 1.5^2 * eye(3); p.R_vel 0.05^2 * eye(3);改主脚本时只改 params.m做多组对比实验时不用每次搜索替换常量也方便把同一份参数传给画图脚本做误差评估。6.2 扩展状态加零速修正量测低成本惯性器件最常见的增强手段是 ZUPT利用静止时刻速度应为零的约束抑制漂移。状态向量从 15 维扩到 16 维时量测矩阵加一行% 状态排列位置 1:3速度 4:6姿态 7:9 H_z zeros(3, size(X,1)); H_z(:, 4:6) eye(3); % 速度误差对应的列 Z_z -vel_est; % 静止时速度估计应趋近零静止检测通常用陀螺角速率幅值判断低于阈值就进入 ZUPT 量测。这是行人导航和车辆低速导航常用的低成本增强方案配合这套程序里的误差状态骨架改动量不到二十行。6.3 换自己的传感器数据接入自采数据的关键是按 data 文件夹的字段组织好 imu_acc、imu_gyro、gnss_pos、gnss_vel。常见做法是写一个脚本把自家日志转成 .mat 字段对齐单位必须严格统一IMU 的加速度用 m/s²角速度用 rad/sGNSS 位置用度、速度用 m/s。采样频率可以不同只要在配置里同步改 dt 即可但时间基准必须同源否则组合结果迟早发散。从那以后我每次跑组合导航程序都强制先花十分钟检查坐标系、单位、时间基准这三个约定再打开主脚本。这套程序的数学骨架并不难难的是带着自己的数据还能一次跑通希望帮到你。本文还有配套的精品资源点击获取
阅读完成 · 觉得有帮助？
