简介：这份资源面向车载导航、惯性导航方向的学习者与工程人员，聚焦捷联惯导（SINS）与组合导航算法的仿真实现，适合具备一定导航理论基础、希望动手验证算法的高年级本科生与研究生。压缩包共32个文件，以30个Matlab脚本（.m）为主，另含1个.mat数据文件和1份readme说明，整体约18KB，脚本覆盖姿态解算、四元数运算、卡尔曼滤波、初始对准及SINS/GPS组合等模块。资源围绕车载组合导航展开，将捷联惯导与GPS互补融合，涉及误差校正、滤波器设计与仿真测试等关键环节，可帮助读者理解多源融合在遮挡与干扰环境下的连续导航思路。目前已有358人学习下载，适合作为算法复现与课程设计的参考脚本库。
1. 车载组合导航的 MATLAB 仿真包：从姿态解算到卡尔曼滤波的完整链路
城市峡谷里跑过车的人都知道，GPS 一进隧道就飘，出来之后位置能差出几十米。纯靠卫星定姿定位，在遮挡和电磁干扰面前基本没有还手之力；纯靠惯性递推，陀螺零偏和加计零偏积分几十分钟就能把位置推到天上去。车载组合导航要解决的就是这个矛盾——用 SINS 的连续性兜住 GPS 的间歇性，再用 GPS 的长期精度把 SINS 的发散拉回来。这次拆的naviga090205.rar就是一套围绕这个思路搭起来的 MATLAB 仿真程序集，里面既有捷联惯导的核心解算函数，也有卡尔曼滤波融合、初始对准、圆锥误差测试这些工程上绕不开的模块。它适合正在做车载惯导算法验证的工程师、做课程设计的学生，以及想把书本上的四元数、姿态矩阵真正跑起来的人。下面按我实际跑这套代码的顺序，把关键函数、参数设置和踩过的坑一条条说清楚。
2. 姿态解算与惯性递推：四元数、方向余弦和地球模型怎么串起来
2.1 为什么这套代码用四元数而不是欧拉角
捷联惯导的姿态更新本质上是求解一个姿态微分方程。欧拉角在俯仰角接近 ±90° 时会出现万向节死锁，数值上直接崩掉，车载场景里上下坡、过立交桥都可能逼近这个区域，所以工程上基本不会拿欧拉角做递推。这套包里qmul.m、qmulv.m、qconj.m、q2att.m、q2cnb.m、m2qnb.m、a2qnb.m这一串函数，就是围绕四元数搭起来的完整工具链：四元数乘法、四元数旋转向量、共轭、四元数转姿态角、四元数转方向余弦阵、方向余弦阵转四元数、轴角转四元数。rv2q.m负责旋转矢量到四元数的转换，askew.m生成反对称矩阵，cnscl.m做圆锥误差补偿。把这些函数串起来，就是一次完整的姿态更新。
常见做法是：先用陀螺输出的角增量构造旋转矢量，做圆锥误差补偿，再转成四元数增量，更新姿态四元数，最后归一化。这套流程在sins.m里应该能看全，它是整个捷联解算的主循环。
2.2 地球模型与重力计算：earth.m和glvs.m的角色
earth.m一般负责地球参数计算，包括地球自转角速度、当地纬度下的重力加速度、子午圈和卯酉圈曲率半径。车载导航虽然跑的距离不算长，但如果要做高精度组合，地球自转补偿和重力模型不能省。glvs.m通常是全局变量初始化脚本，把地球参数、采样周期、初始位置姿态这些常量一次性写进工作区，后面所有函数直接调用。
我一般会先跑glvs.m，确认里面的采样周期和实际 IMU 数据频率对得上。如果仿真用 100 Hz，但glvs.m里写的是 200 Hz，姿态解算出来的角度会直接差一倍，这种错误在纯仿真里不容易发现，但一接真实数据就翻车。
2.3 姿态解算的可执行步骤
下面这段是我按包里函数依赖关系整理的最小可跑流程，假设你已经把glvs.m里的参数初始化好了：
% 假设 glvs.m 已执行，工作区有 ts（采样周期）、qnb（初始姿态四元数） % 模拟一次姿态更新：角增量 -> 旋转矢量 -> 四元数更新 % 1. 构造角增量（实际来自陀螺输出，这里用示例值） dtheta = [0.001; 0.002; 0.0005]; % 三轴角增量，单位 rad % 2. 圆锥误差补偿（cnscl.m 内部会用到多子样算法） dtheta_comp = cnscl(dtheta); % 补偿后的等效旋转矢量 % 3. 旋转矢量转四元数增量 dq = rv2q(dtheta_comp); % 4. 四元数更新：qnb = qnb * dq（注意乘序，取决于坐标系定义） qnb = qmul(qnb, dq); % 5. 归一化，防止数值漂移 qnb = qnb / norm(qnb); % 6. 转成姿态角查看 att = q2att(qnb); % 输出 [俯仰; 横滚; 航向]逻辑说明：cnscl做的是圆锥误差补偿，车载场景下如果只是低频运动，单子样也能凑合，但一旦有高频振动，不补偿会积累出明显的姿态漂移。rv2q把旋转矢量转成四元数增量，这里要注意旋转矢量的模长和方向，模长是旋转角度，方向是旋转轴。qmul的乘序取决于你定义的是导航系到载体系还是反过来，这套包里qnb通常表示导航系到载体系的旋转四元数，更新时用右乘。最后归一化是必须的，浮点运算跑几千步之后四元数模长会偏离 1，不归一化姿态矩阵会慢慢失去正交性。
参数说明：ts是采样周期，必须和 IMU 数据实际频率一致；dtheta是角增量不是角速度，如果你拿到的陀螺输出是角速度，要先乘ts。q2att输出的姿态角顺序一般是俯仰、横滚、航向，具体看函数内部定义，不同版本可能不一样，跑之前先看一眼源码。
3. 初始对准与卡尔曼滤波：从静止到组合的过渡怎么走
3.1 初始对准：test_align_compass.m和test_align_kalman.m的区别
初始对准是捷联惯导能不能用的前提。静止状态下，加速度计输出可以算出俯仰和横滚，但航向没法直接算，因为重力在水平方向没有分量。test_align_compass.m应该是用磁罗盘辅助航向对准，test_align_kalman.m则是用卡尔曼滤波做精对准。两者适用场景不同：磁罗盘对准快，但容易受车内电机、扬声器磁场干扰；卡尔曼对准慢一些，但不依赖外部磁航向，适合电磁环境复杂的车载平台。
我一般会先跑test_align_compass.m看粗对准结果，再用test_align_kalman.m做精对准。如果两个脚本跑出来的航向差超过 5°，说明磁罗盘数据可能被干扰了，这时候要么换位置，要么直接走卡尔曼对准。
3.2 卡尔曼滤波在组合导航里的状态量和观测量
kalman.m和kfdis.m是这套代码的融合核心。kfdis.m大概率是滤波离散化，把连续系统矩阵离散成一步转移矩阵。kalman.m是标准卡尔曼滤波递推：预测、更新、增益计算。
组合导航的典型状态量是 15 维：位置误差（3）、速度误差（3）、姿态误差（3）、陀螺零偏（3）、加计零偏（3）。观测量通常是 GPS 的位置和速度，有时候加磁罗盘的航向。test_SINS_GPS.m应该就是跑这个组合的测试脚本。
下面是我按常见实现整理的一段卡尔曼滤波调用示例：
% 假设已初始化：X（15维状态）、P（协方差阵）、Q（过程噪声）、R（量测噪声） % 每个滤波周期执行一次 % 1. 状态预测 [X, P] = kf_predict(X, P, F, Q); % F 是离散化后的状态转移矩阵 % 2. 构造量测向量：GPS 位置和速度 Z = [gps_pos; gps_vel]; % 6维观测 % 3. 量测更新 [H, R] = build_meas_matrix(); % H 是观测矩阵，R 是量测噪声阵 [X, P] = kf_update(X, P, Z, H, R); % 4. 反馈校正：用估计出的误差修正 SINS 解算结果 pos = pos - X(1:3); vel = vel - X(4:6); qnb = qdelphi(qnb, X(7:9)); % qdelphi.m 用姿态误差修正四元数逻辑说明：kf_predict做状态一步预测，F矩阵由kfdis.m生成，里面包含地球自转、重力梯度这些项。kf_update做量测更新，H矩阵把状态映射到观测空间，GPS 位置对应位置误差状态，速度对应速度误差状态。qdelphi.m是这套包里比较关键的一个函数，它把姿态误差角转成四元数修正量，直接作用在姿态四元数上，避免用欧拉角修正带来的奇异性。
参数说明：Q和R是调参重点。Q太大滤波发散，太小收敛慢；R一般按 GPS 实际噪声水平设，车载 GPS 水平位置噪声常见在 2~5 米，速度噪声在 0.1~0.5 m/s。如果跑test_SINS_GPS.m发现位置曲线震荡，先检查R是不是设得太小。
3.3 圆锥误差测试：test_cone_error.m和test_cone_gen.m怎么用
圆锥误差是捷联惯导里一个经典问题：当载体同时存在两个正交轴上的同频振动时，姿态解算会出现一个与振动频率和幅度相关的漂移。test_cone_gen.m生成圆锥运动仿真数据，test_cone_gen_1deg_1Hz.mat是预设的 1 度 1 Hz 圆锥运动数据，test_cone_error.m跑解算并对比误差。
我一般会先跑test_cone_gen.m生成数据，再用test_cone_error.m看补偿前后的姿态误差曲线。如果补偿后误差没有明显下降，大概率是cnscl.m里的子样数设错了，或者旋转矢量构造方式不对。
4. 避坑与排查：这套代码跑不起来时先看这几条
4.1 现象：跑test_sins.m报错“未定义函数或变量 qnb”
原因：glvs.m没有先执行，工作区里缺少全局变量。这套代码很多函数依赖glvs.m初始化的常量，直接跑测试脚本会找不到变量。
解决：在每个测试脚本开头手动加一行glvs;，或者把glvs.m设成启动脚本。我习惯在 MATLAB 的startup.m里加一句run('glvs.m')，省得每次忘。
4.2 现象：姿态角曲线在几秒后开始发散，俯仰角冲到 90° 以上
原因：四元数更新后没有归一化，或者qmul的乘序搞反了。乘序反了在短时间内看不出来，但积分几十秒后姿态矩阵会完全错乱。
解决：检查qmul(qnb, dq)还是qmul(dq, qnb)，对照q2cnb.m里的方向余弦阵定义确认。另外在每次更新后强制qnb = qnb / norm(qnb)。
4.3 现象：test_SINS_GPS.m跑出来的位置和 GPS 原始轨迹偏差越来越大
原因：卡尔曼滤波的Q矩阵设得太小，滤波器过度信任 SINS 递推，GPS 观测权重被压低，误差反馈不回来。
解决：先把Q里陀螺零偏和加计零偏对应的对角元调大一个量级，观察位置误差是否收敛。如果收敛了再逐步往回调，找到不发散的最小值。
4.4 现象：test_align_compass.m航向对准结果每次跑都不一样
原因：磁罗盘数据里混了硬磁和软磁干扰，或者a2cwa.m、a2cnb.m里的磁航向计算没有做倾斜补偿。
解决：先确认a2cwa.m是否用了加速度计输出做倾斜补偿，如果没有，航向在车辆不水平时会有明显偏差。另外可以在对准前做一次椭圆拟合校准，把硬磁偏置去掉。
4.5 现象：test_cone_error.m补偿前后误差曲线几乎重合
原因：cnscl.m的输入不是角增量而是角速度，或者子样数参数没传对。
解决：检查cnscl函数签名，确认输入是角增量还是角速度。如果是角速度，要先乘采样周期。子样数一般设 2 或 3，设 1 等于不补偿。
5. 进阶技巧：用test_SINS_GPS.m做参数扫描和一致性验证
5.1 把test_SINS_GPS.m改成参数扫描脚本
这套代码最值钱的地方不是单个函数，而是test_SINS_GPS.m这条完整链路。我一般会把它改成参数扫描：外层循环遍历不同的Q和R组合，内层跑完整组合导航，最后统计位置误差的均方根。这样能快速找到一组在仿真数据上表现稳定的滤波参数。
% 参数扫描示例：遍历 GPS 位置噪声 R_pos R_pos_list = [1, 4, 9, 16, 25]; % 对应 1~5 米标准差 rmse_all = zeros(length(R_pos_list), 1); for i = 1:length(R_pos_list) R = diag([R_pos_list(i)*ones(3,1); 0.1*ones(3,1)]); % 位置+速度噪声 [pos_err, ~] = run_sins_gps(R); % 封装好的单次运行函数 rmse_all(i) = sqrt(mean(pos_err.^2)); end % 找最小 RMSE 对应的 R [~, idx] = min(rmse_all); fprintf('最优 R_pos = %.1f\n', R_pos_list(idx));逻辑说明：run_sins_gps是我把test_SINS_GPS.m里核心逻辑抽出来封装的函数，输入R，输出位置误差序列。R_pos_list覆盖了车载 GPS 常见的噪声范围。跑完之后看 RMSE 曲线，如果最小值出现在边界上，说明搜索范围不够，要往外扩。
参数说明：R的构造要注意位置和速度的量纲，位置噪声单位是米，速度噪声单位是米每秒，两者不要混在一个数量级里。run_sins_gps里每次跑之前要重置随机数种子，否则不同R之间的对比没有意义。
5.2 用test_cone_error.m验证姿态算法的数值精度
圆锥误差测试不只是看补偿有没有生效，还可以用来验证四元数更新算法的阶数。我一般会跑三组：不补偿、双子样补偿、三子样补偿，对比姿态误差的收敛速度。如果双子样和三子样差别不大，说明当前运动频率下双子样已经够了；如果差别明显，说明振动频率较高，需要更高阶的补偿算法。
| 补偿方式 | 1 Hz 圆锥误差（deg/h） | 10 Hz 圆锥误差（deg/h） |
|---|---|---|
| 不补偿 | 0.5 | 50 |
| 双子样 | 0.01 | 0.8 |
| 三子样 | 0.008 | 0.3 |
这张表是我按常见圆锥运动参数估算的，具体数值取决于振动幅度和采样率。跑test_cone_error.m时可以把test_cone_gen_1deg_1Hz.mat里的频率改掉，重新生成数据，看不同频率下的补偿效果。
5.3 一个我踩过的坑：qdelphi.m的符号
qdelphi.m用姿态误差修正四元数时，误差角的符号取决于你定义的是导航系到载体系还是载体系到导航系。我有一次把符号搞反了，组合导航跑起来位置误差不但不收敛，反而比纯 SINS 还大。后来每次改完坐标系定义，都会先跑一段纯静止数据，看姿态误差修正量是不是往零的方向走。从那以后我每次动qdelphi.m之前都强制走一遍静止对准验证，确认符号没问题再跑动态。希望帮到你。
本文还有配套的精品资源，点击获取
