刚接触组合导航那会儿，我拿一套MEMS惯导加单频卫星接收机做车载实验。第一次把MATLAB里的卡尔曼滤波跑通，车明明停在原地，解算出来的轨迹却在半小时里漂了两百多米。后来排查了一圈，问题既不在滤波发散，也不在数据同步，而是我对误差状态模型里几个符号的理解出了偏差。这篇东西就是想把三维环境下INS和卫星组合导航的MATLAB实现讲清楚，包含卡尔曼滤波和ESKF两条路线，适合正在做导航算法、想跑通第一版代码，或者在学校里被组合导航作业折磨的同学参考。
1. 先搞明白一件事：组合导航到底在"组合"什么
很多人上来就查卡尔曼滤波公式，结果状态方程、量测方程抄了一堆，却不知道每个变量在三维空间里对应什么物理含义。组合导航这件事，本质上是把两种优缺点完全互补的定位手段绑在一起，用滤波去估计"误差"，再用误差去修正轨迹。
1.1 惯导的长处与短处
惯性导航（INS）靠陀螺仪和加速度计积分出姿态、速度和位置。它的核心优势是完全自主，不依赖任何外部信号，输出频率可以做到100Hz甚至更高，短期内的相对变化非常平滑。你急刹车、过弯道、过隧道，惯导都能跟上。
但它有个天生的毛病：误差随时间累积。陀螺的零偏会让姿态慢慢偏，姿态偏了加速度积分方向就错，位置误差就呈二次方甚至三次方增长。静止十分钟，低成本的MEMS器件就能给你漂出几十米的假位移。这不是算法问题，是物理规律，任何纯惯导系统都逃不掉。
1.2 卫星导航的补位角色
卫星导航（GNSS）给出的是绝对位置，没有长期漂移，误差被稳定在米级。代价是输出频率低（通常1Hz到10Hz），在城市峡谷、隧道、树荫下容易丢星，多径效应严重时位置会突然跳几米甚至几十米。
一个补位，一个纠偏，这就是组合的动机。INS提供高频、连续的相对运动信息，GNSS提供低频、绝对的参考基准，两者一融合，既有了高频平滑的轨迹，又不会漂到天上去。反过来，GNSS短时失锁时，INS还能在几秒到几十秒内维持住一个可用的位置估计。
1.3 松组合与紧组合：先别急着上高难度
组合导航按量测信息的使用方式划分，最常见的是松组合和紧组合。
松组合把GNSS解算出来的位置、速度当作滤波器的量测，直接和INS预测值做差。实现简单，INS和GNSS各自独立工作，任一方出问题都好排查。缺点是GNSS在跟丢卫星、伪距不足时压根解不出位置，你也就没量测可用。
紧组合更进一步，用伪距、伪距率这些原始观测量来做滤波。它能在卫星数不足四颗时继续工作，精度和鲁棒性更好，但状态向量要扩展出接收机钟差、钟漂，量测方程是非线性的，实现复杂度明显上一个台阶。
我建议入门先从松组合入手，把误差模型的物理含义吃透，把滤波器的调试手感练出来，再去看紧组合。这篇分享的主体也是松组合框架下的两种滤波实现。
2. 卡尔曼滤波与ESKF：原理差异与选型逻辑
同样是融合INS和GNSS，MATLAB里最常见的实现路线有两条：一种是直接对导航状态做估计的总状态卡尔曼滤波，另一种是估计导航误差状态再回灌修正的误差状态卡尔曼滤波，也就是ESKF（Error-State Kalman Filter）。两条路都能用，但思路和适用场景有明显区别。
2.1 总状态卡尔曼滤波是怎么工作的
总状态滤波把位置、速度、姿态（通常用四元数或欧拉角）以及传感器零偏全部放进状态向量，系统模型就是INS机械编排方程本身，是非线性的，所以实际用的是扩展卡尔曼滤波（EKF）。
以四元数姿态为例，状态向量通常长这样：
x = [p(3); v(3); q(4); bg(3); ba(3)]
总共16维。预测步直接用IMU数据做机械编排，更新步用GNSS位置和速度修正。看起来直接，但有一个麻烦：四元数要满足单位范数约束，标准EKF的线性更新会破坏这个约束，所以每次更新后必须再归一化。而且四元数的误差定义在更新方程里容易写出歧义，不同参考系下的扰动定义差一个符号，结果就可能南辕北辙。
2.2 ESKF：专门估计"误差"的滤波
ESKF的思路是完全反过来的。状态向量里放的不是导航状态本身，而是导航状态的误差量：
δx = [δp(3); δv(3); δθ(3); δbg(3); δba(3)]
共15维。这里的δθ是姿态误差，用三维向量表示小角度旋转，比四元数直观得多。惯导的机械编排照常做，生成名义轨迹；滤波器只估计这条名义轨迹和真实轨迹之间的偏差。量测更新后，把误差修正回名义状态，然后误差状态清零。
这个设计的妙处在于：
- 姿态误差用三维向量，避免了四元数约束和更新后归一化的麻烦；
- 误差量本身是小量，线性化假设天然成立，滤波器的线性模型更可信；
- 零偏、杆臂这类误差在模型里作为状态被显式估计，可观测性分析更清晰。
2.3 为什么工程实践里ESKF更常见
我个人的体会是，ESKF的调试体验远好于总状态滤波。原因在于误差状态的物理含义非常明确：δp是位置偏差，δv是速度偏差，δθ是姿态小角度偏差。滤波发散的时候，你看误差状态的数值变化就能判断问题出在建模还是量测，而不是在一堆耦合的导航状态里瞎猜。
另一个实际因素是，很多惯导产品本身输出的就是经过内部修正的导航解，外部组合需要估计的是自己的安装误差、时间延迟等。这种情况下，误差状态模型几乎是唯一干净的框架。业界大量开源方案选ESKF不是没有道理的，它的数值稳定性也更好，避免了大数和小数混在一个状态向量里导致的病态问题。
2.4 选型建议
如果只是把GNSS位置融合进惯导轨迹，手上这套系统又已经稳定运行，总状态滤波也够用。但如果你要从零搭建，或者后续要接视觉、气压计、轮速计等多种传感器，我建议直接上ESKF。扩展性不是一个量级的。后面紧组合、多传感器融合，ESKF框架基本不用推倒重来，只是往误差状态里加新的误差项而已。
3. MATLAB源码拆解：三维状态向量、系统矩阵与量测更新
这一节是核心。我不打算给你一段几千行的完整工程代码，而是把从零实现一个能跑的松组合ESKF的每个环节拆开讲，给出关键矩阵和代码骨架。理解了这些，任何完整源码你都能读得懂。
3.1 状态向量设计：15维到底怎么排
三维组合导航里常用的ESKF状态向量如下，顺序要和代码保持一致，否则索引错一位，整个滤波就乱了：
δx = [δp(3); δv(3); δθ(3); δbg(3); δba(3)]
其中：
- δp：位置误差，单位米（在北东地或导航系下）
- δv：速度误差，单位米每秒
- δθ：姿态角误差，单位弧度
- δbg：陀螺零偏误差，单位弧度每秒
- δba：加速度计零偏误差，单位米每二次方秒
代码里用一个列向量管理索引，比如：
idx = struct(); idx.p = 1:3; idx.v = 4:6; idx.th = 7:9; idx.bg = 10:12; idx.ba = 13:15;这个struct在后面拼矩阵的时候非常管用，改状态维数时只需要动一处。
3.2 INS机械编排与误差传播方程
滤波预测步之前，先用IMU做一次机械编排。常用的是北东地（NED）导航系下的捷联解算：
位置更新： p_new = p_old + v_old * dt + 0.5 * (R * (a_meas - ba) + g) * dt^2
速度更新： v_new = v_old + (R * (a_meas - ba) + g) * dt
姿态更新用角增量： q_new = q_old ⊗ q(ω_meas * dt)
这部分是名义轨迹的计算，和滤波无关，误差会随时间累积，这正是滤波器要修正的对象。
误差状态的连续时间传播方程是：
δp' = δv
δv' = -R * [a_meas - ba]_× * δθ - R * δba
δθ' = -[ω_meas - bg]_× * δθ - δbg
其中[a]_×是向量a的反对称矩阵。注意这里姿态误差的符号约定，不同文献可能相反，我用的约定是δθ = R的真实误差R_err对应的右扰动。符号错了会导致修正方向反掉，滤波直接发散，这是新手最容易踩的坑。
3.3 状态转移矩阵的离散化
连续误差方程可以写成：
δx' = F * δx + w
其中F是15×15的矩阵，大部分块是零。把F写出来后，用一阶近似做离散化就够用：
% F矩阵构建 F = zeros(15, 15); F(idx.p, idx.v) = eye(3); F(idx.v, idx.th) = -R * skew(a_meas - ba); F(idx.v, idx.ba) = -R; F(idx.th, idx.th) = -skew(omega_meas - bg); F(idx.th, idx.bg) = -eye(3); % 离散化（一阶近似） Phi = eye(15) + F * dt;过程噪声协方差Q由陀螺和加计的噪声密度积分而来，可以近似为：
Q = Phi * G * Qc * G' * Phi' * dt;更常见的做法是直接对连续时间系统计算离散化Q，这里为了代码简洁先用一阶近似。实际调试时Q的量级对滤波表现影响很大，后面专门讲。
3.4 量测模型：GNSS位置和速度更新
松组合的量测就是GNSS给出的位置和速度。量测方程写为：
z = H * δx + v
其中：
z = [p_gnss - p_ins; v_gnss - v_ins]
H矩阵直接把对应的误差状态挑出来：
H = zeros(6, 15); H(1:3, idx.p) = eye(3); H(4:6, idx.v) = eye(3);注意一个细节：GNSS天线和惯导中心通常不在同一个点，有一段杆臂。如果忽略杆臂，位置量测里会带一个固定的系统偏差。严格的实现要在量测形成前做杆臂补偿，把GNSS位置投影到惯导中心：
p_gnss_corrected = p_gnss - R * lever_arm
3.5 一个能跑通的最小ESKF代码骨架
把上面的矩阵拼起来，标准卡尔曼滤波的五个公式就是全部：
% 预测步 x_pred = Phi * x_est; P_pred = Phi * P_est * Phi' + Q; % 更新步（有GNSS量测时） K = P_pred * H' / (H * P_pred * H' + R); x_new = x_pred + K * (z - H * x_pred); P_new = (eye(15) - K * H) * P_pred * (eye(15) - K * H)' + K * R * K'; % 误差回灌 p = p_ins + x_new(idx.p); v = v_ins + x_new(idx.v); q = q_ins ⊗ quat_from_small_angle(x_new(idx.th)); bg = bg + x_new(idx.bg); ba = ba + x_new(idx.ba); % 误差状态清零 x_new(idx.p) = 0; x_new(idx.v) = 0; x_new(idx.th) = 0; x_new(idx.bg) = 0; x_new(idx.ba) = 0; P_est = P_new;P的更新我用了Joseph形式，数值稳定性比简化式好得多，而且能保证对称正定，强烈建议养成这个习惯。误差清零后P不要清零，它保留的是估计的不确定度，下一轮预测会继续用。
4. 调试实录：初始对准、噪声协方差与滤波发散
代码能跑和跑得好完全是两回事。我第一次实现时，静止状态下轨迹一路南漂。后来把调试过程总结下来，发现大部分问题都集中在几个点上，而且有规律可循。
4.1 初始姿态不准导致的第一轮漂移
组合导航滤波器对初始姿态极其敏感。初始俯仰和横滚差1度，重力分解到水平方向就有大约0.17m/s²的等效加速度误差，位置误差会按t²增长。如果初始航向差5度，那基本是灾难性的。
解决办法是进滤波前先做几分钟静止对准。利用加速度计测量重力方向求俯仰和横滚，用陀螺仪积分或者磁力计辅助求航向。静止对准后再启动组合，滤波器才能"站得住"。
我在调试时遇到的一个典型现象是：滤波没发散，但轨迹整体向一个方向倾斜偏移。排查下来就是初始姿态不准，水平误差不到1度，位置却在十分钟内偏了上百米。把初始俯仰横滚校准到0.1度以内后，问题立刻消失。
4.2 过程噪声与量测噪声的调参逻辑
Q和R的设定是组合导航调试里最耗时的一环。很多新手在这里反复试探，其实有基本逻辑可以遵循。
R矩阵代表对GNSS量测的信任程度，可以由GNSS定位的噪声水平估计。单点定位水平在2到5米，速度噪声在0.1到0.3m/s左右，R就按这个量级设。R设得太小，滤波器会过度相信GNSS，轨迹跟着GNSS的跳动噪声走，反而失去平滑性；R设得太大，滤波又懒得修正，INS误差持续累积。
Q矩阵代表过程噪声，即IMU误差模型的驱动强度。陀螺噪声密度通常在0.01到0.1度/√h量级，加速度计噪声密度在0.01到0.1mg/√Hz量级。把IMU手册里的噪声密度换算成连续时间的功率谱密度，乘到Q里，再乘以离散化时间，就是一个合理的起点。
调参的一个实用技巧是看滤波器的新息序列（innovation）。新息过大说明模型或者量测偏差大；新息长期偏小车漂说明Q太小，滤波器过度自信；新息均值不为零则说明存在未建模的系统误差，比如杆臂或者时间同步问题。
4.3 GNSS异常值处理：别让一个跳点毁了整条轨迹
GNSS在城市里出现几米的跳变是常态。卡尔曼滤波假设量测噪声是高斯白噪声，一旦出现非高斯的大跳变，一个异常量测就能把估计拽偏。
一个简单有效的办法是新息卡方检验。计算马氏距离：
d = z' * S^(-1) * z
其中S = H * P * H' + R是新息协方差。d服从卡方分布，自由度等于量测维数。取95%置信度的阈值，超过就直接丢这次量测，不更新。这个逻辑在实际工程里几乎是标配。
另外一个稳妥的做法是给量测加一个滑动窗口的速度一致性检查：当前GNSS位置推算的速度和INS速度差异过大，就认为这次GNSS定位不可靠，降权处理而不是直接丢弃。
4.4 坐标系与单位的前后一致性
这是最隐蔽也最容易翻车的坑。MATLAB里角度用弧度，位置用米，但有些数据文件里姿态角可能是度，速度可能是节，位置可能是经纬度。一个单位没换算，滤波器就可能输出一个看起来正常但实际完全错误的轨迹。
坐标系方面，要明确：
- IMU输出的角速度和加速度是在载体坐标系（b系）；
- 组合导航解算通常在导航系（n系，北东地或东北天）；
- GNSS位置通常给的是经纬高，要先转换成导航系下的局部坐标（比如以初始点为原点的米制坐标）。
我在代码里专门写了一个单位检查函数，在数据加载阶段就把所有量统一成标准单位，并对姿态角做取值范围断言。这些小习惯看起来繁琐，但在数据源多、格式杂的项目里能省掉大量排查时间。
5. 从松组合到紧组合：代码框架怎么扩展
如果你已经跑通了松组合ESKF，想进一步做紧组合或者融合其他传感器，代码框架的扩展路径其实很清晰。
5.1 紧组合的状态扩展
紧组合用伪距和伪距率做量测，需要在状态向量里加上接收机钟差和钟漂：
δx = [δp(3); δv(3); δθ(3); δbg(3); δba(3); δclk(1); δclkd(1)]
共17维。量测方程变成了每条伪距对应一个非线性函数，需要用卫星位置和接收机估计位置计算预测伪距：
% 伪距量测 z_r = rho_meas - (norm( sat_pos - p_est ) + clk_est); % 对应的H行向量 H_row = [-(sat_pos - p_est)' / norm(sat_pos - p_est), 0,0,0,0,0,0,0,0,0,0,0, 1, 0];紧组合的好处是卫星数少于四颗时照样能工作，伪距率还能在GNSS速度解算质量差时提供速度修正。代价是每个历元要遍历所有可见卫星，计算量明显增加，而且伪距里包含电离层、对流层延迟，不做差分修正的话精度受限。
5.2 融合其他传感器的扩展思路
ESKF框架最爽的一点是传感器融合像搭积木。想加气压计测高度？只需要在量测里加一行，H矩阵对应δp_z的位置放一个1。想加轮速计约束？量测方程变成载体前进方向速度等于轮速，H矩阵对应v和姿态误差的线性组合。
每个新传感器进入框架后，关键仍是检查可观测性。比如纯GNSS位置加高度计，对δθ的可观测性提升有限；加了轮速计后对航向的可观测性会有明显改善。做滤波设计时，先算一下可观测性矩阵的秩，能少走很多弯路。
5.3 结果验证的三个硬指标
最后说下怎么验证你的实现对不对。单看轨迹图是不够的，我用三组指标判断：
第一是姿态误差。如果实验室有高精度转台或参考系统，直接对比滤波输出的姿态和参考值，看稳态误差在多少度以内。
第二是位置误差的收敛性和平稳性。GNSS可用时，位置误差应该稳定在量测噪声量级附近，不应该呈持续增长趋势。把误差画成时间序列，如果形状像一个缓慢上升的坡道，说明存在未修正的系统性偏差。
第三是新息统计特性。新息序列应该近似零均值、白噪声，协方差和理论值S吻合。用MATLAB的autocorr函数看一下新息的自相关，如果出现明显的非零滞后相关，说明过程模型和实际系统存在偏差，需要回头检查状态转移矩阵或者Q的设置。
我自己调试时最常用的是最后一个方法，因为它不依赖外部真值，只要有滤波运行日志就能给出判断方向。
6. 数据驱动的调试工具：可视化脚本怎么写
算法部分聊得差不多了，我想专门分享一个实战中特别有用的东西：调试用的可视化脚本。组合导航出问题时，光看数字很难定位根源，把轨迹、误差、新息都画出来，很多时候一眼就能看出问题在哪。
6.1 三个必画的图
第一个是平面轨迹图，把GNSS原始轨迹、INS纯机械编排轨迹、组合导航输出画在同一个坐标系里。三者之间的关系非常直观：纯INS轨迹会漂，GNSS轨迹有噪声，组合轨迹应该贴近GNSS但更平滑。如果组合轨迹明显偏离这两者，说明滤波器状态有问题。
第二个是位置误差时间序列图，画出组合输出与GNSS的差值。正常情况应该围绕零值小幅波动；如果呈现线性增长或者台阶式跳变，分别对应系统误差和GNSS量测异常。
第三个是新息序列图。我习惯把新息和2倍标准差带画在一起，新息持续超出边界就是滤波发散的前兆，应该立刻暂停数据回放检查。
6.2 一个简单的回放框架
调试用的MATLAB脚本不需要多复杂，但要支持分段回放。我通常把数据按时间切块，对每个时间段重新跑一次滤波，观察滤波器从不同初始条件出发的收敛情况。
% 分段回放：处理第seg_num段数据 seg_start = (seg_num - 1) * seg_len + 1; seg_end = min(seg_start + seg_len - 1, N); for k = seg_start:seg_end % 机械编排 % 滤波预测 % 量测更新 % 记录结果 end % 绘制本段轨迹与误差 plot_trajectory_segment(results, seg_num);这个框架的价值在于，你可以故意把某一段数据的量测全部去掉，模拟GNSS失锁，观察INS纯积分段的漂移速度，从而评估系统在真实场景下的鲁棒性。我在做隧道场景模拟时就靠这个功能验证了失锁30秒以内的轨迹保持能力。
7. 总结一点个人心得
做组合导航这几年，我最大的感触是：算法公式本身并不难，难的是让每一个工程细节都对上。坐标系、单位、时间基准、杆臂补偿、噪声模型，任何一个地方出错，滤波器的表现都可能是灾难性的。MATLAB作为原型验证工具非常合适，代码写起来快，调试可视化方便，数据分析和算法迭代的效率远高于其他语言。但要注意，MATLAB原型和真正的嵌入式实现之间还有一段距离，包括数值精度、实时性、内存管理，这些都需要在后续工程化阶段重新考量。
如果你正在搭自己的第一版INS/GNSS组合导航，我的建议是从松组合ESKF开始，先用仿真数据或者采集的静态数据把滤波器跑稳，再逐步加上动数据、失锁模拟、异常量测。每一步都验证清楚再往前走，看似慢，实际是最快路径。最后再分享一个小技巧：在滤波代码的每个关键步骤后加一行注释，标明当前变量的坐标系和单位，这个习惯在项目后期查问题的时候能救你命。
