简介严恭敏老师《捷联惯导算法与组合导航原理》配套MATLAB仿真程序面向需要完成惯性导航相关课程设计、毕业设计的学生与导航算法入门者。压缩包共65个文件以61个.m仿真脚本为主体涵盖捷联惯导解算、惯性器件误差建模、卡尔曼滤波、初始对准及惯导/GPS组合导航等典型模块另有说明文档、许可文件与Git配置文件便于阅读和二次开发整包约66KB轻量易用。资源已有693人学习下载。代码经过验证可直接运行既能帮助理解SINS的欧拉运动方程、陀螺仪与加速度计的漂移噪声特性也可用于验证重力/地球自转校正和粗对准算法。借助这些源码学习者能快速搭建仿真环境分析不同参数对导航性能的影响或在此基础上优化滤波策略是理论联系实际的实用资料。1. 你手上的这份MATLAB程序包严恭敏教材最值得跑通的一版仿真代码在惯性导航这个方向论文里能讲清楚的东西只是冰山一角真正让人头疼的是姿态更新、速度更新和组合导航滤波之间的耦合关系。严恭敏老师《捷联惯导算法与组合导航原理》配套的MATLAB仿真程序就是把书里那些公式变成能跑的代码。这个压缩包不是给你上课交差用的它是一个能跑出轨迹、能出姿态误差曲线的完整算法框架从纯惯导解算到GPS组合导航都有落地实现。很多人把它当成论文复现的起点也有人用它给自研代码做基准对照。这份程序适合两类人一是刚接触捷联惯导、想把四元数更新和比力方程落地的研究生二是做组合导航但不想从零造轮子的工程师用这套程序做基线再往里加自己的误差模型或滤波方案。需要说明的是我手上没有这份压缩包的原始目录但按这个领域最常见的配套结构来拆解它一般包含轨迹仿真、惯导解算、初始对准和组合导航滤波几个模块。下面我按“先跑通、再看懂、再改参数、再避坑”的顺序来讲。2. 先把环境理清再动手MATLAB版本选择与解压后的目录结构调整2.1 MATLAB版本兼容性为什么说2018b到2023b之间最稳妥这套仿真程序是教材配套代码编写年代基本锁定在R2014到R2018之间。你用太新的MATLAB版本跑老代码最常见的翻车点不是语法错误而是函数行为变更。比如zeros、eye这些基础函数不会出问题但涉及randn的随机数生成器默认算法改变、plot的默认颜色顺序变化、以及strsplit等字符串函数的参数规则调整都会让老代码报出莫名其妙的错误。我一般建议直接用本地安装的MATLAB 2018b到2023b版本不要用在线网页版跑。原因是这套程序涉及大量脚本文件和数据文件的路径依赖网页版的工作目录管理方式会让addpath和save/load的行为变得不可预期。如果你手上只有2024a之后的版本也不是不能跑但需要在运行前手动关闭某些新版本的警告提示并且在后续排查报错时先怀疑版本兼容性别一上来就认为是算法写错了。注意如果你用的是2023b及更新版本首次运行前先执行warning(off,all)或者只在当前脚本顶部关闭指定警告ID避免新版MATLAB对datetime、height等变量名与内置函数冲突的警告刷屏。2.2 解压后的标准目录结构哪些子文件夹必须保留、哪些可以动拿到压缩包解压后第一件事不是双击运行某个.m文件而是先把目录结构理清楚。这个程序包通常在根目录下有若干脚本文件和子文件夹子文件夹一般按功能划分轨迹生成、捷联解算、对准算法、组合滤波、数据文件等。很多新手一上来就在MATLAB里双击main.m运行结果报错找不到函数原因就是没有把子文件夹加入搜索路径。我建议解压后先做一次“瘦身”把书上的说明文档和PDF单独放一个文件夹把.m代码文件和.mat数据文件分开。然后在MATLAB里把根目录和所有一级子文件夹用addpath(genpath(你的路径))一次性加入路径。注意genpath会把子文件夹下所有层级都加进来如果你的磁盘上有其他无关项目混在同一个父目录里会被一并加入造成同名函数冲突。这个坑我踩过后面专门说。2.3 第一个必须跑通的脚本轨迹仿真程序与惯性器件数据生成这套程序里最基础、也是最应该先跑通的是轨迹仿真模块。它先由你给定一条运动轨迹通常包含直线加速、匀速、转弯、减速等多个阶段然后反向生成陀螺仪和加速度计的理想测量值。这个模块的价值在于你有了理想测量值才知道后面惯导解算的误差应该为零才能验证解算算法本身的正确性。运行这个模块的标准动作是先找到轨迹生成脚本设置好初始纬度和经度以及轨迹各阶段的时长和机动幅度然后运行检查输出是否生成了.mat文件。这个过程中你大概率会遇到一个问题脚本里写死了输出路径而你的工作目录跟作者的目录不一致。解决方法是打开脚本查找所有save语句把路径改成你本地实际路径。% 轨迹仿真主入口示意 % 设定初始参数 init_lat 30.5 * pi / 180; % 初始纬度单位弧度 init_lon 104.0 * pi / 180; % 初始经度单位弧度 init_alt 500; % 初始高度单位米 % 轨迹阶段参数每段时长与运动方式 seg_duration [10, 20, 15, 10]; % 单位秒 seg_type [1, 2, 3, 2]; % 1-匀加速 2-匀速 3-转弯 % 调用轨迹生成函数具体函数名以你解压后的文件为准 % traj generate_trajectory(init_lat, init_lon, init_alt, ... % seg_duration, seg_type); % 保存为后续解算可用的数据文件 % save(traj_data.mat, traj);这段代码的逻辑很直接先定义初始位置和轨迹各阶段参数再调用轨迹生成函数。关键是seg_type的编码方式不同版本的教材代码定义不同有的用1表示匀速、2表示转弯有的相反。你要先看脚本里的注释或函数定义确认编码含义后再改参数。这里最需要注意的是单位所有角度都用弧度如果你用度数直接传进去后面解算出来的位置会直接偏离几个经度。3. 捷联惯导解算程序拆解姿态、速度、位置三个更新环节3.1 姿态更新子程序四元数归一化与圆锥误差补偿跑通轨迹生成后下一步就是理解本体解算程序。捷联惯导解算的核心是三个更新姿态更新、速度更新、位置更新。姿态更新是第一环也是最容易写错的一环。这套程序里姿态更新用的是四元数法原理是陀螺输出的角增量或角速度与上一时刻的姿态四元数做卷积。四元数更新后用范数归一化否则长时间递推会出现姿态漂移。这里有一个算法层面的关键点角增量输入时要做圆锥误差补偿角速度输入时则不需要因为角速度模式本身包含了更高阶的积分信息。教材代码通常在圆锥误差补偿上做了简化用二子样或三子样算法。你在跑通程序后可以做的第一个实验是把圆锥补偿项注释掉再对比姿态误差曲线你会看到在静态情况下差异不大但一旦有角振动或高机动误差会明显增长。% 四元数姿态更新核心示意 % q: 当前姿态四元数, w: 陀螺角速度(rad/s), dt: 更新周期 % 角增量计算 dtheta w * dt; % 圆锥误差补偿量(简化的单子样补偿) % 实际代码中常使用双子样/三子样形式 coning cross(dtheta(1:2), dtheta(3)); % 示意实际按子样数展开 % 四元数更新 norm_dtheta norm(dtheta); q_delta [cos(norm_dtheta/2); (dtheta(1)/norm_dtheta)*sin(norm_dtheta/2); (dtheta(2)/norm_dtheta)*sin(norm_dtheta/2); (dtheta(3)/norm_dtheta)*sin(norm_dtheta/2)]; % 更新并归一化 q_new quat_mult(q, q_delta); q_new q_new / norm(q_new);这里quat_mult如果是MATLAB自带函数在Aerospace Toolbox里但这套程序一般自带四元数乘法实现避免依赖工具箱。你运行前先用which quat_mult确认一下看它解析到的是工具箱函数还是程序包里的本地函数。如果解析到工具箱版本四元数的乘法顺序可能跟教材定义不同导致姿态结果整体反向这就属于“跑道跑通了但结果全错”的隐性坑。3.2 速度更新子程序比力方程与重力校正速度更新解决的是“在导航坐标系下的速度变化率是多少”的问题。基本原理是加速度计测到的比力扣除有害加速度包括哥里奥利加速度和重力加速度然后积分得到速度。这套程序里速度更新通常放在一个独立的子函数里输入是加速度计输出、当前姿态和位置输出是速度增量。容易出错的有两处一是重力模型。教材代码一般用简单的正常重力公式而不是完整EGM96模型。如果你的仿真轨迹高度变化很大或者纬度跨度很大简单重力公式带来的误差会在位置解算中累积。二是哥里奥利项的处理方式。有的代码把2*omega_ie_n×v这一项放在速度更新里有的放在位置更新前的整理阶段两种写法在数学上等价但如果在程序里实现时把交叉项符号搞错姿态不变的情况下速度就会持续发散。% 速度更新示意 % f_n: 导航系下比力, v_n: 导航系速度, g_n: 重力 % omega_en_n: 导航系相对地球系角速度 in 导航系 % omega_ie_n: 地球自转角速度 in 导航系 % 哥里奥利修正项 coriolis cross(2 * omega_ie_n omega_en_n, v_n); % 速度增量 dv (f_n - coriolis g_n) * dt; % 更新速度 v_new v_n dv;注意这里的cross维数导航系一般用北东地或东-北-天不同教材坐标系定义不同。严恭敏教材使用北东地坐标系所以g_n的方向是向下为负。如果你在程序里看到g_n是正值那大概率坐标定义是东北天。这个判断可以直接决定后续所有结果的正负号所以拿到代码第一件事是翻注释确认坐标系定义。3.3 位置更新与轨迹对比为什么位置会随时间漂移位置更新相对简单用速度和上一时刻位置做积分即可。但在整套解算中位置更新是误差累积的终点。姿态误差通过速度误差传导到位置误差所以如果你看到位置误差曲线随时间线性增长那说明姿态和速度环节大概率有偏差。跑通这套程序后一个非常有价值的验证动作是把程序解算出来的轨迹和轨迹仿真生成的参考轨迹做差画出经度误差、纬度误差和高度误差三条曲线。理想情况下没有人为加入陀螺和加计噪声时误差应该在数值精度范围内。如果误差曲线呈现明显的增长趋势反过来检查姿态更新是否归一化、速度更新是否扣除了哥里奥利、位置更新是否用了正确的纬度和地球半径。% 位置误差分析示意 load(nav_result.mat); % 解算结果 load(ref_traj.mat); % 参考轨迹 % 计算位置误差 lat_err nav_result.lat - ref_traj.lat; lon_err nav_result.lon - ref_traj.lon; alt_err nav_result.alt - ref_traj.alt; figure; subplot(3,1,1); plot(lat_err); title(纬度误差 (rad)); subplot(3,1,2); plot(lon_err); title(经度误差 (rad)); subplot(3,1,3); plot(alt_err); title(高度误差 (m));画出来的误差曲线如果量级在10^-8到10^-6之间属于正常数值积分误差如果量级在10^-2以上或者发散说明前面某一步有逻辑错误。这里我见过最典型的翻车就是坐标系搞反导致纬度误差随时间线性增大位置一路偏到海里去了。4. 组合导航部分怎么用Kalman滤波模型与松组合参数设置4.1 组合导航的两种模式松组合和紧组合在代码里的区别这套程序里的组合导航模块是很多人真正想用的部分。组合导航有松组合、紧组合和深组合之分教材配套程序一般实现的是松组合惯导给出位置和速度GPS也给出位置和速度两者做差作为量测用Kalman滤波估计惯导误差状态再反馈校正。松组合的代码结构通常是一个主循环里先跑惯导解算然后检查GPS数据是否有效有效则构造量测向量、更新Kalman滤波器、输出误差估计最后做反馈校正。紧组合则直接使用伪距和伪距率作为量测不在教材配套代码的典型范围内。所以如果你下载的压缩包里看到文件名带loose、tight等字样优先跑松组合。% 松组合Kalman滤波量测更新示意 % z: 量测向量 [位置误差(3); 速度误差(3)] % H: 量测矩阵对应状态向量中的位置误差和速度误差 % R: 量测噪声阵 z [pos_ins - pos_gps; vel_ins - vel_gps]; % Kalman增益 K P * H / (H * P * H R); % 状态更新 dx K * z; % 协方差更新 P (eye(size(K,1)) - K * H) * P; % 反馈校正惯导输出 pos_ins pos_ins - dx(1:3); vel_ins vel_ins - dx(4:6);这段代码的关键在R矩阵的设置。GPS位置量测噪声一般设成米级速度噪声设成分米级或米级。如果R设得太小滤波会过度相信GPS导致惯导本身的误差特性被掩盖输出曲线剧烈抖动如果设得太大滤波又跟不上真实误差变化反馈校正形同虚设。常见做法是先跑一次开环观测惯导误差曲线的量级再据此设置Q和R。4.2 Kalman滤波参数整定Q阵和R阵的量级从哪里来Kalman滤波在组合导航里的表现一半靠算法推导一半靠参数整定。很多人在这一步上头是因为不知道Q和R该怎么设。这里有一个实用经验先设R再调Q。因为R可以从GPS数据说明书或实验数据里直接估算而Q反映的是陀螺和加计的噪声特性需要通过Allan方差分析得到或者直接参考器件手册。如果你用的是仿真数据器件噪声是轨迹仿真时人为注入的那么Q的理论值可以直接从注入噪声的方差反推。麻烦的是实际采集数据场景这时你可以用惯导静止时陀螺输出的标准差来近似估计角度随机游走。不要一开始就同时调Q和R那样你永远定位不到问题出在哪。% 参数设置示例 % 状态向量一般取[位置误差(3); 速度误差(3); 姿态误差(3); 陀螺漂移(3); 加计零偏(3)] % 对应15维状态 % 陀螺角度随机游走 gyro_white 0.01 * pi / 180; % 0.01 deg/sqrt(hr) 转为 rad/sqrt(s) accel_white 0.001; % 1 mg 噪声 % Q阵构建示意 Q_blk diag([...位置噪声... , vel噪声, gyro_white^2, accel_white^2]); % 实际需要按连续时间离散化公式 Qd G*Qc*G*dt 计算上面代码里Q_blk是示意实际程序中需要用连续时间噪声密度矩阵通过离散化得到离散时间Q阵。这一步教材里有推导程序包里的实现有的直接建立离散Q阵有的用连续模型。你跑通后可以刻意做一件事把Q阵整体放大10倍和缩小10倍观察滤波输出的平滑度和收敛速度这对你理解滤波器行为很有帮助。4.3 反馈校正的两种方式闭环反馈和前馈修正的取舍组合导航Kalman滤波估计出误差状态后需要对惯导输出做校正。常见有两种做法闭环反馈校正和开环前馈修正。闭环方式把误差估计直接反馈到惯导解算内部修正姿态、速度和位置状态量同时把Kalman滤波的状态估计归零或部分归零。开环方式则不清零滤波器状态而是把误差估计累积起来对输出做后处理修正。这套程序里通常使用闭环反馈因为仿真环境下不存在传感器延迟问题闭环稳定性容易保证。但你在实际项目中如果处理的是实时数据流开环前馈往往更稳妥因为闭环反馈一旦滤波器发散会直接把惯导输出污染。注意观察程序里滤波器状态更新完之后有没有把状态向量清零或者重新初始化这决定了它是闭环还是开环。5. 仿真程序的常见坑与排查数据格式、坐标系和初始化三个高频源头5.1 坑一save和load的文件路径不一致导致数据错乱现象轨迹仿真脚本运行正常但后面解算脚本运行时报错“无法打开文件”或者读取到的数据全是0。原因脚本里写死了.mat文件的保存路径你的工作目录不同save写到了当前位置但load仍从原路径读取。解决打开每个脚本搜索所有save和load语句统一改成你本地的绝对路径或者用cd切到工作目录后再运行。5.2 坑二坐标系定义搞反导致速度误差发散现象纯惯导解算结果和参考轨迹对比时速度和位置误差随时间快速发散即使是在没有注入噪声的情况下。原因导航系定义是北东地但你在初始姿态设置时用了东北天的习惯或者四元数初始化时旋转顺序不对。解决先看程序注释里关于坐标系的说明把初始姿态角转成四元数时注意旋转顺序是Z-Y-X还是Z-X-Y。常见的做法是写一个临时脚本把姿态角先转成方向余弦矩阵再转四元数每一步打印出来和手算结果对比。5.3 坑三MATLAB内置函数与程序自带函数同名冲突现象运行到某个环节报错说函数输入参数个数不对但检查代码发现调用方式没错。原因程序包自带了一个att_update.m而你的MATLAB工具箱里也有同名函数addpath的搜索顺序导致调用了工具箱版本。解决运行前用which 函数名 -all列出所有同名函数位置看当前解析到哪个。然后用rmpath移掉不需要的路径或者修改本地函数名。这个坑在带quat、att、ins前缀的代码里尤其常见。5.4 坑四数据文件里的单位假设与程序不匹配现象姿态误差曲线正常但位置误差曲线出现周期性波动。原因GPS数据文件里的经纬度可能是度而程序内部使用弧度导致差分时量测跳变。解决检查所有数据加载后有没有做单位转换比如乘以pi/180。有的版本代码把单位转换封装在初始化脚本里如果初始化脚本被跳过执行就会出现这种错位。5.5 坑五初始化失准导致滤波前期震荡剧烈现象组合导航跑通后前100秒位置误差很大后面才收敛。原因滤波状态初值设置不合理P阵初值太小导致滤波器认为初始误差为零不敢采信新量测。解决把P阵初值设得保守些位置误差方差给到10^2量级姿态误差方差给到(1度)^2量级让滤波器在前几十秒内有足够的自由度去校正误差。6. 把仿真结果变成论文级图表误差曲线规范与算法验证技巧6.1 三种必画的误差曲线姿态、速度、位置的误差对比图跑通程序只是第一步真正见功夫的是把结果可视化让审稿人或导师一眼能看到算法的性能边界。至少要画三组图姿态误差曲线横滚、俯仰、航向、速度误差曲线东向、北向、天向、位置误差曲线经度、纬度、高度。每组图都用同一把尺子画参考轨迹和仿真轨迹误差单独列出不要混在一张图里。画图有一个实用技巧横轴统一用时间秒纵轴标注清楚单位和量级。姿态误差用度或角秒速度误差用m/s位置误差用米。不要直接画弧度值让读者自己换算也不要画成对数坐标除非你要展示的数量级跨越超过三个量级。标注字体大小统一线宽用1.5以上英寸导出的图片在Word里才不会糊。% 论文级误差曲线标准模板 set(groot, DefaultAxesFontSize, 11); set(groot, DefaultLineLineWidth, 1.5); figure(Units, normalized, Position, [0.1, 0.1, 0.8, 0.8]); subplot(3,1,1); plot(t, roll_err * 180/pi, r); ylabel(Roll Error (deg)); grid on; xlim([0, t(end)]); subplot(3,1,2); plot(t, pitch_err * 180/pi, g); ylabel(Pitch Error (deg)); grid on; xlim([0, t(end)]); subplot(3,1,3); plot(t, yaw_err * 180/pi, b); ylabel(Yaw Error (deg)); xlabel(Time (s)); grid on; xlim([0, t(end)]); sgtitle(Attitude Error Curves);6.2 算法正确性的三个自检方法零偏注入、轨迹退化、与理论误差模型对比如果你要在这个程序包基础上做二次开发最好先验证你对代码的理解是否正确有三个低成本的自检手段。一是零偏注入测试在陀螺和加计读数上人为加一个常值偏差观察解算结果是出现线性漂移还是持续发散判断姿态解算的反馈校正是否生效。二是退化测试把轨迹设成完全静止或匀速直线运动看解算结果是否保持恒定。三是理论对比用教材上的误差传播公式对简化场景推导误差曲线的理论斜率再和仿真结果对比。如果两条趋势线明显不符说明程序里可能存在你不知道的内部校正环节。6.3 我最后要提醒的一件事改动代码前先备份原始版本这是老生常谈但在这套程序上尤其重要。因为不同版本的教材配套代码之间差异不小有的改了坐标系定义有的换了滤波器结构你一旦改了文件后面想对照原版就麻烦了。我吃过这个亏当年为了调参数把某个子程序改了十几次最后发现越改越乱只能重新解压再从头来过。现在我的习惯是保留一份只读的原始压缩包工作目录里永远是复制出来的副本每次改动前用git做一次提交改坏了随时回退。希望这份拆解能帮你少走弯路。如果你跑通后再把自己的参数配置和踩坑记录沉淀下来这套程序的价值会远远超出教材本身。本文还有配套的精品资源点击获取