把INS和GNSS伪距放进同一个滤波器里融合这可能是我做组合导航以来最值得写的一篇续篇。上一篇聊完INS基础的机械编排和误差方程后不少朋友催更紧组合的实现细节。这篇文章就一个目标把“INS/伪距组合导航”从公式层面拉到代码层面讲清楚伪距这条观测链路到底怎么长出来的、滤波器前后端怎么搭、以及真机数据里真实存在的坑。内容主要面向正在做GNSS/INS紧组合、跑过RTKLIB或PX4代码的人也欢迎刚入门但啃过惯性导航误差方程的朋友一起讨论我会尽量把每个关键选择背后的理由都说透。1. 从松组合到伪距紧组合为什么要动这条最底层的观测链路1.1 松组合的瓶颈卫星一少组合环路就断了上篇实现的松组合观测值来源是GNSS接收机解算出的位置和速度。逻辑上很顺GNSS输出位置INS输出位置两者求差丢进滤波器。但工程上最头疼的问题是——卫星数量一旦低于4颗接收机输出不了位置解整个组合环路直接断开。城市高架下、两边高楼遮挡的路段、树荫浓密的小路太容易出现这种情况了。更麻烦的是即使能解算松组合拿到的位置解本身已经经过了接收机内部的滤波平滑和INS的误差特性并不匹配融合时反而会引入一些相关噪声。换句话说松组合把“已经消化过一遍的数据”再消化一遍信息损失是不可避免的。紧组合的思路完全不同直接使用接收机的原始观测量——伪距以及多普勒伪距率送到滤波器最底层。1.2 紧组合的系统框架INS做预测伪距做修正松紧组合的差异可以用一句话概括松组合修正的是“定位结果”紧组合修正的是“观测原始值”。紧组合的整体框架如下IMU数据进入INS机械编排以200Hz或更高频率输出位置、速度、姿态GNSS接收机输出原始观测量伪距、多普勒、星历利用INS当前预测的位置、速度逐星预测伪距和伪距率预测伪距减去实测伪距得到新息新息乘以卡尔曼增益得到状态误差的最优估计估计出的误差反馈修正INS状态同时估计陀螺漂移、加计零偏、钟差和钟漂。这个框架的关键在第3步把“位置差”换成“伪距差”多了一层从位置误差到伪距的投影。数学推导不复杂但这层投影恰恰是工程上无数坑的来源后面我会逐步拆。1.3 坐标系与状态向量统一到ECEF少绕一层弯我在第一篇里用了经纬高坐标系来做INS机械编排但紧组合的状态方程我强烈建议直接用ECEF地心地固系。原因很实在GNSS卫星星历给出的位置、接收机算出的几何距离全部都在ECEF下。如果INS那边输出经纬高还要先转成ECEF再做伪距预测中间任何旋转变换差一个小角度都会让视线向量偏一点、新息偏一段。状态向量取17维状态符号维数单位位置误差δr3m速度误差δv3m/s姿态误差φ3rad陀螺漂移bg3rad/s加计零偏ba3m/s²接收机钟差cδtu1m接收机钟漂cδtdot1m/s这里姿态误差用的是psi角模型下的平台误差角与上篇推导的INS误差方程保持一致。钟差和钟漂直接以“米”和“米/秒”为单位因为伪距观测方程天然就在这个量纲下省去换算。2. 状态方程误差传播和时间更新2.1 INS误差传播模型的物理拆解状态方程的核心是INS误差传播模型。直接堆公式容易让人头晕我先讲清楚每一项的物理含义再给离散化方法。姿态误差微分方程的简化形式$$\dot{\phi} \approx -\omega_{in}^{n} \times \phi - C_b^n b_g - C_b^n w_g$$这里$C_b^n$是姿态矩阵$b_g$是陀螺漂移$w_g$是陀螺角速率白噪声。物理含义很直白姿态误差会被导航系相对惯性系的旋转耦合拖拽同时陀螺漂移直接注入角速度误差。换句话说姿态误差不是静止不变的它一边被地球自转和载体运动带着转一边被陀螺的漂移持续污染。速度误差微分方程的简化形式$$\delta\dot{v} \approx -(2\omega_{ie}^{n} \omega_{en}^{n}) \times \delta v - \phi \times f C_b^n b_a C_b^n w_a$$这里$f$是比力$b_a$是加计零偏。这个方程里最大的坑是“$\phi \times f$”这个交叉耦合项——姿态误差在重力方向的分量会直接导致水平速度误差。这也是纯惯导系统定位漂移与姿态误差强相关的根源。很多新手在调滤波器时发现水平位置飘得离谱优先怀疑位置方程其实问题经常出在姿态误差没有及时被约束住。位置误差微分方程在ECEF下非常简洁$$\delta\dot{r} \delta v$$这比经纬高版本省事得多不用考虑子午圈半径、卯酉圈半径、纬度分母那些东西。陀螺漂移和加计零偏按随机常数建模$$\dot{b}g 0 w{bg}, \quad \dot{b}a 0 w{ba}$$意思是零偏在滤波周期内基本不变但允许通过过程噪声吸收它的随机游走。2.2 时钟误差状态钟差和钟漂必须同时进滤波器接收机时钟的误差模型很多人会忽略但紧组合里它直接和位置误差耦合在一起不能不建。时钟状态方程$$\delta\dot{t}_u \delta f_u$$ $$\delta\dot{f}_u w_c$$换成距离单位$$c \cdot \delta\dot{t}_u c \cdot \delta f_u$$ $$c \cdot \delta\dot{f}_u w_c$$为什么钟漂必须进状态因为接收机晶振的频率误差会导致伪距随时间线性累积偏差如果不估计钟漂滤波器会把这段误差强行分配到位置误差上造成位置解缓慢漂移。动态场景下这个问题更明显车辆加减速时频漂造成的伪距率偏差和真实速度变化混在一起很难分辨。所以17维状态里钟差和钟漂各占一维别省。2.3 状态转移矩阵离散化与Q阵给法实际代码里一般先构造连续系统的状态转移矩阵$F$17×17然后做离散化。工程上常用的近似$$\Phi \approx I F\Delta t \frac{1}{2}(F\Delta t)^2$$如果你的IMU是200Hz$\Delta t 0.005s$一阶近似大多够用但如果做无人机这类振动大的平台IMU数据频率又不稳定建议用矩阵指数精确离散化C可以用Eigen库的矩阵指数函数Python可以直接用scipy.linalg.expm。系统噪声协方差阵$Q_d$的离散化$$Q_d \approx G Q_c G^T \Delta t$$$G$是噪声输入矩阵$Q_c$是连续时间噪声功率谱密度。给Q阵时我习惯这样初始化参数器件误差项参考值符号陀螺角度随机游走ARW0.05~0.5 deg/sqrt(h)白噪声驱动姿态和速度加速度计速度随机游走VRW0.05~0.2 m/s/sqrt(h)白噪声驱动速度陀螺漂移随机游走0.01 deg/h/sqrt(h)驱动bg状态加计零偏随机游走0.001 m/s²/sqrt(s)驱动ba状态接收机钟漂随机游走1~10 m/s/sqrt(s)驱动钟漂状态视晶振质量这里需要提醒给Q阵最忌讳“拍脑袋给一个先跑再说”。Q给太小滤波器过度相信模型预测观测的新息总是被压抑最终位置会“黏”在INS的漂移轨迹上Q给太大系统噪声淹没观测位置会跟着伪距噪声大幅抖动。我一般先从器件手册的ARW/VRW换算跑完一段数据再看新息序列特性微调。3. 观测方程伪距如何映射到状态空间3.1 伪距观测模型与接收机输出差异第$i$颗卫星的伪距观测模型$$\rho_i |r_s^i - r_u| c(\delta t_u - \delta t_s^i) I_i T_i \varepsilon_i$$各项含义$r_s^i$是卫星位置$r_u$是接收机天线位置$c\delta t_u$是接收机钟差$c\delta t_s^i$是卫星钟差由星历计算$I_i$是电离层延迟$T_i$是对流层延迟$\varepsilon_i$是热噪声加多路径。这里有一个所有新手都会踩的坑接收机输出的伪距到底是不是“原始伪距”不同厂商、不同固件差异很大。有的接收机在固件里已经减掉了卫星钟差有的甚至做了粗定位后才输出伪距。我当年拿到一台接收机直接用RTKLIB解算的伪距当原始观测量结果滤波器里又减了一遍卫星钟差新息整体偏了大约一个卫星钟差的量级位置解的偏差莫名其妙。拿到新接收机的第一件事建议拍一组伪距和星历手工按公式把几何距离、卫星钟差、电离层、对流层都算一遍和接收机输出比对弄清楚它到底做了哪些修正。这个排查步骤花不了半小时能省后面好几天。3.2 视线向量与H矩阵的逐行推导伪距观测方程线性化后就变成了状态量到观测量的投影关系。假设滤波器的预测位置是$r_{u,pred}$来自INS机械编排卫星$i$的视线向量$$e_i \frac{r_s^i - r_{u,pred}}{|r_s^i - r_{u,pred}|}$$$e_i$就是接收机指向卫星的单位向量。把几何距离在预测位置处一阶泰勒展开$$|r_s^i - r_u| \approx |r_s^i - r_{u,pred}| - e_i^T (r_u - r_{u,pred})$$定义状态量$\delta r r_u - r_{u,pred}$那么伪距残差$$\delta\rho_i \rho_i - \rho_{pred,i} -e_i^T \delta r c\delta t_u \varepsilon_i$$其中预测伪距$$\rho_{pred,i} |r_s^i - r_{u,pred}| c(\delta t_{u,pred} - \delta t_s^i) I_{model} T_{model}$$注意这里要把滤波器当前估计的钟差$c\delta t_{u,pred}$加进预测值否则残差里会多出一个常数偏置。对应第$i$颗卫星的H矩阵行$$H_{\rho,i} [-e_i^T, ; 0_{1\times3}, ; 0_{1\times3}, ; 0_{1\times3}, ; 0_{1\times3}, ; 1, ; 0]$$意思是伪距残差对位置误差的灵敏度是$-e_i^T$对钟差的灵敏度是1对其他状态没有直接投影。如果同时使用多普勒伪距率观测观测模型是$$\dot{\rho}_i e_i^T (v_s^i - v_u) c(\delta f_u - \delta f_s^i) \varepsilon_d$$线性化后的H行$$H_{\dot{\rho},i} [0_{1\times3}, ; -e_i^T, ; 0_{1\times3}, ; 0_{1\times3}, ; 0_{1\times3}, ; 0, ; 1]$$伪距率的加入对速度误差和钟漂的可观测性帮助很大尤其是车辆匀速或静止时多普勒观测能有效压制INS速度误差的缓慢漂移。我在工程实现里通常是伪距和伪距率一起用H矩阵堆叠成$2m \times 17$的形式$m$是可见卫星数。3.3 大气延迟、地球自转与天线相位中心电离层延迟是伪距里最大的随机误差源天顶方向几米到十几米低高度角时更大。处理方式分三个层次双频接收机用双频伪距组合消掉一阶电离层效果最好单频加Klobuchar模型用广播星历里的8个参数能消掉50%~70%单频无修正不推荐残差直接影响滤波。对流层延迟用Saastamoinen或Hopfield模型再配合NMF或GMF映射函数。天顶对流层延迟2~3米低高度角能达到10米级。如果你发现滤波器在低高度角卫星可见时新息偏大大概率是大气残差没建模好。实用的做法是设低高度角截止比如排除低于10°的卫星同时在R阵里给低高度角卫星更大的噪声方差。地球自转修正是另一个容易被忽略的项。卫星信号从卫星端传播到接收机端约需几十毫秒这段时间地球自转了几米ECEF坐标系下的卫星位置需要做一个Sagnac修正。如果不做视线向量的方向会偏一点新息里出现系统偏差位置解在东西方向上会缓慢偏移。天线相位中心的问题卫星端和接收机端的相位中心偏移在高精度应用里必须改正。消费级接收机可以忽略接收机端偏移但卫星端的PCO、PCV在RTKLIB星历里通常有对应参数最好在伪距预测时一并处理。3.4 观测噪声R阵怎么给高度角加权模型伪距观测噪声不是恒定的低高度角卫星的伪距噪声明显大于天顶方向卫星。早期我偷懒R阵给固定值0.3m跑仿真一切正常一上真机新息序列明显偏大低高度角卫星的新息比高高度角大接近一个量级滤波结果自然不好看。后来改成高度角加权模型$$\sigma_{\rho} a / \sin(el)$$或更常用一点$$\sigma_{\rho} a \left(1 \frac{b}{\sin(el)}\right)$$$a$的初始值取0.3~0.5m天顶方向$b$取0.2左右。伪距率噪声给0.03~0.1 m/s。如果接收机直接输出载噪比CN0也可以用CN0查表映射到噪声方差动态环境下CN0变化很快高度角模型虽然糙但胜在稳定、好调、可复现。4. EKF组合滤波器的工程实现4.1 时间更新与量测更新的时序编排紧组合系统有一个典型的时间匹配难题IMU是200HzGNSS伪距通常1~10Hz两个数据源的时间戳必须严格对齐。伪距测量对应的是接收机信号测量时刻IMU数据对应的是数据采集时刻。在代码里我建议这样组织主循环while (有数据) { if (新的IMU数据 该IMU时刻 当前伪距时刻) { 执行时间更新传播状态与协方差; } if (到达伪距量测时刻) { 逐颗卫星计算视线向量、预测伪距、新息; 组装H阵、R阵; 执行量测更新; 反馈修正INS状态清零误差状态; } }这个“先传播到量测时刻再做更新”的顺序符合卡尔曼滤波的马尔可夫假设。很多初版实现图省事把GNSS数据攒到IMU周期边界再统一处理结果就是观测时刻和状态时刻对不齐动态环境下新息总是带一个与加速度相关的偏差。4.2 新息生成的细节清单新息计算是整条链路里最容易出错、也最值得花时间写清楚的地方。完整写法$$y_i \rho_{measured,i} - \rho_{predicted,i}$$而$\rho_{predicted}$展开后$$\rho_{predicted} |r_s^i - r_{INS,pred}| c(\delta t_{u,est} - \delta t_s^i) I_{model} T_{model}$$三个容易做错的点我逐个说第一卫星钟差重复修正。如果接收机输出的伪距已经包含了卫星钟差修正这里就不能再减$c\delta t_s^i$否则新息会整体偏移。判断方法就是3.1里说的拿一组数据手工核验。第二地球自转修正和视线向量的一致性。计算$r_s^i$时要用发射时刻的卫星位置并补偿Sagnac效应不能直接用接收时刻的ECEF位置。视线向量$e_i$也相应使用补偿后的卫星位置计算。第三大气模型修正的基准问题。Klobuchar模型修正的电离层延迟是对流层顶以上的部分Saastamoinen模型修正的对流层延迟是几何距离减去中性大气折射路径两者分别作用于伪距的不同区段。实现时最好都用“斜路径延迟”而不是天顶延迟否则低高度角时残差会有明显规律性。4.3 反馈修正与误差清零策略紧组合工程实现里INS状态和滤波误差状态的关系特别容易绕晕。我采用的方案是闭环反馈步骤如下滤波器估计出$\delta r$、$\delta v$、$\phi$等误差状态用这些误差修正INS的位置、速度、姿态把状态向量中的位置误差、速度误差、姿态误差清零陀螺漂移、加计零偏、钟差和钟漂这四个状态不能清零。为什么不把后四类状态清零因为它们是估计出来的传感器物理参数和接收机时钟参数清掉等于把之前积累的有效估计全扔了下次又得从零开始收敛。只有位置、速度、姿态误差是“当前时刻与真值的差”修正完成后归零下一次时间更新再重新积累。反馈的频率也有讲究。有些实现每个量测周期都反馈这没问题但要注意如果IMU内部还有自己的IMU内参补偿反馈的频率和补偿逻辑可能冲突需要跟器件固件行为对齐。4.4 滤波器初值、调参与收敛观察滤波器初值对紧组合的影响比很多人想象中更大。一套我常用的初始参数状态初始协方差说明位置误差10m依据GNSS粗定位精度速度误差0.1m/s依据接收机测速精度姿态误差0.5°依据初始对准方式陀螺漂移10 deg/h依据陀螺性能加计零偏1 mg依据加计性能接收机钟差100m依据粗同步精度接收机钟漂10 m/s依据晶振稳定性比较隐蔽的一个问题是钟差初值。如果钟差初值给得不准滤波器会把钟差误差和位置误差混淆导致位置解先偏向一侧直到钟差收敛后才能拉回来。解决方法是利用接收机单点定位反推一个粗略钟差作为初值并把P阵钟差对应项设到100m量级让滤波器快速收敛钟差。调参的观察顺序我个人习惯是先看新息序列再调滤波器。新息应该是零均值、无明显时间相关的序列。如果新息均值不为零先查模型和坐标系问题如果新息方差比R阵大很多再调R如果新息正常但位置轨迹发散才回头调Q和P。一上来就调Q和R是最容易走弯路的方式。5. 真机数据下绕不开的坑与排查经验5.1 时间戳不对齐一个100ms的教训我之前用的一台接收机输出伪距的时间戳用的是本地RTC时间和IMU的UTC时间不完全对齐偏差大概100ms。跑车时车速60km/h100ms对应位置移动约1.7米再加上城市里的动态机动新息序列表现出一副“莫名其妙偏大”的样子。当时我先怀疑星历、坐标转换、大气模型全查了一遍没问题后来把GNSS输出位置和RTK真值轨迹放一起对比才定位到时间戳问题。用互相关方法估计GNSS和IMU的时间偏移后把100ms修正掉滤波器性能立刻恢复正常。从那以后凡是新项目的第一件事就是先做时间和坐标系的标定不然后面的调试全是浪费。5.2 杆臂效应天线中心和IMU中心差多少误差就偏多少车载场景IMU一般装在车辆后轴中心附近GNSS天线在车顶或前挡风玻璃处两者之间的杆臂在ECEF下是一个固定矢量。伪距观测对应的是天线相位中心的位置而INS机械编排输出的是IMU中心的位置。如果忽略杆臂位置误差在水平面上会直接偏一个固定量车辆转弯或俯仰时误差方向还会跟着变。正确处理方式是在预测伪距前补偿杆臂$$r_{antenna} r_{imu} C_b^n l_b$$其中$l_b$是杆臂在载体坐标系下的矢量$C_b^n$是当前姿态矩阵。量杆臂时注意高度方向不要测错车辆有俯仰角时高度误差会投影到伪距上导致高程解异常。5.3 多路径粗差基于新息的RAIM剔除城市环境里多路径造成的伪距粗差是紧组合最大的敌人之一粗差可达几米甚至几十米。我的通用做法是两步检测第一步量测更新前做新息卡方检验。计算$$\nu y^T S^{-1} y$$其中$S H P^- H^T R$是预测新息协方差阵。$\nu$超过卡方分布95%或99%分位数时认为存在故障观测。第二步逐卫星检查归一化新息$|y_i| / \sqrt{S_{ii}}$把最大的一颗剔除重新做卡方检验直到通过。这套基于残差的RAIM思路在紧组合里很实用。但要注意如果一次剔除了超过2~3颗卫星通常不是多路径问题而更可能是时间同步、杆臂校正或某颗卫星星历异常应该先查全局原因而不是继续盲目剔除。5.4 滤波发散的排查顺序碰到滤波器发散我强烈建议按下面的顺序排查而不是一上来就调参数先看新息序列。如果新息本身几十米优先查坐标系统一致性、时间同步、伪距类型理解错没错新息正常但位置发散大概率是Q阵给太小或P阵初始值太小滤波器过度相信状态预报高度发散但水平正常查对流层残差、高程基准统一静止正常、动态发散重点查时间同步和杆臂长时间运行后缓慢发散检查陀螺漂移和加计零偏是否收敛必要时开启车辆运动约束例如ZUPT零速修正来提升可观测性。调试时一定要预留“新息记录”功能把每颗卫星的新息、S对角元、归一化新息都记录下来跑完数据后画出来看远比盯着最终轨迹猜问题高效得多。我在实际项目里靠这个习惯解决了不少诡异问题建议所有做紧组合的人都把数据可视化的调试手段当标配。