第1章：序贯重要性采样（SIS）——粒子滤波的“心脏”

说实话，很多搞SLAM的朋友一上来就学粒子滤波，结果被各种公式砸晕了。我当年也是这么过来的。后来才发现，只要搞懂了序贯重要性采样（SIS），粒子滤波就掌握了一半。今天咱们就来聊聊这个核心算法。

1.1 为什么需要SIS？

先问个问题：如果状态空间是线性的、噪声是高斯的，我们用卡尔曼滤波就搞定了。但现实世界哪有那么完美？

我在做室内定位项目时遇到过这种情况：WiFi信号受墙体遮挡，噪声分布根本不是高斯。这时候卡尔曼滤波直接崩了。怎么办？

粒子滤波的思路很直接——用一堆带权重的粒子来近似后验概率分布。但问题来了：我们怎么得到这些粒子？怎么更新它们的权重？

这就是SIS要解决的事。

1.2 SIS算法推导——从重要性采样说起

重要性采样，说白了就是“借鸡生蛋”。

我们想从后验概率p(x|z)中采样，但直接采样太难了。那就找一个容易采样的提议分布q(x|z)，从里面采样，然后给每个样本加个权重来修正偏差。

公式长这样：

w(x) = p(x|z) / q(x|z)

嗯，这里要注意：权重就是真实分布和提议分布的比值。

那序贯重要性采样呢？就是把重要性采样扩展到时间序列上。我们不是一次性采样所有时刻的状态，而是一步一步来。

假设我们有t-1时刻的粒子集合{x_{t-1}^{(i)}, w_{t-1}^{(i)}}，现在要得到t时刻的粒子。推导过程是这样的：

1. 从提议分布中采样新粒子：x_t^{(i)} ~ q(x_t | x_{t-1}^{(i)}, z_t)
2. 计算重要性权重：

w_t^{(i)} = w_{t-1}^{(i)} * [p(z_t | x_t^{(i)}) * p(x_t^{(i)} | x_{t-1}^{(i)})] / q(x_t^{(i)} | x_{t-1}^{(i)}, z_t)

这个公式看着复杂，其实拆开看就三部分：

• p(z_t | x_t)：观测似然——当前状态能解释观测数据的程度
• p(x_t | x_{t-1})：状态转移概率——根据运动模型预测的状态
• q(x_t | x_{t-1}, z_t)：提议分布——我们实际采样的分布

w_t^{(i)} = w_{t-1}^{(i)} * p(z_t | x_t^{(i)})

1.3 权值计算的细节

权值计算有个容易踩坑的地方——数值稳定性。

你想想看，随着时间推移，有些粒子的权重会变得非常小，小到浮点数都表示不了。我曾经在嵌入式平台上跑粒子滤波，debug了一整天，最后发现是权重下溢了。

解决方案有两个：

1. 对数域计算：把乘法变成加法，避免数值下溢
2. 归一化处理：每次更新后对权重做归一化

归一化公式很简单：

w_t^{(i)} = w_t^{(i)} / sum(w_t)

但要注意，归一化要在所有粒子权重计算完之后再做。

1.4 粒子退化问题——SIS的“阿喀琉斯之踵”

SIS算法有个致命缺陷：粒子退化。

什么意思呢？就是经过几次迭代后，大部分粒子的权重都趋近于0，只有少数几个粒子有显著权重。这就像公司里只有几个人干活，其他人都在摸鱼——效率极低。

为什么会这样？

因为权重的方差会随着时间累积而增大。你想想看，每次更新权重都要乘一个似然值，如果某个粒子的状态跟观测不太匹配，它的权重就会指数级下降。

衡量退化程度的指标叫有效粒子数：

N_eff = 1 / sum((w_t^{(i)})^2)

当N_eff小于某个阈值（比如N/2）时，就说明退化严重了。

1.5 应对退化的策略

既然退化不可避免，那怎么应对呢？

主要有三种思路：

重采样是最常用的方法，也是下一章要讲的内容。这里先提一句：重采样不是万能的，它也有副作用——粒子多样性会降低。

我个人建议的做法是：

1. 先监控N_eff，设置一个阈值
2. 只在必要时才重采样
3. 重采样后适当增加过程噪声，保持粒子多样性

1.6 一个简单的SIS实现

说了这么多，咱们看个代码吧。这是一个简化版的SIS实现：

def sis_filter(particles, weights, control, observation):
    """
    序贯重要性采样一步迭代
    particles: 当前粒子集 [N, dim]
    weights: 当前权重 [N]
    control: 控制输入
    observation: 观测值
    """
    N = len(particles)
    
    # 1. 从提议分布采样新粒子
    # 这里用状态转移作为提议分布
    new_particles = motion_model(particles, control)
    new_particles += np.random.randn(N, dim) * 0.1  # 加噪声
    
    # 2. 计算重要性权重
    # 因为提议分布=状态转移，权重简化为观测似然
    new_weights = weights * observation_likelihood(new_particles, observation)
    
    # 3. 归一化
    new_weights /= np.sum(new_weights)
    
    # 4. 检查退化程度
    N_eff = 1.0 / np.sum(new_weights ** 2)
    if N_eff < N / 2:
        print(f"警告：有效粒子数 {N_eff:.2f}，建议重采样")
    
    return new_particles, new_weights

这段代码虽然简单，但包含了SIS的核心步骤。实际项目中，你还需要考虑：

• 提议分布的设计（直接影响性能）
• 观测似然的计算（可能涉及高维数据）
• 数值稳定性处理（防止下溢）

1.7 小结

SIS是粒子滤波的基石。它用带权重的粒子来近似后验分布，通过序贯更新的方式处理时间序列数据。但粒子退化问题是个硬伤，需要配合重采样等技术来解决。

下一章我们会讲重采样算法，看看怎么“优胜劣汰”，让粒子集保持活力。到时候你会发现，SIS+重采样才是粒子滤波的完整形态。

嗯，今天就到这里。如果你在实际项目中遇到SIS相关的问题，欢迎交流。毕竟这些坑，我都踩过。