深入浅出 GNSS 之抗差 Kalman 滤波：原理及公式推导

sucy 20260716

适用对象：测量平差、Kalman 滤波和 GNSS 定位算法初学者
目标：理解“抗差”是什么意思、它与标准 Kalman 滤波的区别、矩阵公式如何推导，以及它适合解决什么问题

1. 什么是抗差 Kalman 滤波

抗差 Kalman 滤波（Robust Kalman Filter，RKF）不是某一个唯一固定的公式，而是一类方法。

常见路线包括：

• 基于 Huber、IGGIII 等 M 估计的等效权方法；
• 基于 Student’s $t$ 等重尾分布的方法；
• 新息限幅或饱和方法；
• 最大相关熵等非二次准则；
• 同时处理观测异常与模型异常的抗差自适应方法。

本文重点讲解测量平差和 GNSS 中最常见的 M 估计—等效权抗差 Kalman 滤波。

它的核心思想是：

在 Kalman 滤波的预测—更新框架中，识别异常信息，并降低异常信息对状态估计的影响。

本文重点讨论观测抗差：根据观测残差动态调整观测权值或观测协方差。针对过程模型异常的抗差方法，需要另外处理状态预测或过程噪声。

标准线性 Kalman 滤波通常要求：

• 状态方程和观测方程基本正确；
• 过程噪声与观测噪声为零均值，且与初始状态相互独立；
• 协方差矩阵 ${\mathbf{Q}}_k$ 和 ${\mathbf{R}}_k$ 设置合理；
• 数据中不存在未建模的严重粗差。

需要注意：

Kalman 滤波作为线性最小均方误差估计，并不严格要求噪声必须服从高斯分布；但在线性高斯条件下，Kalman 结果才同时具有完整贝叶斯后验均值、最小均方误差估计和最大后验估计等更强的最优性解释。

实际 GNSS 数据经常不满足这些条件，例如：

• 伪距多路径和非视距 NLOS；
• 载波相位周跳；
• 卫星钟差或改正数异常；
• 接收机短时失锁；
• 观测值突跳；
• 低高度角异常；
• 动态模型与真实运动不一致。

抗差 Kalman 滤波的作用，就是避免少量异常观测把整个滤波状态“带偏”。

2. “抗差”两个字怎样理解

这里的“差”主要不是正常的小幅随机误差，而是：

粗差
离群值
异常观测
重尾噪声中的极端样本

模型失配也会产生大残差，但它不等于观测粗差。抗差滤波若不能区分二者，可能把真实运动变化误判为异常观测。

“抗差”表示：

估计结果对少量异常数据不敏感，即使出现粗差，状态估计仍尽量保持稳定。

例如某时刻有 8 颗卫星，其中 7 颗伪距正常，1 颗受严重多路径影响，误差达到几十米。

标准 Kalman 滤波可能仍按照原来的观测方差使用这颗卫星，从而导致：

• 位置跳变；
• 接收机钟差异常；
• 对流层参数被污染；
• 后续多个历元持续受影响。

抗差 Kalman 滤波会根据残差判断这颗卫星不可信，并采取：

增大该观测方差
或
降低该观测权值
或
直接剔除该观测

3. 数学记号与 LaTeX 约定

本文统一使用以下记号：

• $\mathbf{a}$：向量；
• $\mathbf{A}$：矩阵；
• ${\mathbf{A}}^{\mathsf{T}}$：矩阵 $\mathbf{A}$ 的转置；
• ${\mathbf{A}}^{-1}$：矩阵 $\mathbf{A}$ 的逆；
• $\mathrm{diag}(\cdot )$：对角矩阵；
• $\Vert \mathbf{a}\Vert$：向量 $\mathbf{a}$ 的范数；
• $\mathcal{N}(\mathbf{\mu },\mathbf{\Sigma })$：均值为 $\mathbf{\mu }$、协方差为 $\mathbf{\Sigma }$ 的高斯分布；
• 上标“$-$”：先验值或预测值；
• 上标“$+$”：后验值或更新值。

显示公式统一使用：

$$
公式
$$

行内公式统一使用：

$公式$

4. 标准 Kalman 滤波模型

4.1 状态方程

$${\mathbf{x}}_k={\mathbf{F}}_{k-1}{\mathbf{x}}_{k-1}+{\mathbf{w}}_{k-1}$$

其中：

• ${\mathbf{x}}_k$：时刻 $k$ 的状态向量；
• ${\mathbf{F}}_{k-1}$：状态转移矩阵；
• ${\mathbf{w}}_{k-1}$：过程噪声；
• ${\mathbf{Q}}_{k-1}$：过程噪声协方差矩阵，满足

$${\mathbf{w}}_{k-1}\sim \mathcal{N}\left(0,{\mathbf{Q}}_{k-1}\right)$$

4.2 观测方程

$${\mathbf{z}}_k={\mathbf{H}}_k{\mathbf{x}}_k+{\mathbf{v}}_k$$

其中：

• ${\mathbf{z}}_k$：观测向量；
• ${\mathbf{H}}_k$：观测设计矩阵；
• ${\mathbf{v}}_k$：观测噪声；
• ${\mathbf{R}}_k$：观测噪声协方差矩阵，满足

$${\mathbf{v}}_k\sim \mathcal{N}\left(0,{\mathbf{R}}_k\right)$$

5. 标准 Kalman 滤波的预测与更新

5.1 状态预测

$${\hat{\mathbf{x}}}_k^{-}={\mathbf{F}}_{k-1}{\hat{\mathbf{x}}}_{k-1}^{+}$$

其中：

• ${\hat{\mathbf{x}}}_k^{-}$：时刻 $k$ 的先验状态估计；
• ${\hat{\mathbf{x}}}_{k-1}^{+}$：时刻 $k-1$ 的后验状态估计。

5.2 协方差预测

$${\mathbf{P}}_k^{-}={\mathbf{F}}_{k-1}{\mathbf{P}}_{k-1}^{+}{\mathbf{F}}_{k-1}^{\mathsf{T}}+{\mathbf{Q}}_{k-1}$$

其中：

• ${\mathbf{P}}_k^{-}$：先验状态协方差矩阵；
• ${\mathbf{P}}_{k-1}^{+}$：上一历元后验状态协方差矩阵。

5.3 新息

$${\mathbf{\nu }}_k={\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_k^{-}$$

其中：

• ${\mathbf{\nu }}_k$：新息向量，也称预测残差；
• 它表示实际观测与先验预测观测之间的差。

5.4 新息协方差

$${\mathbf{S}}_k={\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^{\mathsf{T}}+{\mathbf{R}}_k$$

5.5 Kalman 增益

$${\mathbf{K}}_k={\mathbf{P}}_k^{-}{\mathbf{H}}_k^{\mathsf{T}}{\mathbf{S}}_k^{-1}$$

5.6 状态更新

$${\hat{\mathbf{x}}}_k^{+}={\hat{\mathbf{x}}}_k^{-}+{\mathbf{K}}_k{\mathbf{\nu }}_k$$

5.7 协方差更新

推荐使用 Joseph 形式：

$${\mathbf{P}}_k^{+}=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\mathbf{P}}_k^{-}{\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right)}^{\mathsf{T}}+{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^{\mathsf{T}}$$

其中：

• $\mathbf{I}$：单位矩阵；
• Joseph 形式更有利于保持协方差矩阵的对称性和半正定性。

6. Kalman 更新为什么等价于带先验的加权最小二乘

在时刻 $k$，定义目标函数：

$$J(\mathbf{x})=\frac{1}{2}{\left(\mathbf{x}-{\hat{\mathbf{x}}}_k^{-}\right)}^{\mathsf{T}}{\left({\mathbf{P}}_k^{-}\right)}^{-1}\left(\mathbf{x}-{\hat{\mathbf{x}}}_k^{-}\right)+\frac{1}{2}{\left({\mathbf{z}}_k-{\mathbf{H}}_k\mathbf{x}\right)}^{\mathsf{T}}{\mathbf{R}}_k^{-1}\left({\mathbf{z}}_k-{\mathbf{H}}_k\mathbf{x}\right)$$

第一项表示：

状态不要无依据地偏离先验预测

第二项表示：

状态应尽量拟合当前观测

对 $\mathbf{x}$ 求导：

$$\frac{\partial J}{\partial \mathbf{x}}={\left({\mathbf{P}}_k^{-}\right)}^{-1}\left(\mathbf{x}-{\hat{\mathbf{x}}}_k^{-}\right)-{\mathbf{H}}_k^{\mathsf{T}}{\mathbf{R}}_k^{-1}\left({\mathbf{z}}_k-{\mathbf{H}}_k\mathbf{x}\right)$$

令导数为零：

$$\left[{\left({\mathbf{P}}_k^{-}\right)}^{-1}+{\mathbf{H}}_k^{\mathsf{T}}{\mathbf{R}}_k^{-1}{\mathbf{H}}_k\right]\mathbf{x}={\left({\mathbf{P}}_k^{-}\right)}^{-1}{\hat{\mathbf{x}}}_k^{-}+{\mathbf{H}}_k^{\mathsf{T}}{\mathbf{R}}_k^{-1}{\mathbf{z}}_k$$

所以后验估计为：

$${\hat{\mathbf{x}}}_k^{+}={\left[{\left({\mathbf{P}}_k^{-}\right)}^{-1}+{\mathbf{H}}_k^{\mathsf{T}}{\mathbf{R}}_k^{-1}{\mathbf{H}}_k\right]}^{-1}\left[{\left({\mathbf{P}}_k^{-}\right)}^{-1}{\hat{\mathbf{x}}}_k^{-}+{\mathbf{H}}_k^{\mathsf{T}}{\mathbf{R}}_k^{-1}{\mathbf{z}}_k\right]$$

利用矩阵求逆引理，可以证明它与标准 Kalman 更新完全等价。

这个推导建立在以下条件上：

• ${\mathbf{P}}_k^{-}$ 和 ${\mathbf{R}}_k$ 对称正定；
• 先验误差与观测噪声相互独立；
• 观测模型为线性模型，或已经在当前线性化点附近线性化。

这个推导非常重要，因为本文介绍的 M 估计型抗差 Kalman 滤波正是在观测项上进行修改。

7. 标准 Kalman 滤波为什么怕粗差

在线性加权最小二乘或线性高斯 MAP 的解释下，标准 Kalman 更新对应平方损失：

$$\rho (e)=\frac{1}{2}{e}^2$$

其中：

• $e$：标准化后的残差。

平方损失的特点是：

残差增大2倍
→ 损失增大4倍

因此，大残差会对解算结果产生很强影响。

单个异常观测可能在目标函数中占据主导地位，从而把状态估计拉向错误方向。

8. 抗差 Kalman 滤波优化了什么

抗差 Kalman 滤波把平方损失替换为增长更慢的抗差损失函数：

$$\rho (e)=\{\begin{array}{ll}\frac{1}{2}{e}^2,& |e|\text{较小}\\ \text{较慢增长},& |e|\text{较大}\end{array}$$

其作用是：

• 小残差仍按正常观测使用；
• 大残差被降权；
• 极端异常值可以接近剔除。

在本文讨论的观测抗差方案中，优化点不是改变状态预测模型，而是：

动态调整当前观测的可信度，使异常观测不再拥有与正常观测相同的影响力。

其他类型的抗差 Kalman 滤波也可以同时处理过程噪声异常、状态突变或模型不确定性，但其公式不再只是修改 ${\mathbf{R}}_k$。

常见抗差函数包括：

• Huber；
• IGGIII；
• Tukey 双权函数；
• Cauchy；
• Hampel。

9. 异常检测残差与抗差迭代残差

这里容易混淆两种残差。

9.1 预测阶段的新息

新息为：

$${\mathbf{\nu }}_k={\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_k^{-}$$

新息协方差为：

$${\mathbf{S}}_k={\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^{\mathsf{T}}+{\mathbf{R}}_k$$

新息反映“实际观测”与“先验状态预测观测”之间的差，适合在更新前做整体一致性检验。

常用新息平方统计量为：

$$D_k^2={\mathbf{\nu }}_k^{\mathsf{T}}{\mathbf{S}}_k^{-1}{\mathbf{\nu }}_k$$

其中：

• $D_k^2$：归一化新息平方，也称 NIS；
• ${\mathbf{\nu }}_k$：新息向量；
• ${\mathbf{S}}_k$：新息协方差矩阵。

在线性高斯假设成立时，$D_k^2$ 近似服从自由度为观测维数的卡方分布，可用于判断当前观测向量与先验预测是否整体一致。

若要逐分量检查，可对 ${\mathbf{S}}_k$ 做 Cholesky 分解：

$${\mathbf{S}}_k={\mathbf{L}}_{S,k}{\mathbf{L}}_{S,k}^{\mathsf{T}}$$

并定义白化新息：

$${\mathbf{r}}_{\nu ,k}={\mathbf{L}}_{S,k}^{-1}{\mathbf{\nu }}_k$$

其中：

• ${\mathbf{L}}_{S,k}$：新息协方差的 Cholesky 因子；
• ${\mathbf{r}}_{\nu ,k}$：白化新息。

当 ${\mathbf{S}}_k$ 不是对角阵时，白化后的某个分量通常是多个原始观测的线性组合，不一定能直接对应某一颗卫星或某一种观测。若需要定位具体异常观测，应结合独立观测模型、分组检验、删除诊断或其他故障隔离方法。

9.2 M 估计迭代中的观测残差

严格的 M 估计推导通常使用：

$$\mathbf{v}(\mathbf{x})={\mathbf{z}}_k-{\mathbf{H}}_k\mathbf{x}$$

并由观测协方差分解：

$${\mathbf{R}}_k={\mathbf{C}}_k{\mathbf{C}}_k^{\mathsf{T}}$$

构造白化观测残差：

$$\mathbf{e}(\mathbf{x})={\mathbf{C}}_k^{-1}\mathbf{v}(\mathbf{x})$$

其中：

• $\mathbf{v}(\mathbf{x})$：给定状态 $\mathbf{x}$ 时的观测残差；
• ${\mathbf{C}}_k$：观测协方差的 Cholesky 因子；
• $\mathbf{e}(\mathbf{x})$：用于计算抗差权值的白化观测残差。

因此：

白化新息
→ 适合更新前的一致性检测

白化观测残差
→ 适合M估计和IRLS迭代推导

工程实现也可以直接依据白化新息进行方差膨胀，但这属于新息型抗差策略，与严格的观测残差 M 估计并不完全相同。

10. Huber 抗差权函数

Huber 损失函数为：

$$\rho (e)=\{\begin{array}{ll}\frac{1}{2}{e}^2,& |e|\le c\\ c|e|-\frac{1}{2}{c}^2,& |e|>c\end{array}$$

其中：

• $e$：标准化残差；
• $c$：阈值参数。

其影响函数为：

$$\psi (e)=\frac{d\rho (e)}{de}=\{\begin{array}{ll}e,& |e|\le c\\ c\mathrm{sign}(e),& |e|>c\end{array}$$

对应权函数为：

$$w(e)=\frac{\psi (e)}{e}=\{\begin{array}{ll}1,& |e|\le c\\ \frac{c}{|e|},& |e|>c\end{array}$$

其中：

• $w(e)$：抗差权值；
• $w=1$：正常使用；
• $0<w<1$：降低该观测影响；
• $w\to 0$：接近剔除。

11. IGGIII 抗差权函数

GNSS 和测量平差中常见 IGGIII 三段权函数：

$$w(e)=\{\begin{array}{ll}1,& |e|\le k_0\\ \frac{k_0}{|e|}{\left(\frac{k_1-|e|}{k_1-k_0}\right)}^2,& k_0<|e|\le k_1\\ 0,& |e|>k_1\end{array}$$

其中：

• $e$：标准化残差；
• $k_0$：正常观测与可疑观测的分界阈值；
• $k_1$：可疑观测与拒绝观测的分界阈值；
• $w(e)$：抗差权值。

三段含义为：

|e| ≤ k0
→ 正常观测，权值不变

k0 < |e| ≤ k1
→ 可疑观测，逐渐降权

|e| > k1
→ 严重异常，权值置零

12. 抗差权值怎样进入矩阵公式

设原观测协方差为：

$${\mathbf{R}}_k$$

对其进行分解：

$${\mathbf{R}}_k={\mathbf{C}}_k{\mathbf{C}}_k^{\mathsf{T}}$$

定义观测残差的白化形式：

$$\mathbf{e}={\mathbf{C}}_k^{-1}\left({\mathbf{z}}_k-{\mathbf{H}}_k\mathbf{x}\right)$$

使用抗差目标函数：

$$J_R(\mathbf{x})=\frac{1}{2}{\left(\mathbf{x}-{\hat{\mathbf{x}}}_k^{-}\right)}^{\mathsf{T}}{\left({\mathbf{P}}_k^{-}\right)}^{-1}\left(\mathbf{x}-{\hat{\mathbf{x}}}_k^{-}\right)+\sum _{i=1}^m\rho (e_i)$$

其中：

• $m$：当前历元观测数量；
• $e_i$：第 $i$ 个白化观测残差；
• $\rho (\cdot )$：抗差损失函数。

令：

$$w_i=\frac{\psi (e_i)}{e_i}$$

构造对角权矩阵：

$$\mathbf{W}=\mathrm{diag}\left(w_1,w_2,\dots ,w_m\right)$$

在一次迭代中，抗差目标可近似为：

$$J_R(\mathbf{x})\approx \frac{1}{2}{\left(\mathbf{x}-{\hat{\mathbf{x}}}_k^{-}\right)}^{\mathsf{T}}{\left({\mathbf{P}}_k^{-}\right)}^{-1}\left(\mathbf{x}-{\hat{\mathbf{x}}}_k^{-}\right)+\frac{1}{2}{\mathbf{e}}^{\mathsf{T}}\mathbf{We}$$

代入 $\mathbf{e}$：

$$J_R(\mathbf{x})\approx \frac{1}{2}{\left(\mathbf{x}-{\hat{\mathbf{x}}}_k^{-}\right)}^{\mathsf{T}}{\left({\mathbf{P}}_k^{-}\right)}^{-1}\left(\mathbf{x}-{\hat{\mathbf{x}}}_k^{-}\right)+\frac{1}{2}{\left({\mathbf{z}}_k-{\mathbf{H}}_k\mathbf{x}\right)}^{\mathsf{T}}{\left({\mathbf{C}}_k^{-1}\right)}^{\mathsf{T}}\mathbf{W}{\mathbf{C}}_k^{-1}\left({\mathbf{z}}_k-{\mathbf{H}}_k\mathbf{x}\right)$$

因此，等效观测信息矩阵为：

$${\bar{\mathbf{R}}}_k^{-1}={\left({\mathbf{C}}_k^{-1}\right)}^{\mathsf{T}}\mathbf{W}{\mathbf{C}}_k^{-1}$$

等效观测协方差为：

$${\bar{\mathbf{R}}}_k={\mathbf{C}}_k{\mathbf{W}}^{-1}{\mathbf{C}}_k^{\mathsf{T}}$$

其中：

• ${\bar{\mathbf{R}}}_k$：抗差调整后的等效观测协方差矩阵；
• $\mathbf{W}$：抗差权矩阵。

当某个 $w_i=0$ 时，${\mathbf{W}}^{-1}$ 不存在。工程实现通常采用以下方式之一：

• 删除该观测；
• 把权值限制为很小的正数 $w_{\min }$；
• 使用广义逆，但仍需防止矩阵病态。

若观测相互独立，${\mathbf{R}}_k$ 为对角阵，则可简化为：

$${\bar{\sigma }}_i^2=\frac{{\sigma }_i^2}{w_i}$$

其中：

• ${\sigma }_i^2$：原始观测方差；
• ${\bar{\sigma }}_i^2$：抗差调整后的等效方差。

这说明：

降低权值
等价于
增大观测方差

13. 抗差 Kalman 更新公式

将原来的 ${\mathbf{R}}_k$ 替换为抗差等效协方差 ${\bar{\mathbf{R}}}_k$。

13.1 抗差新息协方差

$${\bar{\mathbf{S}}}_k={\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^{\mathsf{T}}+{\bar{\mathbf{R}}}_k$$

13.2 抗差 Kalman 增益

$${\bar{\mathbf{K}}}_k={\mathbf{P}}_k^{-}{\mathbf{H}}_k^{\mathsf{T}}{\bar{\mathbf{S}}}_k^{-1}$$

13.3 状态更新

$${\hat{\mathbf{x}}}_k^{+}={\hat{\mathbf{x}}}_k^{-}+{\bar{\mathbf{K}}}_k{\mathbf{\nu }}_k$$

13.4 协方差更新

$${\mathbf{P}}_k^{+}=\left(\mathbf{I}-{\bar{\mathbf{K}}}_k{\mathbf{H}}_k\right){\mathbf{P}}_k^{-}{\left(\mathbf{I}-{\bar{\mathbf{K}}}_k{\mathbf{H}}_k\right)}^{\mathsf{T}}+{\bar{\mathbf{K}}}_k{\bar{\mathbf{R}}}_k{\bar{\mathbf{K}}}_k^{\mathsf{T}}$$

抗差 Kalman 滤波并没有推翻 Kalman 框架，而是重新评估当前观测的可信度。

13.5 怎样理解更新后的协方差

若 ${\bar{\mathbf{R}}}_k$ 是预先给定、与当前数据无关的真实观测协方差，则 Joseph 形式具有标准 Kalman 协方差传播含义。

抗差滤波中，${\bar{\mathbf{R}}}_k$ 是根据当前残差计算的，因此它与观测数据相关。此时用 Joseph 形式得到的 ${\mathbf{P}}_k^{+}$ 更适合作为：

工作协方差
内部精度指标
工程近似不确定度

它不一定等于抗差估计量的严格统计协方差。若需要严谨的不确定度评定，可进一步采用：

• M 估计的夹心协方差；
• 蒙特卡洛仿真；
• Bootstrap；
• 与真实误差或外部基准进行一致性检验。

14. 为什么通常需要迭代

抗差权值依赖残差：

$$w_i=w\left(e_i\right)$$

而残差又依赖待估状态：

$$e_i=e_i\left(\mathbf{x}\right)$$

因此，状态和权值互相依赖，需要使用迭代重加权最小二乘（IRLS）。

第 $j$ 次迭代时：

固定权值时的正规方程为：

$$\left[{\left({\mathbf{P}}_k^{-}\right)}^{-1}+{\mathbf{H}}_k^{\mathsf{T}}{\left({\bar{\mathbf{R}}}_k^{(j)}\right)}^{-1}{\mathbf{H}}_k\right]{\hat{\mathbf{x}}}_k^{(j+1)}={\left({\mathbf{P}}_k^{-}\right)}^{-1}{\hat{\mathbf{x}}}_k^{-}+{\mathbf{H}}_k^{\mathsf{T}}{\left({\bar{\mathbf{R}}}_k^{(j)}\right)}^{-1}{\mathbf{z}}_k$$

也可以写成等价的 Kalman 形式：

$${\hat{\mathbf{x}}}_k^{(j+1)}={\hat{\mathbf{x}}}_k^{-}+{\bar{\mathbf{K}}}_k^{(j)}\left({\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_k^{-}\right)$$

其中：

$${\bar{\mathbf{K}}}_k^{(j)}={\mathbf{P}}_k^{-}{\mathbf{H}}_k^{\mathsf{T}}{\left[{\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^{\mathsf{T}}+{\bar{\mathbf{R}}}_k^{(j)}\right]}^{-1}$$

状态收敛条件可写为：

∥x^k(j+1)−x^k(j)∥<εx\\left\\lVert \\hat{\\mathbf{x}}_k^{(j+1)} – \\hat{\\mathbf{x}}_k^{(j)} \\right\\rVert < \\varepsilon_x​x^k(j+1)​−x^k(j)​​<εx​

其中：

• $j$：迭代编号；
• ${\epsilon }_x$：状态收敛阈值；
• $\Vert \cdot \Vert$：向量范数。

也可以同时检查权值变化：

max⁡i∣wi(j+1)−wi(j)∣<εw\\max_i \\left\\vert{} w_i^{(j+1)} – w_i^{(j)} \\right\\vert{} < \\varepsilon_wimax​​wi(j+1)​−wi(j)​​<εw​

其中：

• ${\epsilon }_w$：权值收敛阈值。

线性模型中，${\mathbf{H}}_k$ 固定；扩展 Kalman 滤波中，还需根据新的线性化点重新计算观测预测和雅可比矩阵。

15. 抗差 Kalman 滤波流程

#mermaid-svg-a9VI3tofAdSVxiOJ{font-family:\”trebuchet ms\”,verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-a9VI3tofAdSVxiOJ .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-a9VI3tofAdSVxiOJ .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-a9VI3tofAdSVxiOJ .error-icon{fill:#552222;}#mermaid-svg-a9VI3tofAdSVxiOJ .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-a9VI3tofAdSVxiOJ .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-a9VI3tofAdSVxiOJ .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-a9VI3tofAdSVxiOJ .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-a9VI3tofAdSVxiOJ .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-a9VI3tofAdSVxiOJ .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-a9VI3tofAdSVxiOJ .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-a9VI3tofAdSVxiOJ .marker{fill:#333333;stroke:#333333;}#mermaid-svg-a9VI3tofAdSVxiOJ .marker.cross{stroke:#333333;}#mermaid-svg-a9VI3tofAdSVxiOJ svg{font-family:\”trebuchet ms\”,verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-a9VI3tofAdSVxiOJ p{margin:0;}#mermaid-svg-a9VI3tofAdSVxiOJ .label{font-family:\”trebuchet ms\”,verdana,arial,sans-serif;color:#333;}#mermaid-svg-a9VI3tofAdSVxiOJ .cluster-label text{fill:#333;}#mermaid-svg-a9VI3tofAdSVxiOJ .cluster-label span{color:#333;}#mermaid-svg-a9VI3tofAdSVxiOJ .cluster-label span p{background-color:transparent;}#mermaid-svg-a9VI3tofAdSVxiOJ .label text,#mermaid-svg-a9VI3tofAdSVxiOJ span{fill:#333;color:#333;}#mermaid-svg-a9VI3tofAdSVxiOJ .node rect,#mermaid-svg-a9VI3tofAdSVxiOJ .node circle,#mermaid-svg-a9VI3tofAdSVxiOJ .node ellipse,#mermaid-svg-a9VI3tofAdSVxiOJ .node polygon,#mermaid-svg-a9VI3tofAdSVxiOJ .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-a9VI3tofAdSVxiOJ .rough-node .label text,#mermaid-svg-a9VI3tofAdSVxiOJ .node .label text,#mermaid-svg-a9VI3tofAdSVxiOJ .image-shape .label,#mermaid-svg-a9VI3tofAdSVxiOJ .icon-shape .label{text-anchor:middle;}#mermaid-svg-a9VI3tofAdSVxiOJ .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-a9VI3tofAdSVxiOJ .rough-node .label,#mermaid-svg-a9VI3tofAdSVxiOJ .node .label,#mermaid-svg-a9VI3tofAdSVxiOJ .image-shape .label,#mermaid-svg-a9VI3tofAdSVxiOJ .icon-shape .label{text-align:center;}#mermaid-svg-a9VI3tofAdSVxiOJ .node.clickable{cursor:pointer;}#mermaid-svg-a9VI3tofAdSVxiOJ .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-a9VI3tofAdSVxiOJ .arrowheadPath{fill:#333333;}#mermaid-svg-a9VI3tofAdSVxiOJ .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-a9VI3tofAdSVxiOJ .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-a9VI3tofAdSVxiOJ .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-a9VI3tofAdSVxiOJ .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-a9VI3tofAdSVxiOJ .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-a9VI3tofAdSVxiOJ .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-a9VI3tofAdSVxiOJ .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-a9VI3tofAdSVxiOJ .cluster text{fill:#333;}#mermaid-svg-a9VI3tofAdSVxiOJ .cluster span{color:#333;}#mermaid-svg-a9VI3tofAdSVxiOJ div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:\”trebuchet ms\”,verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-a9VI3tofAdSVxiOJ .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-a9VI3tofAdSVxiOJ rect.text{fill:none;stroke-width:0;}#mermaid-svg-a9VI3tofAdSVxiOJ .icon-shape,#mermaid-svg-a9VI3tofAdSVxiOJ .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-a9VI3tofAdSVxiOJ .icon-shape p,#mermaid-svg-a9VI3tofAdSVxiOJ .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-a9VI3tofAdSVxiOJ .icon-shape .label rect,#mermaid-svg-a9VI3tofAdSVxiOJ .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-a9VI3tofAdSVxiOJ .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-a9VI3tofAdSVxiOJ .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-a9VI3tofAdSVxiOJ :root{–mermaid-font-family:\”trebuchet ms\”,verdana,arial,sans-serif;}

每历元都抗差

先做门限检验

否

是

否

是

输入上一历元后验状态和协方差

状态与协方差预测

计算新息和NIS

采用何种策略?

初始化IRLS状态

NIS或质量指标是否异常?

使用原始R完成标准更新

计算观测残差并白化

由Huber或IGGIII计算权值

构造等效观测协方差R_bar

固定权值重新求解后验状态

状态和权值是否收敛?

用最终R_bar计算工作协方差

输出状态、协方差和质量标志

NIS 门限检验是可选步骤，而不是抗差滤波的必要组成部分。整体 NIS 正常并不保证每个观测都正常；整体 NIS 异常也可能由状态模型失配或协方差设置错误引起。

16. 标准 Kalman 与抗差 Kalman 的区别

17. 抗差滤波与自适应滤波不要混淆

17.1 抗差滤波

主要针对：

少量突发粗差
离群值
重尾噪声
异常观测

常用手段：

残差检验
降权
方差膨胀
剔除

17.2 自适应 Kalman 滤波

主要针对：

Q或R设置不准确
噪声统计随时间变化
系统模型缓慢变化

常用手段：

在线估计Q
在线估计R
遗忘因子
协方差匹配

17.3 二者可以结合

抗差模块
→ 处理突发异常值

自适应模块
→ 调整长期变化的噪声水平

这种方法常称为抗差自适应 Kalman 滤波。

18. 抗差 Kalman 滤波针对什么问题

18.1 观测粗差

例如：

• GNSS 伪距突跳；
• NLOS；
• 严重多路径；
• 传感器脉冲干扰；
• 数据解码错误。

18.2 重尾噪声

真实误差分布比高斯分布更容易出现大误差。

18.3 部分模型失配

例如车辆突然急转弯，而滤波器仍使用常速度模型。

此时新息可能变大。

但必须谨慎：

大新息不一定是观测粗差，也可能是状态模型错误。

如果把所有大新息都当作观测异常并降权，滤波器可能拒绝真实运动变化。

对模型失配，更合适的方法可能是：

• 增大过程噪声 ${\mathbf{Q}}_k$；
• 使用多模型 IMM；
• 切换运动模型；
• 使用自适应过程噪声。

19. GNSS 中的典型应用

19.1 单点定位和 RTK

伪距容易受到：

• 多路径；
• NLOS；
• 低高度角；
• 接收机跟踪异常。

抗差方法可以对异常卫星或异常频点降权。

19.2 PPP

以简化的单系统 PPP 为例，状态可写为：

$$\mathbf{x}={\left[\begin{array}{cccc}{\mathbf{r}}^{\mathsf{T}}& c\delta t& T_z& {\mathbf{N}}^{\mathsf{T}}\end{array}\right]}^{\mathsf{T}}$$

其中：

• $\mathbf{r}$：接收机坐标；
• $c\delta t$：接收机钟差参数，采用距离单位表示；
• $c$：真空光速；
• $\delta t$：接收机钟差，采用时间单位表示；
• $T_z$：天顶对流层湿延迟；
• $\mathbf{N}$：载波相位模糊度向量。

多系统、多频或非组合 PPP 还可能包含系统间偏差、电离层延迟、硬件偏差和更多钟差参数，因此上式只用于说明状态之间的误差传播关系。

异常伪距可能污染：

• 坐标；
• 钟差；
• 对流层参数。

异常载波相位可能污染：

• 模糊度；
• 坐标；
• 滤波连续性。

对于已经确认的周跳或失锁，通常应重置对应模糊度状态，而不是只依靠抗差降权。

19.3 GNSS/INS 组合导航

GNSS 位置更新偶尔跳变时，抗差滤波可降低异常 GNSS 更新对 INS 状态的冲击。

19.4 多传感器融合

适用于：

• GNSS；
• IMU；
• 轮速计；
• 视觉；
• 激光雷达；
• 气压计。

当某一传感器短时异常时，抗差机制可以避免它主导融合结果。

20. 抗差 Kalman 滤波的优势

20.1 抑制少量异常观测

少数粗差不会轻易把整个状态估计拉偏。

20.2 保留递推结构

它仍然使用 Kalman 的预测—更新框架，便于嵌入现有实时系统。

20.3 比简单剔除更平滑

简单粗差检验通常只有：

保留
或
删除

抗差权函数可以实现：

正常使用
轻度降权
重度降权
完全剔除

20.4 便于与现有随机模型结合

高度角、信噪比和频点模型仍可先构造基础 ${\mathbf{R}}_k$，再叠加抗差权值。

20.5 适合实时处理

相比批处理抗差估计，抗差 Kalman 更适合连续导航和实时状态估计。

21. 它有没有“独有”的使用场景

严格来说，抗差 Kalman 滤波没有“只有它才能使用”的绝对独有场景。

但以下场景特别适合：

21.1 不能轻易删除整历元数据

例如动态 GNSS/INS 导航中，每个历元都要连续输出。

21.2 大多数观测正常，少数观测偶发异常

这是抗差方法最擅长的情况。

21.3 异常观测会污染长期状态

例如 PPP 中的：

• 模糊度；
• 对流层参数；
• 接收机钟差；
• IMU 零偏。

21.4 需要实时、递推和连续输出

相比大规模批处理优化，抗差 Kalman 可以按历元递推。若使用多次 IRLS 迭代，计算量仍高于标准 Kalman，因此嵌入式实现通常需要限制最大迭代次数。

22. 抗差 Kalman 滤波的局限性

22.1 大残差不一定是粗差

它可能来自：

• 状态模型错误；
• 周跳；
• 状态初始化错误；
• 协方差设置过小；
• 时间同步错误；
• 坐标系或单位错误。

22.2 阈值过小会误伤正常数据

阈值过小会导致：

• 精度下降；
• 收敛变慢；
• 状态约束不足。

22.3 多数观测同时异常时可能失效

抗差估计通常假设：

正常观测占多数
异常观测占少数

22.4 观测相关性处理不当会重复降权

更合理的方法是：

• 先白化；
• 或在完整协方差框架下构造等效权矩阵。

22.5 协方差可能过于乐观

如果只修改状态而没有同步修改 ${\bar{\mathbf{R}}}_k$ 和 ${\mathbf{P}}_k^{+}$，输出协方差会不可信。即使同步更新，数据依赖权值下的 Joseph 协方差通常也只是工程近似。

22.6 可能出现掩盖和错判

当多个异常观测同时存在，或某个观测具有较高杠杆作用时，残差可能被状态吸收，导致异常观测残差反而不大。这种现象称为掩盖效应。

因此，抗差滤波应与以下方法配合：

• 卫星或传感器分组检验；
• 解算前质量控制；
• 几何与可观性检查；
• 删除诊断或故障隔离。

23. GNSS 工程实现建议

23.1 先做硬故障检测

以下问题通常不应只靠降权：

• 明确周跳；
• 失锁；
• 数据缺失；
• 时间标签错误；
• 观测类型不匹配；
• 星历或改正数失效。

23.2 再做抗差降权

对仍可使用但可信度下降的观测，例如：

• 轻度多路径；
• 短时 NLOS；
• 异常伪距；
• 低质量多普勒；

可以使用 Huber 或 IGGIII 降权。

23.3 不同观测类型分开设阈值

伪距、载波相位和多普勒的精度差异很大，不应直接使用同一个未标准化阈值。

23.4 记录抗差诊断信息

建议输出：

• 原始新息；
• 标准化新息；
• 抗差权值；
• 等效方差；
• 被降权的卫星和观测类型；
• 迭代次数；
• 状态变化量。

24. 简化伪代码

下面给出线性观测模型下的 M 估计—IRLS 实现框架。

输入：
上一历元后验状态 x_plus
上一历元后验协方差 P_plus
当前观测 z
状态模型 F、Q
观测模型 H、R

1. 预测：
x_minus = F * x_plus
P_minus = F * P_plus * F^{\\mathsf{T}} + Q

2. 可选的一致性检查：
innovation = z – H * x_minus
S = H * P_minus * H^{\\mathsf{T}} + R
NIS = innovation^{\\mathsf{T}} * inverse(S) * innovation

3. 初始化IRLS：
x_iter = x_minus或标准Kalman后验
对R做Cholesky分解：R = C * C^{\\mathsf{T}}

4. IRLS迭代：
residual = z – H * x_iter
e = solve(C, residual)

根据Huber或IGGIII计算W

若某个权值为0：
删除对应白化分量或相关观测
或令w = w_min

R_bar = C * inverse(W) * C^{\\mathsf{T}}

K = P_minus * H^{\\mathsf{T}} *
inverse(H * P_minus * H^{\\mathsf{T}} + R_bar)

x_new = x_minus + K * (z – H * x_minus)

若状态变化和权值变化均小于阈值：
x_iter = x_new
停止
否则：
x_iter = x_new
继续迭代

5. 最终状态：
x_plus = x_iter

6. 工作协方差：
使用最终K和R_bar的Joseph形式计算P_plus

7. 输出：
x_plus、P_plus、权值、NIS和异常标志

注意：

• 上述代码针对线性模型；
• 非线性 EKF 中，应重新计算观测函数、线性化残差和雅可比矩阵；
• 若 $\mathbf{R}$ 非对角，白化分量不一定与原始观测一一对应，不能简单按某个白化分量删除某颗卫星；
• 数值实现宜使用 Cholesky 分解和线性方程求解，避免显式计算矩阵逆。

25. 总结

标准Kalman滤波
→ 所有观测按照预设R参与更新

抗差Kalman滤波
→ 根据残差重新评估观测可信度
→ 异常观测增大方差、降低权值或剔除

“抗差”表示：

对少量粗差和离群值不敏感

对于本文介绍的 M 估计型观测抗差方法，其数学本质是：

把观测项的平方损失
替换为抗差损失

再通过IRLS
转换为动态观测权值或等效R矩阵

抗差 Kalman 最适合：

大多数观测正常
少数观测偶发异常
同时要求实时、递推和连续输出

它不能代替：

• 周跳检测；
• 故障诊断；
• 状态模型设计；
• ${\mathbf{Q}}_k$、${\mathbf{R}}_k$ 合理建模；
• 自适应滤波；
• 完整的数据质量控制。

最关键的一句话是：

抗差 Kalman 滤波不是让滤波器“看不见误差”，而是让滤波器在发现异常残差后，不再盲目信任异常观测。