4. 粒子滤波基础
粒子滤波，说白了就是一套「用一堆随机点去猜状态」的方法。我最早接触它是在做地下车库定位的时候，GPS信号全丢，轮速里程计又飘得厉害，最后就是靠粒子滤波硬生生把车拽回来的。今天咱们就把这块硬骨头啃下来。
4.1 贝叶斯滤波：一切滤波的祖宗
贝叶斯滤波的核心思想其实就一句话：用当前观测去修正对状态的信念。你想想看，我们做定位，本质上就是在回答一个问题——「已知过去所有的控制指令和传感器读数，我现在最有可能在哪？」
数学上，它写成这样：
预测步： p(x_k | z_{1:k-1}) = ∫ p(x_k | x_{k-1}) p(x_{k-1} | z_{1:k-1}) dx_{k-1}
更新步： p(x_k | z_{1:k}) = η · p(z_k | x_k) · p(x_k | z_{1:k-1})这里 p(x_k | x_{k-1}) 是运动模型，p(z_k | x_k) 是观测模型。贝叶斯滤波给出了一个完美的理论框架，但问题是——那个积分，在绝大多数情况下根本算不出来。
关键认知： 贝叶斯滤波是「道」，粒子滤波是「术」。道告诉你应该怎么做，术告诉你算不出来的时候怎么近似。
4.2 蒙特卡洛方法：用随机性对抗复杂性
蒙特卡洛方法，名字听着高大上，其实就是「用大量随机样本去逼近一个分布」。我习惯把它理解成：如果解析解太难求，那就撒一把点，用点的统计量去估计。
举个例子，你想算一个不规则图形的面积。解析法可能需要积分，但蒙特卡洛的做法是：往图形所在的矩形里随机撒点，数一数落在图形内的点占比，乘以矩形面积，完事。
在定位里，我们撒的不是点，是粒子。每个粒子代表一个可能的车辆位姿，粒子的密度代表该位姿的概率大小。
# 蒙特卡洛估计期望值的伪代码
def monte_carlo_expectation(f, samples):
# samples 是从分布 p(x) 中采样的粒子
return np.mean([f(x) for x in samples])嗯，这里要注意：蒙特卡洛方法本身不挑分布，但它的精度和采样数量直接相关。采样越多，估计越准，但计算量也越大。我在项目中一般用 500~2000 个粒子，再多了实时性就扛不住了。
4.3 重要性采样：从已知分布中借力
问题来了：我们想从后验分布 p(x_k | z_{1:k}) 中采样，但这个分布我们根本不知道长什么样。怎么办？
重要性采样的思路很巧妙：从一个容易采样的已知分布（提议分布）中采样，然后给每个样本加一个权重，用来修正采样分布和目标分布之间的差异。
数学表达：
权重 w_k^i = p(x_k^i | z_{1:k}) / q(x_k^i | x_{k-1}^i, z_k)其中 q 是提议分布，通常取运动模型 p(x_k | x_{k-1})。这样每个粒子就有了两个属性：状态值和权重。权重越大，说明这个粒子越「可信」。
我的经验： 提议分布的选择直接影响滤波效果。如果提议分布和目标分布差得太远，大部分粒子的权重会趋近于零，这就是后面要讲的粒子退化问题。我建议在运动模型噪声较大时，考虑将观测信息融入提议分布中。
4.4 粒子退化：为什么粒子会「死掉」
粒子退化是粒子滤波最头疼的问题。说白了就是：经过几次迭代更新后，大部分粒子的权重变得极小，只有少数几个粒子拥有绝大部分权重。
为什么会这样？你想想看，每次更新时，我们根据观测给粒子重新赋权。如果某个粒子的状态和真实观测不太吻合，它的权重就会变小。几轮下来，大部分粒子都变成了「无效粒子」，只有少数几个「幸运儿」还活着。
衡量退化程度的指标是有效粒子数：
N_eff = 1 / Σ (w_k^i)^2当 N_eff 远小于实际粒子数 N 时，说明退化严重。我曾经在一个项目中，500个粒子跑了几步就只剩下3个有效粒子，定位结果直接崩了。
避坑指南： 我曾经在高速场景下忽略了粒子退化问题，结果车辆定位突然跳变，差点导致控制模块误判。后来我养成了一个习惯：每次更新后都计算 N_eff，一旦低于阈值（比如 N/2），立即触发重采样。
4.5 重采样：让粒子「优胜劣汰」
重采样就是解决粒子退化的标准手段。核心思想：根据权重重新抽取粒子，权重高的粒子多复制几份，权重低的粒子直接淘汰。
常见的重采样方法有几种：
| 方法 | 原理 | 特点 |
|---|---|---|
| 多项式重采样 | 按权重分布随机抽取 | 实现简单，方差较大 |
| 残差重采样 | 先按整数部分复制，再随机分配余数 | 方差较小，效率高 |
| 系统重采样 | 等间隔划分累积权重区间 | 方差最小，我常用这个 |
系统重采样的代码实现：
def systematic_resample(weights):
N = len(weights)
positions = (np.arange(N) + np.random.random()) / N
cumsum = np.cumsum(weights)
indices = np.zeros(N, dtype=int)
i, j = 0, 0
while i < N:
if positions[i] < cumsum[j]:
indices[i] = j
i += 1
else:
j += 1
return indices重采样后，所有粒子的权重被重置为 1/N。但要注意，重采样不是万能的——它会导致粒子多样性降低，也就是「粒子贫化」问题。所有重采样后的粒子都来自少数几个「祖先」，如果这些祖先本身就有偏差，那后续的估计也会跟着偏。
我的建议： 不要每步都重采样，只在 N_eff 低于阈值时触发。这样既能抑制退化，又能保留粒子多样性。阈值我一般设在 N/2 到 N/3 之间，具体看场景的观测噪声水平。
4.6 知识体系总览
下面这张图把粒子滤波的四个核心模块串起来了。你可以把它当作调试时的检查清单：
从这张图可以看得很清楚：贝叶斯滤波是理论基石，蒙特卡洛方法提供了实现手段，重要性采样解决了「从哪采样」的问题，而粒子退化和重采样则是一对「问题与解决方案」。这四个模块环环相扣，缺一不可。
最后说一句：粒子滤波不是银弹。它在高维空间里效率会急剧下降，而且对提议分布的选择很敏感。但如果你做的是二维平面上的定位，或者三维空间中的姿态估计，它依然是最实用的工具之一。我在量产项目中见过不少用粒子滤波跑得稳稳当当的方案，关键就在于把上面这四个模块调明白了。