Part I · 第 4 章 概率 · 4.10Part I · Ch. 4 Probability · 4.10

粒子滤波The Particle Filter

卡尔曼滤波只会用一个高斯表示"我在哪"，可真实的信念常常有好几个峰。粒子滤波 (particle filter) 干脆放弃用公式描述信念：撒一群带权重的样本，每个样本是一个"可能的机器人"。预测时让每个样本自己走一步，加权时按传感器读数给每个样本打分，重采样时让高分者繁殖、低分者淘汰。用它做的机器人定位叫蒙特卡洛定位 (MCL)。 A Kalman filter represents "where am I" with one Gaussian, yet real beliefs often have several peaks. The particle filter simply gives up on describing the belief by a formula: it scatters a crowd of weighted samples, each one a "possible robot". On predict every sample takes its own step, on weight every sample is scored by the sensor reading, on resample high scorers multiply and low scorers die. Robot localization built on it is called Monte Carlo localization (MCL).

约 30 分钟~30 min 前置：4.7 贝叶斯滤波 · 4.9 采样方法 · 4.8 卡尔曼滤波（对比）Prereqs: 4.7 Bayes Filter · 4.9 Sampling Methods · 4.8 Kalman Filter (for contrast) 3 个交互3 interactives

    1为什么机器人需要它Why robots need this

    卡尔曼滤波 (4.8) 又快又准，可它有两个硬伤：信念只能是单峰的高斯，模型最好是线性的。现实不总这样：

    • 走廊里有几扇一模一样的门，机器人看到一扇门，它可能在任何一扇门前：信念天然是多峰的。
    • 机器人被人突然抱到别的房间（绑架问题，kidnapped robot）：它必须放弃原来的自信，在全图上重新搜索。
    • 激光对地图的匹配、里程计的转弯，都是强非线性的，线性化 (EKF) 会失真。

    4.7 节的网格（直方图）滤波能处理这些，但网格数随维度指数增长：三维位姿 \((x,y,\theta)\) 在一张大地图上要几百万个格子。粒子滤波用样本代替网格：只在"概率大的地方"放样本，所以算力集中在真正重要的区域，实现简单，还能直接使用任何你能仿真的运动模型和传感器模型。这也是 ROS 里最常用的室内定位器 AMCL 的核心。

    The Kalman filter (4.8) is fast and accurate, but it has two hard limits: its belief must be a single-peaked Gaussian, and the model should be linear. Reality is not always so kind:

    • A corridor has several identical doors; seeing one, the robot may be at any of them: the belief is inherently multimodal.
    • Someone picks the robot up and carries it to another room (the kidnapped robot problem): it must drop its old confidence and search the whole map again.
    • Laser-to-map matching and odometry through turns are strongly nonlinear, and linearization (EKF) distorts them.

    The grid (histogram) filter of Lesson 4.7 handles all of these, but the number of cells grows exponentially with dimension: a 3D pose \((x,y,\theta)\) on a large map needs millions of cells. The particle filter replaces the grid by samples: it puts samples only "where the probability is", so compute goes to the regions that matter; it is easy to implement and can use any motion and sensor model you can simulate. It is also the core of AMCL, the most common indoor localizer in ROS.

    2直觉：预测、加权、重采样Intuition: predict, weight, resample

    回到 4.7 节那条有五扇门的走廊，这次位置是连续的。我们用 \(N\) 个粒子表示信念，每个粒子是走廊上的一个位置 \(x^{[i]}\)，带一个权重 \(w^{[i]}\)。每一轮三步：

    1. 预测：下令"向右走 1 m"，每个粒子独立地走 \(1\) m 再加一点随机噪声（就是从运动模型中采样，4.9 节）。粒子群整体平移、散开。
    2. 加权：传感器说"我看到一扇门"。门里的粒子得到高分（似然 \(a\)），墙边的粒子得到低分（\(1-a\)）。这一步粒子不动，只改权重（FIG 1 里圆点变大、竖线变长）。
    3. 重采样：权重是"繁殖概率"。把带权重的粒子集合替换成 \(N\) 个等权重的新粒子：高权重的被复制多份，低权重的多半消失。这样粒子就重新集中到可能性大的地方。

    重采样的做法就是 4.9 节的逆 CDF 采样：把权重首尾相接铺成一条 [0,1] 的线段（图中紫色长条），按等间距撒 \(N\) 根指针（系统重采样），每根指针落在谁的线段上，就复制谁。

    Back to the five-door corridor of Lesson 4.7, now with a continuous position. The belief is \(N\) particles, each a position \(x^{[i]}\) on the corridor with a weight \(w^{[i]}\). Each round has three steps:

    1. Predict: command "move right 1 m"; every particle moves 1 m independently plus some random noise (sampling from the motion model, Lesson 4.9). The cloud shifts and spreads.
    2. Weight: the sensor says "I see a door". Particles inside doors get a high score (likelihood \(a\)), those by walls a low one (\(1-a\)). The particles do not move in this step, only their weights change (dots grow and stems lengthen in FIG 1).
    3. Resample: weights act as "reproduction probabilities". Replace the weighted set by \(N\) equally weighted new particles: high-weight ones are copied several times, low-weight ones mostly vanish. The cloud re-concentrates where the belief is high.

    Resampling is exactly the inverse-CDF sampling of Lesson 4.9: lay the weights end to end into a [0,1] segment (the long purple bar in the figure), place \(N\) equally spaced pointers (systematic resampling), and copy whichever particle's segment each pointer lands on.

    FIG 1走廊里的粒子滤波：三步循环A particle filter in the corridor: the three-step loop
    顶部是走廊（蓝块 = 门，可拖动的大圆点 = 真实机器人）。中间一行是粒子：青 = 预测后，紫 = 加权后（点越大、竖线越长，权重越高），黄 = 重采样后（等权重）。紫色长条是累积权重，竖直的刻度是重采样的指针，细线连到被选中的粒子。底部柱状图是"由粒子估计的每格概率"，可以和 4.7 节的直方图滤波对比。 The top is the corridor (blue blocks = doors, the large draggable dot = the true robot). The middle row shows the particles: cyan = after predict, purple = after weighting (bigger dot, longer stem, higher weight), amber = after resampling (equal weights). The long purple bar is the cumulative weight; the ticks are the resampling pointers, joined by thin lines to the particles they select. The bar chart at the bottom is "probability per cell estimated from the particles", comparable with the histogram filter of Lesson 4.7.
    试一试Try this
    • 多峰：先把机器人拖到一扇门（蓝块）上，"重置"，再"加权"（应读到"门"）、"重采样"：粒子集中到几扇门前，出现至少两个相距 5 m 以上、各占 18% 以上质量的簇（簇 = 一格加左右相邻两格）。如果读到的是"墙"，重置后再试一次。Multimodal: drag the robot onto a door (blue block), Reset, then Weight (it should read "door") and Resample: the particles gather at several doors, giving at least two clusters 5 m apart, each holding 18%+ of the mass (a cluster = a cell plus its two neighbours). If the reading was "wall", Reset and try again.
    • 定位成功：交替"移动 → 加权 → 重采样"，让真实位置 ±1 m 内的粒子占 90% 以上。Localize: alternate move → weight → resample until 90%+ of the particles are within ±1 m of the true position.
    • 粒子太少：把粒子数调到 12 个以下，切到"多项式重采样"，不移动，反复"加权 → 重采样"：看看会不会"一个粒子也没落在真实位置附近"，滤波器还一脸自信（一格里聚了 60% 以上的粒子）。Too few particles: set the particle count to 12 or fewer, switch to multinomial resampling and, without moving, repeat Weight → Resample: can you reach a state where not a single particle is near the true position while the filter looks confident (60%+ of the particles in one cell)?
    核心洞见Key insight

    粒子滤波就是 4.7 节贝叶斯滤波的"蒙特卡洛版"：粒子的疏密就是概率密度。预测 = 让每个粒子按运动模型采样；更新 = 按似然给粒子加权；重采样 = 把权重"兑现"成粒子的疏密。它不需要线性、不需要高斯，代价是随机性：样本不够时，估计会抖，甚至把真值整个漏掉。

    The particle filter is the "Monte Carlo version" of the Bayes filter of Lesson 4.7: the density of particles is the probability density. Predict = sample every particle from the motion model; update = weight particles by likelihood; resample = cash weights in as particle density. It needs no linearity and no Gaussians, at the price of randomness: with too few samples the estimate jitters and may miss the truth entirely.

    3动手玩：重采样本身也会"伤"粒子Play: resampling itself damages the particle set

    重采样有一个副作用：每次都是有放回抽样，高权重的粒子被复制，别的粒子消失，粒子群里不同的"祖先"越来越少。即使没有任何新信息（所有权重相等），只要你每一步都重采样，多样性也会像"遗传漂变"那样一代代流失，最后所有粒子都是同一个祖先的拷贝。这叫粒子贫化 / 粒子耗尽 (sample impoverishment / particle depletion)。注意它和"权重退化"（少数粒子占去全部权重）不是一回事：权重退化靠重采样来治，粒子贫化却是重采样造成的。

    FIG 2 把每一代粒子画成一行，从上往下是时间。起初粒子均匀铺在 \([0,10]\) 上，没有运动、没有观测，只做重采样，看看有多少个不同的祖先能活下来。

    Resampling has a side effect: each time we sample with replacement, high-weight particles are copied and others vanish, so the set contains fewer and fewer distinct ancestors. Even with no new information at all (all weights equal), resampling at every step loses diversity generation after generation, like "genetic drift", until all particles are copies of one ancestor. This is particle depletion (also called sample impoverishment). Do not confuse it with weight degeneracy (a few particles carrying all the weight): resampling cures weight degeneracy, but it causes depletion.

    FIG 2 draws each generation as one row, with time running downward. Particles start spread uniformly over \([0,10]\), with no motion and no measurement, only resampling. How many distinct ancestors survive?

    FIG 2祖先的消失：多样性随代数下降Vanishing ancestors: diversity falls with the generations
    每一行是一代粒子（纵轴向下是时间），每个点是一个粒子，画在它最初祖先的位置上，相邻的祖先颜色不同，所以一列同色的点就是同一个祖先的后代。右侧曲线是"不同祖先数 / N"。"总是重采样"每代都做多项式重采样；"仅当 N_eff < N/2"只在权重严重不均时才重采样。"弱观测"模式每代加一次很弱的观测（一个宽高斯），权重略不均匀。 Each row is one generation (time runs downward) and each dot is a particle drawn at its original ancestor's position, with neighbouring ancestors in different colours, so a column of same-coloured dots is one ancestor's descendants. The curve on the right is "distinct ancestors / N". "Always resample" does multinomial resampling every generation; "Only if N_eff < N/2" resamples only when the weights get badly uneven. "Weak measurement" mode adds one very weak measurement (a wide Gaussian) each generation so the weights are mildly uneven.
    试一试Try this
    • 取 N = 50，"总是重采样"、无观测，跑 60 代：看看还剩几个不同的祖先（通常不到 10 个）。With N = 50, "Always resample" and no measurement, run 60 generations: how many distinct ancestors remain (usually fewer than 10)?
    • 切换到"仅当 N_eff < N/2"，同样设置再跑 60 代：多样性保住了吗？为什么？Switch to "Only if N_eff < N/2" and run 60 generations again: is diversity preserved? Why?
    一个反直觉的事实A counter-intuitive fact

    无信息时，"总是重采样"让粒子群像一个无记忆的随机游走：期望 \(O(N)\) 代之内，所有粒子就会合并成同一祖先。这意味着：重采样是必须付出的代价（它让计算资源别浪费在低权重粒子上），但是不该每一步都做。常用的规则是只在有效粒子数 \(N_{\mathrm{eff}}=1/\sum_i (w^{[i]})^2\) 低于 \(N/2\) 时才重采样；系统重采样（等间距指针）方差比多项式重采样小，而且权重全相等时一个粒子都不会丢。

    With no information, "always resample" makes the cloud a memoryless random walk: within about \(O(N)\) generations all particles merge into one ancestor. So resampling is a price you have to pay (it keeps compute from being wasted on low-weight particles) but should not be done at every step. A common rule resamples only when the effective particle count \(N_{\mathrm{eff}}=1/\sum_i (w^{[i]})^2\) drops below \(N/2\); systematic resampling (equally spaced pointers) has lower variance than multinomial resampling and loses no particle at all when the weights are equal.

    4再看数学The math

    提示：悬停下面带底色的符号，FIG 1 与 FIG 3 中对应的元素会高亮。Tip: hover the shaded symbols below and the matching element of FIG 1 and FIG 3 lights up.

    算法The algorithm

    信念用 \(N\) 个带权样本表示：\(\mathrm{bel}(x_k)\approx\sum_{i=1}^N w_k^{[i]}\,\delta\big(x_k - x_k^{[i]}\big)\)。每个时刻：

    The belief is represented by \(N\) weighted samples: \(\mathrm{bel}(x_k)\approx\sum_{i=1}^N w_k^{[i]}\,\delta\big(x_k - x_k^{[i]}\big)\). At every time step:

    $$ \text{1. predict:}\quad \hl{part}{x_k^{[i]}} \sim p\big(x_k \mid x_{k-1}^{[i]},\,u_k\big) $$ $$ \text{2. weight:}\quad \hl{wt}{w_k^{[i]}} \;\propto\; w_{k-1}^{[i]}\;p\big(z_k \mid x_k^{[i]}\big),\qquad \sum_i w_k^{[i]}=1 $$ $$ \text{3. resample (if } N_{\mathrm{eff}}=1\Big/\textstyle\sum_i (w_k^{[i]})^2 < \tfrac N2 \text{):}\quad \text{draw } N \text{ indices } \propto w_k^{[i]};\;\; w_k^{[i]}\leftarrow 1/N $$

    这个版本叫自举粒子滤波 (bootstrap filter)，是最简单的一种。

    This version is called the bootstrap particle filter, the simplest one.

    为什么权重是 \(p(z\mid x)\)？重要性采样的视角Why is the weight \(p(z\mid x)\)? The importance-sampling view

    我们想从 \(\mathrm{bel}(x_k)\propto p(z_k\mid x_k)\int p(x_k\mid x_{k-1},u_k)\,\mathrm{bel}(x_{k-1})\,dx_{k-1}\) 里抽样，它直接抽不出来。但"预测"步骤恰好提供了一个容易抽的提议分布：先按旧信念抽 \(x_{k-1}\)，再按运动模型抽 \(x_k\)，得到的分布就是 \(\mathrm{bel}^-(x_k)\)。按 4.9 节的重要性采样，权重 = 目标 ÷ 提议：

    We want samples from \(\mathrm{bel}(x_k)\propto p(z_k\mid x_k)\int p(x_k\mid x_{k-1},u_k)\,\mathrm{bel}(x_{k-1})\,dx_{k-1}\), which cannot be sampled directly. But the predict step happens to provide an easy proposal: draw \(x_{k-1}\) from the old belief and then \(x_k\) from the motion model, which gives exactly \(\mathrm{bel}^-(x_k)\). By the importance sampling of Lesson 4.9, weight = target ÷ proposal:

    $$ w \;=\; \frac{\text{target}}{\text{proposal}} \;=\; \frac{p(z_k\mid x_k)\,\mathrm{bel}^-(x_k)}{\mathrm{bel}^-(x_k)} \;=\; \hl{wt}{p(z_k\mid x_k)}. $$

    于是"加权"的就是传感器模型。重采样则是把一组带权样本转成等权样本，防止权重退化（少数粒子占去全部权重）。当 \(N\to\infty\)，估计收敛到真实后验，误差按 \(1/\sqrt N\) 下降。

    So what we weight by is the sensor model. Resampling converts a weighted sample set into an equally weighted one, to prevent weight degeneracy (a few particles carrying all the weight). As \(N\to\infty\) the estimate converges to the true posterior with error shrinking like \(1/\sqrt N\).

    系统重采样Systematic resampling

    取一个随机数 \(u_0\sim\mathrm{U}(0,1/N)\)，令指针 \(u_j=u_0+j/N\)（\(j=0,\dots,N-1\)），选中满足 \(\sum_{i'\lt i}w^{[i']}\lt u_j\le\sum_{i'\le i}w^{[i']}\) 的那个粒子 \(i\)。只用一个随机数，复杂度 \(O(N)\)，粒子 \(i\) 被复制的次数是 \(\lfloor Nw^{[i]}\rfloor\) 或 \(\lceil Nw^{[i]}\rceil\)，比 \(N\) 次独立抽样（多项式重采样，次数服从二项分布）的方差小得多。

    Draw one random number \(u_0\sim\mathrm{U}(0,1/N)\) and set pointers \(u_j=u_0+j/N\) (\(j=0,\dots,N-1\)); select the particle \(i\) with \(\sum_{i'\lt i}w^{[i']}\lt u_j\le\sum_{i'\le i}w^{[i']}\). It uses one random number, costs \(O(N)\), and copies particle \(i\) either \(\lfloor Nw^{[i]}\rfloor\) or \(\lceil Nw^{[i]}\rceil\) times, a much lower variance than \(N\) independent draws (multinomial resampling, whose copy counts are binomial).

    从粒子读出估计Reading an estimate from the particles

    位置的估计是加权平均 \(\hat{\mathbf p}=\sum_i w^{[i]}\mathbf p^{[i]}\)。角度不能直接平均：358° 和 2° 的平均应是 0°，不是 180°。要用圆周平均 \(\hat\theta=\operatorname{atan2}\big(\sum_i w^{[i]}\sin\theta^{[i]},\,\sum_i w^{[i]}\cos\theta^{[i]}\big)\)。更要警惕：多峰分布的均值可能落在没有任何粒子的地方（FIG 3 里的"镜像"就是例子），这时应对粒子聚类，取最大的簇，或者干脆把整个粒子集交给下游。

    The position estimate is the weighted mean \(\hat{\mathbf p}=\sum_i w^{[i]}\mathbf p^{[i]}\). Angles cannot be averaged directly: the mean of 358° and 2° should be 0°, not 180°. Use the circular mean \(\hat\theta=\operatorname{atan2}\big(\sum_i w^{[i]}\sin\theta^{[i]},\,\sum_i w^{[i]}\cos\theta^{[i]}\big)\). More importantly, the mean of a multimodal belief can land where there is no particle at all (the "mirror" in FIG 3 is an example); then cluster the particles and take the biggest cluster, or hand the whole set downstream.

    5机器人中的它：对称房间里的蒙特卡洛定位In robotics: Monte Carlo localization in a symmetric room

    一台带 8 束测距的机器人在 10 m × 6 m 的房间里。房间几乎中心对称：两个方块、两段隔墙关于房间中心互为镜像，只有右下角那个小方块是"独一份"。机器人知道地图，但一开始不知道自己在哪、朝哪：全局定位，粒子均匀铺满整张地图的位置和朝向。

    点"前进 / 左转 / 右转"，或打开"自动巡航"，看粒子怎样聚拢。大多数时候你会看到两团：一团在真实位置，一团在镜像位置，直到机器人看见那个独一份的小方块才能排除。接着拖动机器人把它"绑架"到别处，看粒子滤波能不能醒悟；再打开"随机注入"救它。

    A robot with 8 range beams sits in a 10 m × 6 m room. The room is almost point-symmetric: two blocks and two partition walls mirror each other through the room's center, and only the small block at the lower right is unique. The robot knows the map but initially not where or which way it is: global localization, with particles spread uniformly over all positions and headings.

    Press "Forward / Left / Right", or switch on "Auto drive", and watch the particles gather. Most of the time you will see two clusters, one at the true pose and one at its mirror image, until the robot catches sight of the unique small block. Then drag the robot to kidnap it and see whether the filter wakes up; switch on "random injection" to rescue it.

    FIG 3蒙特卡洛定位 (MCL)：全局定位、镜像歧义与被绑架Monte Carlo localization: global localization, mirror ambiguity and kidnapping
    黄色小点是粒子（带朝向刻度），带朝向短线的大圆点是真实机器人（可拖动，等于绑架），紫线是它的 8 束测距，青色是加权平均得到的估计位姿，红线是估计与真值的误差。灰色是已知地图，右下角的小方块打破了对称。"随机注入"每次重采样时把 5% 的粒子换成随机位姿。 Amber dots are particles (with heading ticks), the large dot with a heading stub is the true robot (draggable, which kidnaps it), purple lines are its 8 range beams, cyan is the weighted-mean pose estimate, and the red line is the error between estimate and truth. Grey is the known map; the small block at the lower right breaks the symmetry. "Random injection" replaces 5% of the particles by random poses at each resample.
    试一试Try this
    • 镜像歧义：重置后驱动机器人，等到粒子分成两团：真实位姿和它的点对称镜像各占 25% 以上。这时加权平均落在哪里？Mirror ambiguity: after Reset, drive the robot until the particles split into two clusters: the true pose and its point-symmetric mirror each hold 25%+. Where does the weighted mean land?
    • 全局定位成功：让粒子的位置散布 < 0.5 m，估计误差 < 0.4 m。（提示：绕到右下角，让机器人看到那个独一份的小方块。）Global localization succeeded: get the particle spread below 0.5 m and the estimate error below 0.4 m. (Hint: head toward the lower right and let the robot see the unique small block.)
    • 绑架并救援：定位成功后把机器人拖到 3 m 以外，打开"随机注入"，让滤波器重新收敛（误差 < 0.5 m）。Kidnap and rescue: after localizing, drag the robot 3+ m away, switch on "random injection", and let the filter reconverge (error below 0.5 m).
    • 粒子不够：把粒子数降到 40 以下并重置，驱动 8 步以上：滤波器常常自信地收敛到错误的位置（误差 > 1.5 m 而散布 < 0.4 m）。Not enough particles: drop the particle count below 40, reset and drive 8+ steps: the filter often converges, confidently, to a wrong place (error above 1.5 m with spread below 0.4 m).
    真实系统里In real systems

    ROS 导航栈里的 AMCL 就是这样的滤波器：状态 \((x,y,\theta)\)，运动模型来自里程计，观测模型来自激光对占据栅格地图的匹配（似然场模型）。它用 KLD 采样自适应地决定粒子数（粒子占据的状态空间格子越多，就用越多粒子），并在短期平均似然相对长期平均似然突然下降时注入随机粒子（这就是 FIG 3 的"随机注入"，对应增广 MCL）。仓库 AGV 常常遇到"整排货架长得一样"的歧义，靠多峰信念和多走几步来消除。

    AMCL in the ROS navigation stack is exactly such a filter: state \((x,y,\theta)\), motion model from odometry, observation model from laser scans matched against an occupancy-grid map (the likelihood-field model). It adapts the number of particles with KLD sampling (the more state-space bins the particles occupy, the more particles it uses), and injects random particles when the short-term average likelihood drops sharply below the long-term one (the "random injection" of FIG 3, known as augmented MCL). Warehouse AGVs often face rows of identical shelves, resolved by keeping a multimodal belief and driving a few more steps.

    6常见误区Common pitfalls

    误区 1：粒子数太少Pitfall 1: too few particles

    全局定位时，粒子要足够密才能"碰巧"有一个落在真值附近。粒子太少时，真值附近一个粒子也没有，别的粒子在错误的地方越聚越紧，滤波器自信地走向错误（FIG 3 的最后一个任务）。需要的粒子数随状态维度和地图大小快速增长；跟踪（已知大致位置）时几十到几百个就够，全局定位可能要几千个。

    In global localization the particles must be dense enough that one happens to land near the truth. With too few, nothing sits near the truth while the others cluster tighter and tighter in the wrong place, and the filter confidently marches off (the last task of FIG 3). The number needed grows quickly with state dimension and map size: tens to hundreds suffice for tracking (rough position known), global localization may need thousands.

    误区 2：传感器太"准"，权重一下塌了Pitfall 2: an over-sharp sensor model collapses the weights

    如果似然很尖（传感器噪声设得很小，或者束数很多），几乎所有粒子的权重都接近 0，只有一两个例外，\(N_{\mathrm{eff}}\) 塌到 1 到 2，一次更新就丢掉绝大多数粒子。工程办法是：故意把传感器噪声设得比实际大（相当于对似然做"退火"），只取一部分束，或给似然加一个均匀的底噪混合（对付野值）。

    If the likelihood is very sharp (tiny assumed sensor noise, or many beams), nearly all particles get weights near 0 with one or two exceptions, \(N_{\mathrm{eff}}\) collapses to 1 or 2, and one update throws away most of the particles. The engineering remedies: deliberately assume more sensor noise than there really is (tempering the likelihood), use only a subset of the beams, or mix a uniform floor into the likelihood (which also handles outliers).

    误区 3：绑架后没有注入随机粒子Pitfall 3: no random injection after a kidnapping

    跟 4.7 节的"零概率"同一个病：粒子群已经收缩到旧位置，新位置周围没有任何粒子，重采样不会凭空造出来。要么定期混入一小撮全图随机粒子，要么监测平均似然、在它突然下降时扩散粒子。代价是注入过多会让稳定的估计变得抖动，要调。

    Same disease as the "zero probability" of Lesson 4.7: the cloud has contracted around the old position, nothing sits near the new one, and resampling cannot create particles out of thin air. Either mix in a small share of map-wide random particles regularly, or monitor the mean likelihood and spread the particles when it suddenly drops. The price: too much injection makes a stable estimate jittery, so tune it.

    误区 4：用加权平均当"答案"，且角度直接平均Pitfall 4: the weighted mean as "the answer", with angles averaged naively

    多峰时的平均值可能是一个没有任何粒子、甚至在墙里的点；角度用算术平均会在 ±180° 附近翻车。读估计时，先看分布（聚类或直方图），再决定怎么报告。

    The mean of a multimodal cloud can be a point with no particles, even inside a wall; the arithmetic mean of angles breaks near ±180°. Look at the distribution (clusters or histogram) before deciding how to report an estimate.

    误区 5：每一步都重采样Pitfall 5: resampling at every step

    FIG 2 已经演示：重采样会损失多样性。用 \(N_{\mathrm{eff}}\) 阈值触发，并且用系统重采样代替多项式重采样。

    FIG 2 showed it: resampling costs diversity. Trigger it by an \(N_{\mathrm{eff}}\) threshold and prefer systematic over multinomial resampling.

    7小练习Check yourself

    1. 四个粒子的归一化权重为 \((0.5,\,0.5,\,0,\,0)\)。有效粒子数 \(N_{\mathrm{eff}}\) 是多少？1. Four particles have normalized weights \((0.5,\,0.5,\,0,\,0)\). What is the effective particle count \(N_{\mathrm{eff}}\)?
    \(N_{\mathrm{eff}}=1/\sum w^2=1/(0.25+0.25)=2\)：只有两个粒子在起作用，另外两个权重为 0，等于白占位置。\(N_{\mathrm{eff}}=1/\sum w^2=1/(0.25+0.25)=2\): only two particles are doing any work; the other two have weight 0 and are dead weight.
    2. 用 \(N=10\) 个粒子做多项式重采样，某个粒子的归一化权重是 0.4。它的期望被复制几份？2. With \(N=10\) particles and multinomial resampling, a particle has normalized weight 0.4. How many copies of it do you expect?
    每次抽样选中它的概率是 0.4，抽 10 次，期望 \(10\times0.4=4\) 份（多项式重采样下实际份数服从二项分布，可能是 2 或 6）。系统重采样会把它稳定地复制 4 份。Each draw picks it with probability 0.4, and there are 10 draws, so the expectation is \(10\times0.4=4\) copies (with multinomial resampling the actual count is binomial and may be 2 or 6). Systematic resampling copies it exactly 4 times.
    3. 机器人被"绑架"后，一个已经收敛的粒子滤波为什么恢复不过来？3. After a kidnapping, why does a particle filter that had converged fail to recover?
    粒子滤波的"搜索空间"只有现有的粒子。新位置周围没有粒子时，没有任何一个能得到高权重，权重也只能在旧位置附近的错误粒子间分配。对策是注入随机粒子，或在平均似然骤降时重新全局撒粒子。The filter's "search space" consists of its current particles only. With none near the new position, none can win a high weight, and the weights get shared among wrong particles near the old position. The remedy is to inject random particles, or re-scatter globally when the mean likelihood drops sharply.
    4. 一半粒子（权重各相等）在 \(x=2\)，另一半在 \(x=8\)。粒子的加权平均是 5。这个 5 的含义是？4. Half the particles (equal weights) sit at \(x=2\), the other half at \(x=8\). The weighted mean is 5. What does that 5 mean?
    均值只在单峰时代表"位置"。双峰时它落在空地上，概率密度为 0。要么聚类后分别报告，要么用最大簇的均值，要么把整个粒子集交给下游。The mean represents "the position" only for a unimodal belief. For two peaks it lands in empty space where the density is 0. Cluster and report each, use the biggest cluster's mean, or pass the whole set downstream.

    8延伸阅读Further reading

    • S. Thrun, W. Burgard & D. Fox, Probabilistic Robotics, MIT Press, 2005, Ch. 4.3 & Ch. 8.3 — 粒子滤波与蒙特卡洛定位（含 KLD 采样与随机注入）particle filters and Monte Carlo localization (including KLD sampling and random injection)
    • F. Dellaert, D. Fox, W. Burgard & S. Thrun, "Monte Carlo Localization for Mobile Robots", ICRA, 1999 — MCL 的原始论文the original MCL paper
    • N. J. Gordon, D. J. Salmond & A. F. M. Smith, "Novel approach to nonlinear/non-Gaussian Bayesian state estimation", IEE Proceedings F, 1993 — 自举粒子滤波the bootstrap particle filter
    • T. D. Barfoot, State Estimation for Robotics, Cambridge University Press, 2017, Ch. 4 — 非线性非高斯估计，包括粒子滤波nonlinear non-Gaussian estimation, including particle filters
    • R. Douc & O. Cappé, "Comparison of resampling schemes for particle filtering", ISPA, 2005 — 多项式、分层与系统重采样的比较a comparison of multinomial, stratified and systematic resampling