刚接触组合导航那会儿我拿一套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和GNSSMATLAB里最常见的实现路线有两条一种是直接对导航状态做估计的总状态卡尔曼滤波另一种是估计导航误差状态再回灌修正的误差状态卡尔曼滤波也就是ESKFError-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_arm3.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开始先用仿真数据或者采集的静态数据把滤波器跑稳再逐步加上动数据、失锁模拟、异常量测。每一步都验证清楚再往前走看似慢实际是最快路径。最后再分享一个小技巧在滤波代码的每个关键步骤后加一行注释标明当前变量的坐标系和单位这个习惯在项目后期查问题的时候能救你命。 
长河君 · 2026/10/12 22:04:17 · 阅读 1.2k 基于MATLAB的INS/GNSS组合导航：卡尔曼滤波与ESKF实现
刚接触组合导航那会儿我拿一套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和GNSSMATLAB里最常见的实现路线有两条一种是直接对导航状态做估计的总状态卡尔曼滤波另一种是估计导航误差状态再回灌修正的误差状态卡尔曼滤波也就是ESKFError-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_arm3.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开始先用仿真数据或者采集的静态数据把滤波器跑稳再逐步加上动数据、失锁模拟、异常量测。每一步都验证清楚再往前走看似慢实际是最快路径。最后再分享一个小技巧在滤波代码的每个关键步骤后加一行注释标明当前变量的坐标系和单位这个习惯在项目后期查问题的时候能救你命。