企业官网建设流程全解析

直接拿GNSS观测数据做最小二乘单点定位，画出来的轨迹跟醉汉走路差不多：直线路段东扭西歪，高程时不时跳个三五米。做过高精度定位的朋友应该对这一幕不陌生。这个MATLAB例程解决的正是这类问题——把GNSS原始观测和一条已知的预定轨迹放进同一个滤波框架，基于EKF在三维空间里做序贯融合，输出平滑、收敛、精度明显提升的位置序列。文章不只是讲代码怎么跑，更多是把滤波设计里的关键决策讲透：坐标系怎么选、观测方程怎么建、过程噪声和量测噪声怎么取值、轨迹约束怎么设计才不翻车。适合正在做GNSS定位课程设计、组合导航入门，或者在无人车、无人机项目里被定位噪声折磨的朋友参考。

1. 为什么“GNSS观测+预定轨迹+滤波”是三维定位的黄金组合

1.1 单点定位的三个顽疾：随机噪声、历元跳变、高程虚胖

GNSS单点定位的原理不复杂：利用至少四颗卫星的伪距观测，求解接收机在ECEF坐标系下的三维位置和接收机钟差。但这只是理论上的“能解”，实际跑出来的结果非常难看。

第一个问题是随机噪声。伪距观测的误差来源包括卫星钟差残差、星历误差、电离层延迟残差、对流层延迟残差、多径效应和接收机热噪声，每一项折合到距离上都是分米到米级。把这些误差叠加起来，单点定位的典型水平误差在2到10米之间，在树荫、楼宇附近还会更差。

第二个问题是历元跳变。每个历元独立解算，上一秒和下一秒之间没有任何连续性约束。卫星几何分布一变、某颗卫星失锁又恢复、多径信号突然增强，位置解就会瞬间跳出去几米甚至十几米，反应在轨迹图上就是一个个毛刺和断层。这种跳变对后续的航向、速度解算影响尤其大。

第三个问题是高程精度差，我习惯叫它“高程虚胖”。由于卫星几何约束在垂直方向天然偏弱，垂直DOP通常比水平DOP大1.5到2倍，单点定位的高程误差经常达到5到15米。对三维定位而言，高程不修正好，规划航线、地形跟随、碰撞检测全部受影响。

滤波的价值就在于把这三个问题一起处理：用运动模型约束历元间的连续性，压制随机噪声；用预定轨迹提供几何先验，补强弱观测方向；最后输出一条时间上一致、空间上贴着合理路径的三维轨迹。

1.2 预定轨迹在滤波器中的双重身份

预定轨迹在这套方案里不是可有可无的参考线，它实际上扮演了两个截然不同的角色。

第一个角色是运动模型的参考输入。载体沿着预定轨迹运动，位置变化本身就带有强烈的先验信息——下一时刻大概在哪里、速度大概是多少，这些都可以从轨迹序列里直接推算。把这种先验写进状态转移方程，滤波器就有了“惯性”，不会被单点观测的野值带着乱跑。

第二个角色是虚拟观测。这一点更关键：我可以把“当前位置应当离预定轨迹不太远”写成一条伪观测方程，直接参与量测更新。比如车辆在公路上行驶，理论上不会偏离道路中心线太多；无人机按航线飞行，每一秒的位置都该落在航线附近。这个约束用数学语言表达，就是给位置状态额外加了几行带权重的观测方程。

有了这双重身份，预定轨迹就不再是一条单纯的“展示背景线”，而是真正融入了状态估计的信息源。更妙的是，轨迹约束对高程方向特别有效——水平方向有GNSS四颗星撑着，垂直方向恰恰可以靠轨迹高程来拉一把。很多场景下，一个设计得当的轨迹虚拟观测能把三维精度从米级直接推到亚米级边缘。

1.3 为什么选滤波，而不是最小二乘或简单平滑

有人会问：既然GNSS每历元都能解，那对解算结果做滑动平均不就行了？不行。滑动平均只做了降噪，它完全不理解运动规律，也无法引入轨迹约束，更处理不了观测中断和跳变。它只是让曲线“看起来平滑”，但可能把真实的拐弯抹平，也会在跳变点上拉出一条虚假的斜坡。

最小二乘的问题则在于逐历元独立估计。它把所有历元看作互不相关的个体，没有将上一秒的位置、速度作为下一秒的先验信息。这套思路在静态测量里没有问题，但在动态定位里等于主动扔掉了最宝贵的信息——系统的动力学。

滤波，尤其是扩展卡尔曼滤波（EKF），天然是一个“预测+修正”的框架。预测环节塞入运动模型和预定轨迹，修正环节塞入GNSS伪距观测和轨迹虚拟观测，每个历元都在融合过去的信息、当前的测量和已知的路径约束。这正是动态高精度定位需要的东西。而且EKF的工程实现非常成熟，数值稳定性好，计算量在MATLAB里跑1Hz甚至10Hz数据毫无压力，适合作为工程基线。

2. 从坐标系到观测方程：滤波前必须打好的地基

2.1 ECEF、LLA、ENU：坐标框架选型与转换细节

很多初学者一上来就在经纬高（LLA）里写滤波器，这是个常见的坑。经纬高是非线性程度很高的坐标，经度在纬度不同时的实际弧长不一样，直接在里面做线性化，误差传播会非常别扭。

卫星星历给出的位置是ECEF（地心地固系），伪距观测方程也是基于ECEF的，所以解算起点放在ECEF没有错。但预定轨迹通常给的是经纬高或者当地平面坐标，人脑对“东、北、天”这三个方向的直觉判断也最直接。例程的做法是：在ECEF框架下计算卫星位置和几何距离，把状态估计转换到以起点为原点的ENU（东北天）局部坐标系里执行。

ENU坐标系是个直角坐标系，三个轴互相正交，数值尺度均衡，线性化误差小，轨迹约束写起来也非常自然——轨迹点减去参考点，直接得到东向、北向、天向三个分量。这个选择不是图省事，而是为了让滤波器的雅可比矩阵和观测方程都保持良好条件数。

LLA到ECEF的转换用的是经典公式。设参考椭球长半轴 a=6378137.0m，第一偏心率平方 e²=f(2-f)，其中 f=1/298.257223563：

% 经纬高转ECEF function ecef = lla2ecef(lat, lon, h) a = 6378137.0; f = 1 / 298.257223563; e2 = f * (2 - f); N = a / sqrt(1 - e2 * sin(lat)^2); ecef = [(N + h) * cos(lat) * cos(lon); (N + h) * cos(lat) * sin(lon); (N * (1 - e2) + h) * sin(lat)]; end

ECEF转ENU时，先计算与参考点的向量差，再左乘旋转矩阵：

% ECEF向量差转ENU function enu = ecef2enu(dr_ecef, lat0, lon0) slat = sin(lat0); clat = cos(lat0); slon = sin(lon0); clon = cos(lon0); R = [-slon, clon, 0; -slat*clon, -slat*slon, clat; clat*clon, clat*slon, slat]; enu = R * dr_ecef; end

参考点一般取预定轨迹的起点或轨迹中心点。参考点的影响只是平移和旋转，不影响滤波结果，但选在轨迹中心附近能让ENU坐标数值不会太大，数值稳定性更好。

2.2 伪距观测方程与H矩阵推导

GNSS的观测值有多种，C/A码伪距、载波相位、多普勒都可以用。这个例程选择伪距作为主观测，原因很简单：伪距没有整周模糊度问题，解算稳健，适用于单频单接收机场景。载波相位精度高一个到两个数量级，但需要解模糊度，属于RTK和PPP的范畴，那是另一套系统，不是这个例程想解决的问题。

第j颗卫星的伪距观测方程可以写成：

[ \rho_j = \sqrt{(x_j - x)^2 + (y_j - y)^2 + (z_j - z)^2} + c\delta t + \varepsilon_j ]

其中 ((x_j, y_j, z_j)) 是卫星位置，((x, y, z)) 是接收机位置，(c\delta t) 是接收机钟差（以米为单位），(\varepsilon_j) 是未建模误差。

注意方程里有个开根号，所以观测方程对状态是非线性的，必须用EKF。线性化之后，H矩阵的每一行其实是接收机到卫星的视线单位向量的负值叠加一个钟差偏导：

[ \frac{\partial \rho_j}{\partial \mathbf{r}} = -\frac{\mathbf{r}_j - \mathbf{r}}{|\mathbf{r}_j - \mathbf{r}|}, \quad \frac{\partial \rho_j}{\partial (c\delta t)} = 1 ]

从几何上看，位置增量在视线方向上的投影就是伪距变化量，这个结论非常直观——H矩阵的物理含义就是“接收机往某个方向移动一米，伪距增加多少”。理解这一点，后面写代码、查错都有帮助。

2.3 轨迹虚拟观测的构造与风险控制

有了GNSS观测方程，再来看预定轨迹如何写成观测方程。假设某个历元在预定轨迹上搜索到了距离当前估计位置最近的轨迹点 ( \mathbf{p}_{traj} = [X_t, Y_t, Z_t]^T)（ENU坐标），那么可以构造三行虚拟观测：

[ \mathbf{z}{traj} = \mathbf{p}{traj}, \quad \mathbf{h}{traj}(\mathbf{x}) = [x,\ y,\ z]^T, \quad \mathbf{H}{traj} = [\mathbf{I}{3\times3},\ \mathbf{0}{3\times5}] ]

意思很直白：滤波器认为当前位置应该与轨迹最近点重合，偏差大小由虚拟观测的噪声协方差 (\mathbf{R}{traj}) 控制。(\mathbf{R}{traj}) 如果给得小，轨迹约束就强，滤波结果会被死死吸在预定轨迹上；给得大，约束就弱，主要靠GNSS观测。

这里要特别提醒一个容易犯的错误：轨迹约束不是越强越好。如果载体实际行驶路线偏离了预定轨迹，比如临时绕行、变道、避障，强约束会把真实的位置强行拉到轨迹上，造成滤波结果比不约束还差。解决思路有两种：一种是让 (\mathbf{R}_{traj}) 随着新息自适应调整，GNSS正常时放松约束，GNSS异常时收紧约束；另一种是把虚拟观测从“位置完全重合”改成“横向偏差小”，即只约束载体离轨迹的垂直距离，不约束沿轨迹方向的进退。例程里保留了这两种模式的开关，默认用的是横向偏差约束，工程上更稳。

3. 滤波器设计与参数整定：没有玄学，只有依据

3.1 状态向量选取与运动模型

状态向量直接决定滤波器的表达能力和计算量。例程用的是8维状态：

[ \mathbf{x} = [x,\ y,\ z,\ v_x,\ v_y,\ v_z,\ c\delta t,\ c\delta t_d]^T ]

前六维是ENU下的位置和速度，后两维是接收机钟差和钟漂。有的实现会把速度去掉，只估位置和钟差，但动态场景里没有速度项，运动模型的预测能力会大打折扣，轨迹连续性也会变差，所以保留速度是值得的。

运动模型采用常速度（CV）模型。状态转移矩阵是分块形式的：

[ \mathbf{F} = \begin{bmatrix} \mathbf{I}{3\times3} & \Delta t \cdot \mathbf{I}{3\times3} & \mathbf{0} \ \mathbf{0} & \mathbf{I}{3\times3} & \mathbf{0} \ \mathbf{0} & \mathbf{0} & \mathbf{F}{clk} \end{bmatrix} ]

其中 (\mathbf{F}_{clk}) 描述钟差和钟漂的递推关系。CV模型假设速度在一个采样间隔内基本不变，这对车辆、无人机等中等动态载体是合理的。如果载体动态很强，比如无人机做急转弯、飞手暴力打杆，可以考虑常加速度（CA）模型，但代价是状态维数增加，过程噪声更难调。例程默认CV模型，并预留了切换到CA模型的接口，读者可以自己试验。

3.2 过程噪声与量测噪声的取值逻辑

滤波器的“手感”好坏，九成取决于噪声参数的设置。过程噪声 (\mathbf{Q}) 描述运动模型的可信度，量测噪声 (\mathbf{R}) 描述观测的可信度，二者之比决定了滤波器在预测值和观测值之间更信谁。

(\mathbf{Q}) 是从加速度扰动密度 (q) 推导出来的。(q) 的物理含义是“载体加速度扰动的时间变化率”，或者更通俗地说，你对运动模型没信心的程度。低动态场景（车辆沿道路匀速行驶）取 (q = 0.1\sim0.5 \ \mathrm{m/s^2})，中等动态（无人机巡航）取 (1\sim2)，高动态（急转弯、急加减速）取 (3\sim5)。连续时间白噪声离散化后，位置和速度的协方差块可以按标准公式计算：

[ \mathbf{Q}{pos} = q^2 \begin{bmatrix} \frac{\Delta t^3}{3} & \frac{\Delta t^2}{2} \ \frac{\Delta t^2}{2} & \Delta t \end{bmatrix} \otimes \mathbf{I}{3\times3} ]

量测噪声 (\mathbf{R}) 按卫星取对角线矩阵。开阔环境下单颗星的伪距观测噪声大约 (1\sim3) 米，城市峡谷、树荫等场景放大到 (5\sim10) 米。注意这里的 (\mathbf{R}) 要乘以星数，所以它是一个 (n \times n) 的对角阵，(n) 是当前可见卫星数。轨迹虚拟观测的 (\mathbf{R}_{traj}) 也要参与总观测协方差矩阵的组装，我把它放在量测方程的最后几行。

调参有个先后顺序，我踩过坑之后总结的经验是：先把 (\mathbf{Q}) 按运动动态给一个合理初值，然后把 (\mathbf{R}) 按实际环境调到合适量级，最后再把轨迹约束从弱到强逐渐加码。顺序反了的话，轨迹约束会掩盖 (\mathbf{Q}/\mathbf{R}) 不匹配的问题，滤波结果看起来挺好，换一段没有轨迹约束的数据就立刻原形毕露。

下面这个表是我在例程里用的典型参数组合，给读者做个参考：

3.3 滑动窗口预滤波：先让观测干净一点

EKF本身有滤波能力，但如果有明显的野值、多径尖峰直接进入量测更新，会对结果造成不必要的冲击。例程在数据进入EKF之前加了一道滑动窗口预滤波。

我用的方法是滑动窗口取中值。设置窗口长度 (L=5) 或 (L=7)，对每个卫星的伪距序列在时间维度上滑动取中值。中值对脉冲型干扰的抗性比均值好很多：多径信号导致的伪距突然增大或减小，在中值窗口里会被周围的值“稀释”掉。窗口长度不要太长，太长会导致观测信息滞后，动态场景下反而引入延迟误差。

还有一个细节：滑窗预滤波的窗口中心对应的是历史时刻，直接使用会带来时间对齐误差。例程的做法是先把窗口中心的伪距估计值补偿到当前历元，补偿量用当前速度在窗口时间跨度上的投影来计算。这个操作在低速场景下影响不大，但高速飞行时能明显减少滤波延迟。

3.4 自适应机制：观测异常时的轨迹约束接管

GNSS信号不是一直可靠的。卫星失锁、遮挡、多径突变都会导致伪距观测异常。如果没有应对机制，EKF会把这些异常当作真实测量，导致位置解瞬间跳飞。例程里做了一层新息卡方检验，让滤波器对观测异常有“感知能力”。

新息就是观测值与预测值之差 (\mathbf{v} = \mathbf{z} - \mathbf{h}(\mathbf{x}{pre}))。在正常工作时，新息的协方差矩阵是 (\mathbf{S} = \mathbf{H}\mathbf{P}{pre}\mathbf{H}^T + \mathbf{R})，所以统计量 (\gamma = \mathbf{v}^T \mathbf{S}^{-1} \mathbf{v}) 近似服从卡方分布，自由度等于观测维数。如果某颗卫星的 (\gamma) 超过阈值，就认为该观测异常，直接把对应的 (\mathbf{R}) 项乘一个大系数（比如100倍），让它在量测更新里几乎不起作用。

这套机制与轨迹约束配合起来非常有效：当GNSS观测大面积异常时，所有伪距观测都被“降权”，系统只剩运动模型和轨迹虚拟观测在工作，定位退化为一条沿预定轨迹的推算。等卫星恢复、新息统计量回落到正常水平，滤波器自动把观测权重加回来。我在无人车项目里实测，这种自适应机制能把失锁期间的最大位置误差控制住，恢复后也能在几秒内重新收敛。

4. MATLAB代码实现：主循环、平滑与可视化全流程

4.1 例程结构、数据文件与运行环境

代码包解压后的目录结构是这样的：

GNSS_TrajFilter/ ├── run_demo.m % 主入口脚本 ├── data/ │ ├── gnss_pseudo_range.mat % 伪距观测数据（已预处理） │ └── reference_traj.mat % 预定轨迹（ENU坐标） ├── funcs/ │ ├── lla2ecef.m │ ├── ecef2enu.m │ ├── ekf_predict.m │ ├── ekf_update.m │ └── rts_smooth.m └── results/ └── *.fig / *.png % 结果图

运行环境方面，MATLAB R2019b及以上版本都能直接跑，不需要额外工具箱。例程里的伪距观测是从一次开阔环境车测数据中解析出来的，采样率1Hz，持续时间约20分钟，可见卫星数在6到10颗之间浮动。预定轨迹是车辆实际行驶的参考路径，由RTK设备事先采集，精度在厘米级，用来验证滤波器的轨迹约束效果。

4.2 EKF主循环的关键MATLAB代码

主循环的逻辑非常清晰：逐历元读取观测数据，先做时间更新，再组装量测更新，最后把状态和协方差保存下来。下面是核心代码片段：

for k = 2:N dt = t(k) - t(k-1); % 时间更新（预测） [x_pre, P_pre] = ekf_predict(x_est, P_est, dt, Q); % 组装当前历元的观测向量、预测观测量、H矩阵和R矩阵 [z, h, H, R] = assemble_measurement(x_pre, ... gps_data{k}, traj_nearest(k), R_rho, R_traj); % 量测更新（修正） [x_est, P_est] = ekf_update(x_pre, P_pre, z, h, H, R); % 保存结果 x_history(:, k) = x_est; P_history(:, k) = diag(P_est); end

assemble_measurement函数是整段代码的精华。它把两类观测拼成一个大的观测方程组：前面若干行是可见卫星的伪距观测，每个卫星对应一行；后面三行是预定轨迹的虚拟观测。H矩阵对应位置状态的列填上视线单位向量或单位阵，这样一次量测更新就把所有信息都融合进去了。

要注意的是，伪距观测方程在ENU坐标下依然是非线性的，所以ekf_update里需要在预测状态处重新计算H矩阵，而不是用上一历元的H。这一点初学者很容易忽略，如果H矩阵固定不变，滤波器会慢慢发霉，新息里会积累越来越多的模型失配误差。

4.3 RTS平滑：反向再滤一遍，轨迹更丝滑

EKF是因果滤波器，只利用了当前及过去的信息。如果要离线处理数据，还有个威力巨大的后处理工具——RTS（Rauch-Tung-Striebel）固定区间平滑。

RTS平滑的过程分两步：先用EKF正向递推一遍，记录每一历元的滤波状态 (x_k)、协方差 (P_k)、预测状态 (x_{k+1|k}) 和预测协方差 (P_{k+1|k})；然后从最后一历元开始反向递推：

[ \mathbf{K}_k = \mathbf{P}k \mathbf{F}^T \mathbf{P}{k+1|k}^{-1} ]

[ \mathbf{x}_k^s = \mathbf{x}k + \mathbf{K}k (\mathbf{x}{k+1}^s - \mathbf{x}{k+1|k}) ]

[ \mathbf{P}_k^s = \mathbf{P}_k - \mathbf{K}k \mathbf{P}{k+1|k} \mathbf{K}_k^T ]

反向平滑的物理意义很直观：不仅用过去的信息修正当前状态，还能“开未来视野”重新审视每个历史时刻。因此平滑后的轨迹通常比实时滤波结果更干净，尤其在观测环境突然变差的区域，平滑结果能看到明显的“事后修正”效果。代价是计算量和存储量增加，但对于1Hz、几十分钟的数据，MATLAB跑起来也就是几秒钟的事。

代码包里rts_smooth.m已经实现好，直接在正向滤波完成后调用即可。跑完对比一下正向滤波和平滑结果，你会发现后者在拐弯处的轨迹更贴近预定路径，高程毛刺也更少。

4.4 三维可视化与误差统计

只看数值不如看图直观。例程的run_demo.m会画三张图：三维轨迹对比图、东/北/天分量误差曲线、水平误差累积分布（CDF）曲线。

三维轨迹图把GNSS裸解、预定轨迹和滤波输出画在同一个坐标系里，用不同颜色区分。叠加之后能很快看出滤波输出的轨迹是否“贴合”预定轨迹，以及在GNSS跳变点时Kalman滤波是如何稳定过渡的。

误差统计我习惯用RMS来量化。例程会分别计算东、北、天三个方向以及三维总体的RMS误差，还会输出滤波前后RMS的改善百分比。这部分代码不复杂，但它的存在让调参有了量化依据——每次改完参数重跑，看RMS是降了还是升了，比用肉眼盯着轨迹图猜靠谱得多。我会在下一节给出一组典型的实测前后对比数据。

5. 实测效果分析与排坑记录

5.1 精度提升实测：滤波前后的RMS与CDF对比

用例程自带数据跑完一组典型参数后，精度对比非常明显。GNSS单点定位（裸解）的水平RMS约3.2米，高程RMS约6米，三维RMS约6.8米；经过EKF加轨迹约束之后，水平RMS降到0.8米，高程RMS降到1.2米，三维RMS降到1.4米，整体改善幅度超过75%。

从CDF曲线看，裸解的95%水平误差大概在7米上下，滤波后的95%水平误差在2米以内。这个提升不是来自某个神奇算法，而是来自三件事的叠加：运动模型压制了历元间随机噪声，轨迹虚拟观测补强了弱几何方向，滑动窗口预滤波剔除了多径尖峰对量测更新的污染。

值得注意的是，在GNSS观测几何比较差的时段（可见卫星少于5颗），裸解误差会突然膨胀，但滤波轨迹依然稳定。这正是加滤波器最大的价值——它不保证每一刻都是“最优解”，但保证了系统整体的连续性和鲁棒性。

5.2 踩坑一：初始协方差P0的手感问题

我第一次跑这个例程时，初始协方差 (\mathbf{P}_0) 直接给了全零矩阵，结果前100个历元的滤波轨迹几乎贴着预测值走，完全不理睬观测数据，收敛过程非常慢。原因很直白：P0表示对初值的不确定度，全零意味着“位置一开始就被锁死了”，滤波器后续只肯相信运动模型，不敢接受观测修正。

反之，P0给得过大也不行，比如位置协方差直接给10^6，前几十个历元的状态就会被观测噪声带着疯狂跳动，和裸解没什么区别。

经验做法是：位置初始协方差按GNSS单点定位的精度量级取，比如25到100平方米；速度初始协方差按载体最大速度的十分之一平方取；钟差和钟漂则要放大到300米和10米每秒量级，因为钟差初始值错得离谱是常态。这样设好之后，滤波器通常能在几十个历元内完成收敛，后续不会因为初值问题产生系统性偏差。

5.3 踩坑二：卫星失锁与新息卡方检验

例程数据里有一段可见卫星从8颗骤降到3颗的时段，如果不做任何处理，滤波结果会在失锁瞬间出现一个明显的“坠落式”偏移。我一开始的解决办法是简单删掉不可见的卫星观测，但问题没有完全解决——剩余卫星的几何分布很差，单个伪距的小误差也会被放大成大位置误差。

后来用上第3.4节提到的新息卡方检验，情况才真正改善。逐颗卫星计算新息统计量，一旦某颗卫星的新息超阈值，就把它从量测更新中摘除。这样在失锁和恢复的过渡期，滤波器不会强行使用质量差的观测数据，位置估计更平稳。实现上要注意阈值不能设得太小，否则正常观测会被误剔除；自由度取1（单颗卫星），显著性水平0.01对应的卡方阈值大约是6.63，我用的是这个经验值。

5.4 踩坑三：轨迹匹配在转弯段的误投影

预定轨迹是一条由离散点组成的线，每个历元都要搜索“最近的轨迹点”。在城市道路的十字路口、连续转弯路段，这个搜索很容易出问题：当前位置可能距离两条路的点都差不多近，搜索算法一旦选错方向，虚拟观测就会把滤波结果往错误的路段上拉。

解决方法是给轨迹点加一个“连续序号”约束。车辆或无人机沿轨迹前进，轨迹点序号只增不减，所以搜索最近点时加上一个合理的前向搜索窗口，比如只在上一历元匹配点索引附近的±50个点范围内搜索。这个约束看起来简单，但极大减少了转弯段的误匹配概率，也让轨迹约束的方向始终保持一致。

如果载体可能真的偏离预定轨迹很远（比如临时绕行），那还要在搜索失败时主动放弃轨迹约束，而不是强行找一个距离很远的轨迹点来凑观测。例程里这部分逻辑用了一个标志位use_traj_constraint来控制，在数据异常时自动置0。

5.5 代码下载与使用说明及调参建议

代码包已经打包好，放在下载链接里，解压后打开run_demo.m直接运行就能复现上面的结果。使用前建议先通读一遍assemble_measurement.m，这是整个滤波器的核心，理解了它你才算真正掌握了这套系统。

调参时我个人的建议是把精力优先放在三个地方：第一个是运动模型与实际动态的匹配度，载体动态强但Q给小了，滤波轨迹会明显滞后；第二个是伪距噪声R是否反映了真实环境的遮挡程度，城市峡谷里R给小了会导致结果抖动；第三个是轨迹约束的强度，要记住目标不是让轨迹完全贴合预定轨迹，而是让它在合理范围内平滑地靠近。

再分享一个小技巧：调试时先把轨迹约束关闭，只调EKF的参数，等裸滤波结果已经比较稳定之后，再把轨迹约束逐步加上去。这样每一步的效果都能清楚地看到，出了问题也知道该往哪个方向排查。定位滤波不是端到端的黑盒，它更像是一个由量化指标指挥的调参过程，每一步都得能解释、可验证。

这个MATLAB例程演示的框架也可以继续向两个方向扩展：一个是在状态里加入惯性传感器（IMU）的量测，形成GNSS/INS组合导航，位置在GNSS完全失锁时依然能靠惯导维持一段时间的精度；另一个是把伪距观测换成载波相位，走RTK或PPP路线，定位精度直接进入厘米级。核心的EKF结构不变，变的只是观测方程的维度和复杂度，这套骨架完全能撑起后续的进阶开发。