企业官网建设流程全解析

1. 项目概述：为什么第一篇文章就聊“伪距”

做组合导航的人，迟早会遇到一个绕不开的组合模式——INS/伪距组合。很多新手一开始接触的都是位置、速度松组合，也就是把卫导解算出来的经纬高和速度直接塞进滤波器，和惯导的位置速度做差。这套思路简单直观，但实际工程里大家更愿意用伪距组合，这里面有深层的技术逻辑。

我最早接触这个项目，是在一个高动态载体上做导航方案预研。当时手里有一套光纤惯导，配上双频接收机，需求是让组合系统在卫星信号被遮挡后又恢复的瞬间，位置误差不要跳变，而且速度精度要能顶得住短时失锁的考验。位置速度松组合在信号恢复瞬间经常出现位置跳变，因为卫导定位解本身在被遮挡期间可能已经发散，而起算位置又不准，导致滤波器的量测更新被污染。后来切换到伪距组合，情况明显好转。

这个系列文章，打算从伪距组合的基本原理讲起，逐步深入到误差方程、量测方程、滤波器实现、粗差剔除、时间同步、几何观测性分析，再到实测数据回放和调参经验。第一篇先把骨架搭起来，讲清楚INS/伪距组合导航到底在做什么、为什么这么做、核心环节有哪些。

适合谁来读？如果你是刚接触组合导航的研究生、做飞控或者车载导航的工程师、想把手里的IMU和接收机数据真正融合起来做后处理的人，这篇文章能帮你把概念理顺。即使你暂时不写代码，光是把“伪距组合到底比位置组合强在哪”这个问题想明白，后续做工程也会少走很多弯路。

2. 内容整体设计与思路拆解：为什么必须从“伪距”入手

2.1 先搞清楚两位主角：惯导和卫导各自的脾气

惯导（INS）的优点是短时精度极高，姿态、速度在几秒到几十秒内非常稳，完全不依赖外部信号，输出频率可以做到200Hz甚至更高。缺点是误差随时间累积，尤其是加速度计零偏和陀螺漂移造成的速度、位置误差，会以二次方、三次方的速度增长。你可以把它理解成一个记性极好但会逐渐“跑偏”的跑步者，出发时每一步都很准，跑得越久，偏离真实路线越远。

卫导（GNSS）则恰恰相反。单点定位精度在几米量级，差分后可以做到厘米级，而且误差是有界、不随时间发散的，只要天上卫星几何够好，每一秒给出的位置都是独立、稳定的。缺点是输出频率一般只有1Hz到10Hz，在城市峡谷、树荫、隧道里容易失锁，动态环境下多路径误差会明显增大。它就像地图上的路标，每个路标本身是准的，但路标和路标之间是断开的，你不知道一个人在两个路标之间到底走了什么路线。

这两个系统一个靠积分、一个靠观测，误差特性完全互补。把两者融合在一起，让惯导持续的短时高精度填补卫导低频和失锁的空白，让卫导的有界误差周期性校正惯导的发散趋势，这就是组合导航的基本盘。

2.2 松组合和紧组合的分水岭

很多人问，组合导航到底“组”在哪里？是按照卫导定位结果来校正惯导，还是按照卫导原始观测值来校正惯导？这就是松组合和紧组合的核心区别。

松组合（位置/速度组合）的做法是：卫导接收机先用最小二乘或卡尔曼滤波解算出位置速度，然后把这些结果当作“量测值”，和惯导推算出来的位置速度做差，送入组合滤波器。结构清晰，实现简单，工程上非常成熟。

但松组合有个隐患：卫导定位解本身已经是滤波后的结果，误差的时间相关性很强，而且接收机内部的滤波器和组合滤波器之间存在“滤波套滤波”的问题。信号被遮挡后重新捕获、或者周跳发生时，卫导解算结果会有一段异常收敛过程，组合滤波器很难区分这是真实的运动还是卫导自身的收敛抖动。此外，当可见卫星少于4颗时，卫导根本无法输出定位结果，松组合在这一瞬间就彻底失去量测更新。

紧组合（伪距/伪距率组合）的做法是：直接把接收机输出的伪距、伪距率、载波相位等原始观测值拿过来，和“惯导推算出的接收机位置与卫星位置之间的几何距离”做差，形成残差进入滤波器。这样就不要求接收机先解算出完整的位置，哪怕只有1颗卫星可见，依然能提供有效的量测约束。在信号部分遮挡的场景下，紧组合的优势非常明显。

2.3 伪距组合解决的三个痛点

第一，解决量测信息利用率的问题。松组合在可见星少于4颗时直接“罢工”，而伪距组合只要有1颗卫星就能把惯导在某个方向上的误差投影到视线方向上做校正。虽然校正不完全，但至少能抑制发散。

第二，解决量测噪声时间相关的问题。接收机内部解算出的位置误差往往在数十秒内是高度相关的，这会让组合滤波器的量测噪声设置失真。伪距观测值经过差分、多路径抑制等处理后，其残差特性更接近白噪声，滤波器更容易“吃得动”。

第三，解决系统启动和信号恢复瞬间的瞬态问题。用位置组合时，卫导重新定位的首次位置可能有几米的阶跃跳变，组合滤波器会把这个跳变当成真实运动去校正惯导，导致输出位置跟着跳。伪距组合里每个伪距残差是逐颗星独立进入的，即使某颗星的伪距有偏差，也可以通过残差检验把这一颗剔除掉，不会污染整体状态估计。

这几点合在一起，决定了伪距组合成为工程上更“正式”的组合方式，也是很多论文、教材在讲“深组合”之前必讲的一环。

3. 核心细节解析与实操要点：伪距组合导航到底在算什么

3.1 系统状态：你究竟在估计什么

INS/伪距组合系统最常见的做法是采用误差状态（error-state）形式的扩展卡尔曼滤波。所谓误差状态，就是不去直接估计位置、速度、姿态的绝对值，而是估计惯导解算值与真实值之间的偏差。这背后的理由很实在：惯导解算的轨迹本身已经比较接近真实轨迹，误差是小量，线性化误差可以被接受，而直接用全状态滤波需要应对大范围非线性的问题，数值稳定性反而难保证。

典型的状态向量包含这些分量：

• 姿态误差（3维）：惯导平台系与真实导航系之间的失准角，用φ表示。
• 速度误差（3维）：惯导解算速度与真实速度之差。
• 位置误差（3维）：经度、纬度、高度误差。
• 陀螺零偏（3维）：沿载体轴系的陀螺漂移，一般建模为一阶马尔可夫或随机常数。
• 加速度计零偏（3维）：同理。
• 接收机钟差（1维）：接收机时钟相对于卫星系统时间的偏差。
• 接收机钟漂（1维）：钟差的变化率。

一共15维状态（不含钟差钟漂是15维，加上是17维，具体看是否估计钟漂）。为什么要把钟差、钟漂放到状态里？因为伪距观测值里天然混入了接收机钟差，如果不对它建模，伪距残差里会残留一个共模偏差，导致位置估计被系统性拉偏。

3.2 量测方程：伪距残差是怎么算出来的

伪距的观测方程写出来是这样：

ρ_i = |r_sat_i - r_rec| + c·Δt_rec - c·Δt_sat_i + I_i + T_i + ε_i

其中ρ_i是接收机对第i颗卫星的伪距观测值，r_sat_i是卫星位置，r_rec是接收机位置，c是光速，Δt_rec是接收机钟差，Δt_sat_i是卫星钟差（可以通过导航电文修正），I_i和T_i分别是电离层和对流层延迟，ε_i是观测噪声。

组合滤波器里的做法是：把惯导推算出来的接收机位置r_INS当作接收机位置的参考值，计算它与卫星位置的几何距离：

d_i = |r_sat_i - r_INS|

然后用观测伪距减去这个几何距离（并修正卫星钟差、电离层、对流层），得到的残差里包含的信息是：惯导位置误差在卫星视线方向上的投影 + 接收机钟差 + 未完全修正的传播延迟。

再把“真实位置 = 惯导位置 + 位置误差”代入，做一阶泰勒展开，就得到量测方程：

z_i = ρ_i - d_i - (c·Δt_sat_i + I_i + T_i) = H_i·δx + c·Δt_rec + ε_i

这里的H_i是视线向量（line-of-sight）的负方向与接收机到卫星方向的单位向量有关，具体写出来是三维视线单位向量投影到位置误差上的负号问题，每个滤波器实现里符号要仔细核对。

这里有一个新手容易踩的坑：符号。不同的教材、不同的代码实现里，视线向量定义、误差状态定义的正负号可能不一样，直接抄别人的量测矩阵，最后滤波器发散了都不知道是哪个符号写反了。最好的做法是自己从头推一遍，然后用一个仿真场景去验证：如果位置误差在某个方向是正，残差应该是正是负，做一次“符号走查”。

3.3 卫星位置计算和钟差处理

伪距残差的正确性，高度依赖卫星位置的准确性。卫星位置由广播星历计算，通常采用开普勒轨道参数递推。这里的细节很多：GPS的星历参数和BDS的参数不完全一样，Galileo也有细微差异，计算的时候一定要按对应系统ICD里的公式来。

工程上常用的做法是提前把星历缓存下来，解析出轨道根数，然后按参考时刻外推。外推时间不宜过长，超过2小时的星历外推精度明显下降，组合系统的残差里会掺入系统性偏差。

卫星钟差通过星历里的钟差参数修正。广播星历给出的钟差参数是二阶多项式，钟差修正公式为：

Δt_sat = a0 + a1·(t - t_oc) + a2·(t - t_oc)^2

需要注意的是，这个修正值只适用于常规精度应用。如果要更高精度，需要用到精密星历和精密钟差产品。在组合导航的实时场景里，广播星历的钟差修正足够；在后处理高精度场景里，可以用精密产品替代。

电离层延迟的修正方式取决于接收机是否双频。双频接收机可以直接用无电离层组合消除一阶电离层延迟，单频接收机则需要用Klobuchar模型或格网产品来修正。在伪距组合中，电离层残差如果不被修正，会直接混入伪距残差，影响滤波器的位置估计。实测中，电离层活跃期的残差可以达到十几米，这时候如果不处理，组合导航的水平位置大概率会被拉偏几米。

3.4 最小二乘/卡尔曼滤波的融合过程

伪距组合滤波器的核心流程可以分成预测和更新两步。

预测阶段：使用IMU数据做惯导机械编排（mechanization），也就是更新位置、速度、姿态，同时把状态误差协方差矩阵P按状态转移矩阵外推。这一步的精度取决于IMU的数据质量和机械编排算法的准确性。很多新手在跑仿真时发现滤波器发散，往往不是量测的问题，而是机械编排本身有bug，比如姿态更新里的圆锥补偿没做、速度积分里的划桨补偿漏了，这些误差会直接流进组合滤波器。

更新阶段：先计算每个可见卫星的伪距残差，构架量测矩阵H，然后按卡尔曼增益公式更新状态估计：

K = P·H^T·(H·P·H^T + R)^(-1) δx = K·z P = (I - K·H)·P

这里的R矩阵是伪距观测噪声的方差，一般设成（1米到3米）^2的量级。R的取值直接影响滤波器的响应速度：R设小了，滤波器相信量测、收敛快但容易受噪声干扰；R设大了，滤波器偏向惯导预测、平滑但反应迟钝。实际工程里R很少是固定值，更常见的做法是和卫星仰角挂钩：低仰角卫星的多路径和大气延迟误差大，R相应放大；高仰角卫星R可以放小。

更新完之后，把估计出的误差状态反馈到惯导解算结果里，然后对误差状态做“重置”，也就是把状态估计置零（或者把误差加到导航状态后把误差状态清零）。这一步看似简单，但处理不好会引入新的问题，比如反馈和协方差更新顺序反了，导致状态已经被校正了、协方差却还停留在“误差已经被估计出来”的水平，下一步预测就会过于乐观。

4. 实操过程与核心环节实现：从零搭一套伪距组合滤波器

4.1 数据准备和坐标系对齐

动手写代码以前，必须先把数据准备好，并且把坐标系之间的转换关系理清楚。伪距组合需要三种数据：

一是IMU数据，包含三轴加速度、三轴角速度，以及对应的时间戳。IMU数据要在载体坐标系（b系）下表达，并且需要提前标定好零偏和尺度因子。如果陀螺和加速度计的数据没有做内参补偿，组合滤波器的状态里估计出来的零偏项会去“吸收”这些误差，结果就是位置精度看起来还行，但零偏估计值严重偏离真实值。

二是卫导原始观测数据，包含伪距、载波相位、多普勒、信噪比（C/N0）、卫星星历。一般接收机输出的是RINEX格式或者厂商私有格式，需要解析出每个观测值对应的卫星号、系统、观测时刻。

三是时间基准信息。IMU时间戳和接收机时间戳必须统一到同一时间基准，这个在后面的章节里要单独讲。

坐标系上，伪距观测方程是在地心地固系（ECEF）里表达的，而惯导的机械编排通常是在当地水平坐标系（比如ENU或NED）里完成的。所以每做一个滤波周期，都需要把惯导在导航坐标系下的位置转换成ECEF坐标，再算和卫星的几何距离。

这里的常见错误是忘了考虑地球曲率。用经纬高转ECEF的时候，如果参考椭球参数用错了（比如用了WGS-84但忘了考虑椭球扁率），会产生百米量级的误差，直接导致伪距残差巨大。

4.2 惯导机械编排和误差状态协方差外推

机械编排就是把IMU的角速度和比力积分为位置、速度、姿态的过程。对捷联惯导来说，姿态更新的核心是姿态微分方程：

C_b^n' = C_b^n · (ω_nb^b ×)

这里的ω_nb^b是载体相对导航坐标系的角速度在载体坐标系的投影，需要从陀螺输出的角速度中扣除地球自转和导航系旋转的影响。地球自转分量在静止状态下也会产生明显的误差，不补偿的话，姿态会在数分钟内漂移。

速度更新要处理引力、离心力、科里奥利力等项。加速度计输出的是比力（specific force），要加上重力加速度才能得到真正的加速度：

v_n' = C_b^n · f_b - (2ω_ie^n + ω_en^n) × v_n + g_n

这里的符号细节很多，每个量在哪个坐标系下表达、叉乘的顺序、正负号，都需要按参考书逐一核对。

在组合滤波器的预测阶段，状态转移矩阵F可以由误差模型的线性化得到。很多实践者直接用连续时间F矩阵离散化，用F·dt计算状态转移阵Φ。这里有一个精度问题：如果IMU输出频率较低（比如50Hz），而载体的动态较大，用简单的欧拉近似误差会比较大，更稳妥的做法是用矩阵指数或者对F矩阵做分段恒定假设下的精确离散化。

P矩阵的外推公式是：

P_k|k-1 = Φ·P_{k-1}·Φ^T + Q

Q矩阵是过程噪声矩阵，代表陀螺和加速度计的随机游走强度、零偏时间演化的不确定性。Q的取值直接决定了滤波器对“惯导误差到底有多大”的相信程度，也是最难调的参数之一。

4.3 伪距残差计算里的细节

残差计算是整个流程里最容易写错又最难排查的部分。我把需要注意的点列一下：

先看卫星位置的正确性。用广播星历算出来的卫星位置，一定要把卫星天线相位中心偏移考虑进去。广播星历给出的位置是卫星质心的位置，而伪距观测值对应的是卫星天线相位中心。两者之间的偏差可以达到1到2米，对于要求高精度的组合导航来说不可忽略。

再看相对论效应。GPS卫星处在高中轨道，时钟受引力势影响比地面快，这个效应已经被卫星钟出厂时的频率偏调修正了，但轨道偏心率引起的周期性相对论效应需要用偏心率和偏近点角做周期修正。漏掉这一项，伪距里会残留一个周期性误差，峰值可以达到20米左右。我在早期实现里漏掉过这个修正，回放数据时发现残差呈现正弦形状，花了很长时间才定位到是相对论周期项的问题。

再看接收机天线相位中心偏差。接收机观测值以天线相位中心为参考，而惯导解算的位置一般指IMU测量中心。两者之间的杆臂（lever arm）在位置组合里要补偿，在伪距组合里其实通过量测方程也能吸收掉一部分，但更严格的做法是显式建模杆臂，在把惯导位置转换到卫星几何距离时，加上杆臂在ECEF下的投影。

再看多路径。多路径是不太好建模的误差源。在静态环境里可以通过信噪比（C/N0）阈值筛选卫星，低信噪比卫星的伪距残差权重降低。在动态城市环境里，多路径往往表现为尖峰误差，需要靠残差检验来剔除。后面的章节会专门讲RAIM方法。

4.4 组合滤波器的初始化

滤波器初始化是新手最容易忽视的环节。如果初始状态误差协方差矩阵P设得不合理，滤波器要么收敛得很慢，要么第一轮更新就把状态带偏。

位置方差一般根据惯导初始对准精度设定。如果初始位置来自卫导单点定位，水平位置方差可以设成（5米）^2，高度方差因为卫导垂直精度较差，可以设成（10米）^2。速度和初始对准相关，水平速度方差设成（0.1米/秒）^2比较合理。姿态方差则要看初始对准的方式，如果用了卫导测速辅助的动基座对准，航向角初始不确定度可能还有0.5度到1度，需要反映在P矩阵里。

过程噪声Q的设定有个依赖关系：Q设得越大，滤波器越倾向于相信量测；但Q过大，会让滤波器对量测噪声过于敏感，输出噪声变大。Q设得太小，滤波器对惯导过度自信，一旦惯导误差增长超过预测，状态估计会滞后甚至发散。工程上标准做法是先离线跑一遍数据，调到一个稳定收敛的Q，再拿到实时环境去微调。

4.5 反馈重置与协方差匹配

误差状态卡尔曼滤波里，“校正”不是直接给位置加一个修正量就完事，还要处理协方差的匹配问题。

具体操作流程是：卡尔曼增益算出来后，先用残差乘以增益得到误差状态估计δx，然后把δx叠加到当前导航状态上，得到校正后的位置速度姿态。最后把误差状态估计值清零，同时将P矩阵做相应更新。

这里有个细节：P矩阵更新后，描述的是“当前误差状态的不确定性”。因为已经把δx从状态里剥离出来了，所以理论上更新后的P已经反映了残余不确定性。但如果实现的时候在反馈重置之后又额外对P做了一次处理，或者把重置顺序搞错，就会发现滤波器的协方差和真实误差明显失配。有一个快速验证方法：保存事后处理结果，统计真实位置误差与滤波器输出的1σ边界的匹配程度，如果误差经常超过3σ边界，说明协方差估计偏乐观，需要调大Q或者调小R。

5. 常见问题与排查技巧实录：伪距组合里的“坑”都在哪

5.1 滤波器发散，先查符号和坐标系

我见过太多组合导航的代码“一跑就炸”。通常第一反应是查代码，但有一个更高效的思路：先做符号和坐标系走查。

准备一组静态数据，让载体完全不动。如果滤波器正确，伪距残差应该在零附近震荡，误差状态的估计值应该趋近于零。如果残差呈现出持续增大或者明显的斜坡，多半是符号或坐标系转换的问题。

具体排查路径：第一，看IMU数据是否已经补偿了重力；第二，看机械编排输出的位置是否随时间漂移；第三，看ECEF转换的经纬高是否正确；第四，看卫星位置计算是否正确——可以用接收机广播的电离层参数逆向检查，或者用公开的精密星历对比；第五，看量测矩阵H的视线向量方向是否正确，可以先在“位置误差在某一方向+1米”的情况下，手工推演残差应该是什么符号，再看代码是否一致。

5.2 时间同步误差：看起来不大，杀伤力极强

伪距组合里，时间同步是一个经常被低估的问题。伪距观测值是接收机在某个历元时刻测得的，惯导位置是IMU在另一个时刻的积分结果。如果两者之间的时间差没有对齐，在载体运动时，伪距残差里会引入一个与速度成比例的误差。

举个例子：载体以20米/秒的速度运动，IMU和接收机时间差1毫秒，位置偏差就是2厘米，这个量级在伪距组合里还能忍。但如果时间差达到几十毫秒（接收机输出延迟、串口缓冲、系统调度抖动都可能造成），位置偏差就到几十厘米甚至米级了，这个量级足以让滤波器的精度退化。

解决思路有两条：一是硬件/系统层面做时间同步，比如用PPS脉冲对齐接收机观测时刻和IMU采样时刻；二是软件层面做补偿，在伪距残差计算时，把惯导位置推算到接收机观测时刻，或者在量测方程里加入时间同步偏差作为一个待估状态。

第二种做法在一些论文里被称作“时间偏差估计”，实现起来并不复杂，但要注意可观测性问题：如果载体长时间静止，时间偏差和位置误差之间存在耦合，可能无法同时估计，需要引入额外的约束。

5.3 粗差卫星的识别与剔除

城市环境下伪距粗差很常见，多路径、信号衍射、接收机通道异常都会造成某颗卫星的伪距突然偏差几十米甚至上百米。如果不做剔除，卡尔曼滤波会把这些粗差当成真实量测，导致位置估计被大幅拉偏。

常用方法是基于残差的卡方检验或者“新息一致性”检验。具体做法是：对该颗卫星计算其新息（innovation）v = z - H·δx_pred，并计算新息方差S = H·P·H^T + R。如果|v|超过某个阈值（比如3倍或4倍的标准差），就认为该卫星存在粗差，本周期跳过这颗星的量测更新。

这个方法的局限性在于：如果同一时刻多颗卫星同时出现粗差，或者粗差慢慢累积（比如多路径在载体转向时逐步变化），单星检验可能不够。更稳健的扩展是RAIM（Receiver Autonomous Integrity Monitoring），它对所有可见卫星的新息做一致性检测，寻找“组合后残差最小的卫星子集”。在组合导航里可以把每颗星逐一剔除后检查整体一致性，虽然计算量稍大，但能有效解决多粗差问题。

5.4 卫星几何不好，滤波器怎么表现

当可见卫星都集中在天空的某一小片区域时，伪距组合对垂直方向的约束很弱。最极端的情况是只有一颗卫星，那么滤波器只能约束“载体位置在卫星视线方向上的投影”，其他方向完全靠惯导预测。这种情况下，位置误差协方差在某些方向会持续增长，这是正常现象，不代表滤波器坏了。

但如果卫星几何分布不好，而且接收机钟差估计又不准，伪距残差里会同时混入“位置误差投影”和“钟差偏差”，这两个量在某些方向上不可区分。工程上通常用DOP（精度因子）来衡量这个问题的严重性，PDOP超过5的时候，即使滤波器不发散，也要对输出精度保持警惕。

实际操作中，可以在滤波器里监控新息序列的均值和标准差。如果卫星几何变差，新息的标准差可能仍然正常，但某些状态方向上的协方差会拉大。一个实用的调参经验是：在协方差的反馈中，对垂直方向和接收机钟差方向设置合理的过程噪声，不要为了追求窄边界而把Q压得过小，否则几何变差时滤波器会“硬撑”，输出结果表面平滑、实际偏差很大。

5.5 常见问题速查表

6. 实操总结与经验延伸

写到这里，第一篇的骨架算是立住了。回顾整个INS/伪距组合系统，底层的逻辑并不复杂，就是用一个误差状态滤波器，把惯导的高频短时精度和卫星伪距的低频长期稳度有机拼在一起。真正难的地方全在细节里：符号方向、时间同步、坐标系转换、误差补偿、粗差剔除、参数整定，任何一环做得不干净，最终精度都会被拖下来。

我个人在实际操作中的一个深刻体会是：伪距组合和位置组合的差距，在卫星信号良好的开阔环境下并不明显，但在信号遮挡、恢复、以及高动态的场景下会体现得非常清晰。伪距组合的“抗断”，本质上来自它没有把“卫导结果”当成一个整体来消费，而是把每一颗卫星的原始观测当成独立的约束来使用。这个思想认识到了，后续做深组合、做矢量跟踪、做多传感器融合，思路都是相通的。

还有一个小技巧想分享给刚开始实践的人：搭建完滤波器之后，不要急着上实测数据。先用仿真器生成一组“完美”的数据，比如直线运动、圆周运动、加噪声的伪距，跑通整个链路。确认算法正确之后，再用实测数据去迭代调参。这样可以极大缩短调试时间，因为仿真环境下你掌握着真值，任何一步出错都能立刻暴露，而实测数据里的误差源太多，出了问题往往不知从何下手。

下一篇可以顺着这条线往下走，讲伪距率（多普勒）如何加进来，以及和伪距组合形成更完整的“伪距/伪距率组合”框架。伪距率相当于把卫星和接收机之间的径向速度引入滤波，对速度精度的提升非常明显，也对接收机钟漂的可观测性有帮助。有了这一篇的基础，下一篇的动作会顺畅很多。