模型与观测之间的权衡取舍：城市峡谷环境下的 KF 位置滤波器调优
在 GNSS 位置滤波中使用 Kalman Filter 时，滤波过程可以分为两步：先用动态模型把上一历元的状态外推到当前历元，再用当前 GNSS 观测对预测状态进行修正。
动态模型可靠时，滤波器可以在 GNSS 观测短时退化时维持轨迹连续性；观测可靠时，滤波器可以及时校正模型外推产生的漂移。问题在于，城市峡谷环境下模型预测和观测更新都并非始终可靠。车辆可能停车、转弯、加速，导致状态转移模型失配；GNSS 观测又会受到遮挡、多路径和 NLOS 影响，导致观测误差统计特性偏离随机模型。
因此，城市峡谷里的 KF 调优不应只追求轨迹平滑，而应在每个历元重新评估模型预测与观测更新的相对权重。
1. 从预测到修正：KF 的基本工作流
以常见的位置速度状态为例，可以把状态写成：
\[x_k = \begin{bmatrix} p_k \\ v_k \end{bmatrix}\]其中 $p_k$ 是位置，$v_k$ 是速度。若采用 CV（Constant Velocity，匀速）模型，在采样间隔为 $\Delta t$ 时，状态转移可以写成：
\[\begin{bmatrix} p_k \\ v_k \end{bmatrix} = \begin{bmatrix} I & \Delta t I \\ 0 & I \end{bmatrix} \begin{bmatrix} p_{k-1} \\ v_{k-1} \end{bmatrix} + w_k\]这里的 $w_k$ 是过程噪声，用于表征真实运动相对匀速模型的偏离。对应到 KF 的 Time Update：
\[x_k^- = F x_{k-1}^+\] \[P_k^- = F P_{k-1}^+ F^T + Q\]这一步只依赖上一历元状态、状态转移模型和过程噪声。它给出一个预测状态 $x_k^-$，以及预测协方差 $P_k^-$。
随后进入 Measurement Update。观测模型可以抽象写成：
\[z_k = H x_k + e_k,\quad e_k \sim N(0, R)\]滤波器计算实际观测 $z_k$ 与预测观测 $H x_k^-$ 的差值，得到新息：
\[\nu_k = z_k - H x_k^-\]再通过 Kalman 增益将新息映射为状态修正量：
\[K_k = P_k^- H^T (H P_k^- H^T + R)^{-1}\] \[x_k^+ = x_k^- + K_k \nu_k\]这里最重要的两个调参入口，是 Time Update 里的 $Q$ 和 Measurement Update 里的 $R$。$Q$ 描述状态转移模型误差，间接决定预测协方差的增长速度；$R$ 描述观测误差，直接影响观测在更新中的权重。$Q$ 增大时，预测不确定性增长更快，观测更新更容易修正状态；$R$ 增大时，新息协方差增大，观测对状态更新的影响减弱。
2. 城市峡谷中的主要误差源：遮挡、多路径与 NLOS
城市峡谷对 GNSS 定位的影响，主要不只是观测误差方差增大，而是误差结构发生变化。
遮挡首先会改变卫星几何。高楼会挡住一部分天空，可用卫星数量减少，观测分布变得不均匀。在街道峡谷中，接收机常常只能看到沿街道方向的一条天空带，横向几何约束较弱。此时即使单条观测的随机噪声不大，位置解也可能在弱约束方向上明显发散。
多路径和 NLOS 进一步改变观测误差特性。多路径是直达信号与反射信号叠加，导致码伪距、载波相位或多普勒观测产生偏差，其中码伪距通常受影响更明显；NLOS 则是直达信号被遮挡，接收机跟踪到的主要是反射路径。对伪距而言，这类误差往往不是零均值高斯噪声，而是带方向、带偏差、还会随环境突然变化的系统误差。
这会削弱 KF 随机模型的适用性。Time Update 假设运动模型误差可以由 $Q$ 描述，Measurement Update 假设观测误差可以由 $R$ 描述。但城市峡谷中，运动模型误差可能因机动而突增，观测误差的统计特性也会随遮挡、反射和重捕获发生突变。调优的目标，是让 $Q$ 和 $R$ 覆盖这些变化，同时避免协方差过度膨胀导致有效约束不足。
3. Time Update 的调优：状态转移模型、动力学约束与 Q
Time Update 的核心问题是：状态转移模型与当前载体运动是否匹配？
CV 模型适合匀速或近似匀速运动。车辆在直路上以稳定速度行驶时，CV 模型能够给出合理的短时预测；行人沿路稳定移动时，也能得到连续的位置外推。但 CV 模型不包含制动、掉头、转弯或突然静止等机动行为，这些未建模动态最终都要由 $Q$ 吸收。
如果 $Q$ 设得过小，滤波器会过度约束状态转移模型。轨迹看起来更平滑，但这种平滑往往伴随动态响应滞后。车辆已经转弯，滤波器仍沿原速度方向预测；车辆已经停车，位置状态仍可能继续外推；从遮挡区回到开阔区后，可靠观测也需要较长时间才能校正状态。
如果 $Q$ 设得过大，预测协方差增长过快。这样可以更快响应转弯和停车，但也会让观测噪声更容易进入状态。城市峡谷下如果观测本身受到多路径或 NLOS 污染，过大的 $Q$ 会使状态估计更容易被异常观测或偏差观测带偏。
因此，$Q$ 的设计不应该只是一个固定常数，而应该和运动状态相关。常见做法包括：
- 速度越高，位置过程噪声适当增大，因为同样的模型误差会带来更大的位置偏差。
- 航向变化越剧烈，速度方向相关的过程噪声应增大，避免 CV 模型强行延续旧方向。
- 检测到静止时，可以降低速度过程噪声，并引入零速约束或弱位置保持约束。
- IMU、轮速、航向角可用时，可以用它们增强状态转移，而不是只依赖纯 CV 模型。
- 当 GNSS 质量变差、观测更新不可靠时，不宜单纯减小 $Q$，否则未建模运动误差可能长期保留在状态中。
动力学约束的作用，是让 Time Update 不只是数学外推，而是贴近真实载体运动。车载场景可以利用非完整约束、最大加速度、最大转向率、道路方向等信息；手持场景可以利用步态、静止检测和速度范围。约束越接近真实运动，$Q$ 就越容易调；约束越粗糙，$Q$ 就需要留出更多余量。
4. Measurement Update 的调优：观测模型、FDE 与 R
Measurement Update 的核心问题是：当前观测误差模型是否可信？
在开阔天空下，观测误差常常可以近似看作随仰角、C/N0、频点和接收机噪声变化的随机误差。此时 $R$ 的设计相对直接：通常对高仰角观测赋予较大权重，对低仰角观测赋予较小权重；对强信号观测赋予较大权重，对弱信号观测赋予较小权重。
城市峡谷中，仅靠这种随机模型不够。遮挡、多路径和 NLOS 带来的不只是观测误差方差增大，还可能使观测偏离原本的物理模型。此时 Measurement Update 需要两层机制：先在观测质量控制中识别明显异常，再用 $R$ 表达保留观测的不确定性。
FDE，也就是 Fault Detection and Exclusion，严格来说偏向故障检测与排除。工程实现中通常会把它放在更广义的观测质量控制框架里：明显异常观测用于检测和排除，疑似异常观测可进入降权、隔离、延迟启用或重初始化流程。常用判据包括：
- 单颗卫星伪距残差或新息是否异常。
- 某颗卫星异常是否连续多个历元出现。
- 某个星座、频点或仰角范围是否整体残差偏大。
- 码相位、多普勒和载波相位之间是否一致。
- 加入或剔除某颗卫星后，整体残差 RMS、NIS 或位置跳变是否明显改善。
- 是否有地图、3D 建筑或天际线信息支持 NLOS 判断。
$R$ 的设计则要把观测质量转化为权重。相比简单的仰角模型，城市峡谷下更适合采用多因子观测定权模型。若暂不考虑观测间相关性，可写成：
\[R = \operatorname{diag}(\sigma_1^2,\sigma_2^2,\dots,\sigma_m^2)\]其中 $\sigma_i^2$ 可以由基础码噪声、仰角项、C/N0 项、多路径风险项、NLOS 风险项和历史残差项共同组成。质量越差，$\sigma_i^2$ 越大，观测权重越低。若考虑同源误差、星间相关或公共改正误差，$R$ 还应包含相应的非对角项。
FDE 和 $R$ 不是二选一。$R$ 适合处理“可疑但仍有信息”的观测，隔离适合处理“观测模型已经明显不成立”的观测。低仰角但残差正常，可以降权保留；若载波相位参与滤波，周跳未修复的观测应隔离或重初始化；时间标记异常、强 NLOS 证据明确的观测，也应从当前更新中排除。
城市峡谷中需要避免两类极端策略：一种是过度依赖剔除，把观测剔到几何不可用；另一种是只靠增大 $R$，让明显错误的观测继续污染状态。较稳妥的做法是先降权，再观察连续一致性，证据充分后再隔离。
5. 稳定性怎么看：P 与残差 r
滤波器调完以后，不能只看轨迹是否平滑。平滑可能来自正确建模，也可能只是 $Q$ 太小导致滤波器反应迟钝。稳定性至少要同时看状态不确定性和观测残差。
第一个指标是 $P$，也就是状态协方差。它反映滤波器对状态估计不确定性的内部评估。位置方差突然变大，通常说明几何变差、观测减少或模型不确定性增加。
但 $P$ 只能反映协方差一致性假设下的内部不确定性，不能单独证明估计误差较小。如果 $Q$ 和 $R$ 设得过于乐观，$P$ 可能偏小，位置却已经受到 NLOS 偏差影响。城市峡谷中尤其需要警惕这种“不确定性偏小、实际误差偏大”的不一致状态。
第二个指标是残差。为避免和观测噪声矩阵 $R$ 混淆，这里把单观测残差写成小写 $r$：
\[r_k = z_k - H x_k^+\]也可以看更新前的新息 $\nu_k$。残差反映观测和当前状态是否一致。如果 $P$ 偏小但残差持续偏大，说明协方差评估可能过于乐观；如果 $P$ 偏大但残差不大，说明噪声模型可能过于保守，状态收敛效率不高。
更系统的指标是归一化新息平方 NIS：
\[S_k = H P_k^- H^T + R\] \[\text{NIS}_k = \nu_k^T S_k^{-1}\nu_k\]NIS 把新息、预测协方差和观测噪声放在一起看。若 NIS 长期偏大，通常说明新息相对协方差过大，可能来自 $Q$ 或 $R$ 低估、观测异常未剔除，或状态转移/观测模型失配。若 NIS 长期偏小，则可能是噪声设得太保守，滤波器没有充分利用观测信息。
实际调试时，稳定性应按一组联合指标判断：$P$ 是否合理，残差 $r$ 是否收敛，NIS 是否在可接受范围，位置是否存在明显滞后或跳变。只看其中一个指标，容易误判滤波器状态。
6. 高架下的典型问题：未建模动态会放大调参矛盾
高架下是典型的复杂场景。它既有遮挡，又有反射，还经常伴随车辆行为变化。比如车辆在高架下排队停车、低速蠕行、突然转向进入匝道，或者从高架遮挡区驶出到开阔区域。
如果滤波器仍然使用简单 CV 模型，这些行为都会表现为未建模动态。停车时，CV 模型会继续预测车辆向前运动；转向时，CV 模型会继续沿旧航向外推；低速蠕行时，速度状态可能被噪声和异常观测反复扰动。
与此同时，高架下的 GNSS 观测质量也不稳定。上方结构会遮挡卫星，桥体和周围建筑会产生反射，部分卫星可能在短时间内反复丢失和重捕获。此时 Measurement Update 给出的观测修正不一定可靠。
这会形成以下调参权衡：
- 增大 $Q$，可以更快适应停车和转向，但更容易被多路径观测带偏。
- 减小 $Q$，可以抑制异常观测影响，但会在停车和转向时产生明显滞后。
- 增大 $R$，可以降低异常观测影响，但可能导致 GNSS 恢复后状态校正过慢。
- 减小 $R$，可以快速利用观测，但在 NLOS 场景下容易跳点。
解决这类问题，不宜只依赖一组固定的 $Q$ 和 $R$。更合适的做法是引入场景触发逻辑：检测到低速或静止时启用零速约束；检测到航向突变时增大横向或速度方向过程噪声；检测到遮挡增强或残差集体变大时提高观测噪声；检测到观测连续恢复一致后，再逐步降低 $R$，让 GNSS 观测重新参与状态校正。
高架下的问题通常不在 KF 公式本身，而在于状态转移模型缺少真实运动信息，观测模型缺少环境风险信息。调优应同时补偿运动模型误差和观测模型误差。
7. 小结：调的是权衡，不是单个参数
城市峡谷环境下，KF 位置滤波器的调优思路可以归纳为：
- Time Update 负责状态外推。这里要调状态转移模型、动力学约束和 $Q$。
- Measurement Update 负责观测校正。这里要调观测模型、观测质量控制和 $R$。
- 稳定性不能只看轨迹平滑，而要同时看 $P$、残差 $r$、NIS 和实际运动响应。
- 高架、街口、匝道、停车等场景会暴露未建模动态，需要动态调整 $Q$ 和约束，而不是依赖固定 CV 模型覆盖所有运动状态。
归根到底，$Q$ 和 $R$ 不是两个孤立旋钮。$Q$ 描述动态模型误差，$R$ 描述观测模型误差。城市峡谷调优的关键，是让两者随环境和运动状态变化：观测退化时降低观测更新权重，载体机动时放宽模型预测约束，在每个历元形成可解释的预测-更新权衡。