卡尔曼滤波与粒子滤波

定位仪每秒都给出一个位置，但读数有噪声；运动模型也只能近似预测下一秒。状态估计把这两种不完整的信息按明确的误差模型结合起来。卡尔曼滤波在离散时间线性、高斯假设下，用均值和协方差递推完整后验；粒子滤波用带权样本近似后验，能容纳更复杂的非线性或非高斯分布，但需要更多计算。

一次测量的加权

先不考虑运动，只估计一维位置。观测前的预测是 ${\hat{x}}^{-} = 10$ 米，误差方差 $P^{-} = 4$ 平方米；新传感器报 $y = 13$ 米，测量噪声方差 $R = 9$ 平方米。若两个误差独立、均值零且高斯，测量残差为 $y - {\hat{x}}^{-} = 3$ 米。卡尔曼增益 $K = \frac{P^{-}}{P^{-} + R} = \frac{4}{13}$ 使更新后的估计成为 ${\hat{x}}^{+} = {\hat{x}}^{-} + K (y - {\hat{x}}^{-}) = 10 + \frac{12}{13} = \frac{142}{13} \approx 10.923 m .$ 它比十更靠近新读数，却没有直接跳到十三，因为这次测量比原预测更不精确。更新方差为 $P^{+} = (1 - K) P^{-} = 36 / 13 \approx 2.769 m^{2}$ 。图上后验位置落在预测与观测之间；标出的三点是各自的中心值，图上的间距并不代替方差计算。

一维位置线上先验估计十米、后验估计约十点九二三米、测量十三米依次排列，箭头显示后验只向测量移动了残差的四分之十三 — 传感器噪声方差越大，更新越保守；相对可信度由方差而不是直觉决定。

公式也可从最小方差得到。设更新形式 ${\hat{x}}^{+} = {\hat{x}}^{-} + K (y - {\hat{x}}^{-})$ ，预测误差与测量误差独立，则新误差方差是 $P^{+} (K) = (1 - K)^{2} P^{-} + K^{2} R .$ 对 $K$ 求导并令零： $- 2 (1 - K) P^{-} + 2 K R = 0$ ，所以 $K = P^{-} / (P^{-} + R)$ 。这个推导说明增益并非随意调出的“平滑系数”；独立性或误差方差一旦不对，权重也会不对。

随时间预测和校正

一般线性状态模型写成 $x_{k} = A x_{k - 1} + w_{k - 1}$ 、观测模型写成 $y_{k} = H x_{k} + v_{k}$ 。过程噪声 $w$ 、测量噪声 $v$ 的协方差分别是 $Q, R$ ；标准递推还假设它们零均值、彼此独立，并与起始估计误差独立。每一步先用动态方程预测 ${\hat{x}}_{k}^{-} = A {\hat{x}}_{k - 1}^{+}, P_{k}^{-} = A P_{k - 1}^{+} A^{𝖳} + Q,$ 再用新观测修正 $S_{k} = H P_{k}^{-} H^{𝖳} + R, K_{k} = P_{k}^{-} H^{𝖳} S_{k}^{- 1}, {\hat{x}}_{k}^{+} = {\hat{x}}_{k}^{-} + K_{k} (y_{k} - H {\hat{x}}_{k}^{-}) .$ 数值实现可用 $P_{k}^{+} = (I - K_{k} H) P_{k}^{-} (I - K_{k} H)^{𝖳} + K_{k} R K_{k}^{𝖳}$ （Joseph 形式）保存对称性与非负性，避免浮点误差让协方差变成负值。若没有当步观测，只做预测，不应伪造一个测量。

粒子在同一问题中承担什么

若状态运动模型非线性或可能有两个相距很远的候选位置，单个均值加协方差难以表达整个位置信念。粒子滤波保留 $N$ 个状态样本 $x_{k}^{(i)}$ 及归一化权重 $w_{k}^{(i)}$ ，权重和为一。每步先从状态转移模型抽取新粒子，再依据观测似然 $p (y_{k} ∣ x_{k}^{(i)})$ 更新权重： ${\tilde{w}}_{k}^{(i)} = w_{k - 1}^{(i)} p (y_{k} ∣ x_{k}^{(i)}), w_{k}^{(i)} = \frac{{\tilde{w}}_{k}^{(i)}}{\sum_{j} {\tilde{w}}_{k}^{(j)}} .$ 若大量权重几乎为零，有效粒子数 $N_{e f f} = 1 / \sum_{i} (w_{k}^{(i)})^{2}$ 会远小于 $N$ ；这时可按权重重采样。重采样后恢复相等权重，但不能从它凭空创造新的位置假设。似然过窄、粒子过少或模型根本漏掉一条可能轨迹，滤波仍会失败。

下面是与上面标量例子一致、可独立运行的 Python 3 代码。卡尔曼函数返回更新均值与方差；粒子函数以给定随机种子，从先验 $N (10, 4)$ 抽样、按观测 $13$ 和方差 $9$ 加权，并返回加权均值、有效粒子数与按权重系统重采样后的粒子。不改写输入。粒子输出因抽样而近似，并不会恰好等于 $142 / 13$ 。

Python 3

import math
import random

def kalman_scalar(prior_mean, prior_var, reading, measurement_var):
    if prior_var <= 0 or measurement_var <= 0:
        raise ValueError("variances must be positive")
    gain = prior_var / (prior_var + measurement_var)
    mean = prior_mean + gain * (reading - prior_mean)
    variance = (1 - gain) * prior_var
    return mean, variance

def particle_update(prior_mean, prior_var, reading, measurement_var,
                    count=10000, seed=7):
    if min(prior_var, measurement_var) <= 0 or count < 2:
        raise ValueError("positive variances and at least two particles required")
    rng = random.Random(seed)
    particles = [rng.gauss(prior_mean, math.sqrt(prior_var))
                 for _ in range(count)]
    log_w = [-0.5 * (reading - x) ** 2 / measurement_var
             for x in particles]
    top = max(log_w)
    raw = [math.exp(value - top) for value in log_w]
    total = sum(raw)
    weights = [value / total for value in raw]
    estimate = sum(x * w for x, w in zip(particles, weights))
    effective = 1 / sum(w * w for w in weights)
    start = rng.random() / count
    positions = (start + j / count for j in range(count))
    resampled = []
    index, cumulative = 0, weights[0]
    for target in positions:
        while target > cumulative and index < count - 1:
            index += 1
            cumulative += weights[index]
        resampled.append(particles[index])
    return estimate, effective, resampled

exact_mean, exact_var = kalman_scalar(10, 4, 13, 9)
approx_mean, neff, samples = particle_update(10, 4, 13, 9)
print(round(exact_mean, 3), round(exact_var, 3))
print(round(approx_mean, 1), round(neff), len(samples))

此实现只演示一次观测更新，没有替连续时间跟踪写完整预测循环。标量卡尔曼更新每步耗时与存储为 $O (1)$ ；粒子更新和系统重采样都是 $O (N)$ 时间、 $O (N)$ 空间。实际应用要用留出的轨迹检查位置误差与不确定区间覆盖率，并分别识别过程噪声 $Q$ 与传感器噪声 $R$ ；仅让轨迹看起来平滑不等于估计准确。

参考资料与相关词条