【滤波】粒子滤波

%matplotlib inline

#format the book
import book_format
book_format.set_style()

动机

现在的问题是：我们要跟踪移动中的物体，也许这些物体是战斗机和导弹，或者是在场地里打球的人。但这都没有太大差别。那么，我们所学的滤波器中哪些可以处理这个问题？不幸的是，没有一个是理想的。让我们考虑一下这个问题的特点。

多模态：我们希望同时跟踪零个、一个甚至多个对象;
遮挡：一个对象可以遮挡另一个对象，从而导致对多个对象只进行一次观测；
非线性行为：例如飞机受到风的冲击，球以抛物线运动，人与人相互碰撞；
非线性观测：雷达提供到物体的距离，将其转换为 $(x, y, z)$ 坐标需要一个平方根，这是非线性的；
非高斯噪声：当物体在背景上移动时，计算机视觉可能会将部分背景误认为是物体；
连续：物体的位置和速度（即状态空间）随时间连续平稳变化；
多元：可能同时需要跟踪几个属性，如位置，速度，转弯率等；
未知过程模型：我们可能不知道系统的过程模型。

我们学习的滤波器都不能很好地处理所有这些约束：

离散贝叶斯滤波器：它具有大多数属性。它是多模态的，可以处理非线性观测，并且可以扩展到处理非线性行为。然而，它是离散且单变量的；
卡尔曼滤波器：KF滤波器可以进行高斯噪声的单峰线性系统的最优估计。这些对我们的问题都不适用；
无迹卡尔曼滤波：UKF处理非线性，连续，多变量问题。然而，它不是多模态的，也不能处理遮挡。它可以处理适度的非高斯噪声，但不能很好地处理强非高斯分布或强非线性的问题；
扩展卡尔曼滤波：EKF与UKF具有相同的优点和局限性，只是它对强非线性和非高斯噪声更加敏感。

蒙特卡罗采样

在UKF中，绘制了一个类似的图来说明非线性系统对澳洲人的影响：

import kf_book.pf_internal as pf_internal

pf_internal.plot_monte_carlo_ukf()

在这里插入图片描述

左图显示了基于高斯分布的3000个点的位置：

$\mu = \begin{bmatrix} 0 \\ 0 \end{bmatrix}, \Sigma =\begin{bmatrix} 21 & 15 \\ 15 & 40\end{bmatrix}$

右图显示了这些点经状态转移方程后的位置：

$x = x + y$

$y = 0.1x^{2} + y^{2}$

使用有限个随机采样点来计算结果称为蒙特卡罗方法。这个方法的思路很简单：生成足够多的点来获取问题的代表性样本，然后在建模的系统中运行这些点，最后计算转换后的点的结果。

简而言之，这就是粒子滤波的作用。即生成足够数量的粒子，其中每个粒子代表一个可能的系统状态。最终我们利用粒子的加权统计量，提取估计的状态。

通用粒子滤波算法

随机产生一堆粒子：粒子可以有位置、朝向或者其他需要估计的状态量。每个粒子都有一个权重，用来表示它与系统实际状态匹配的可能性。用相同的权重来初始化每个粒子；
预测粒子的下一个状态：根据预测真实系统的行为来移动粒子；
更新权重：根据观测值更新粒子的权重。与观测值非常匹配的粒子的权重，高于那些与观测值不是非常匹配的粒子；
重采样：丢弃极不可能的粒子，并用更可能的粒子的副本替换它们；
计算估计：计算粒子集合的加权平均值和协方差，以获得状态的估计。

尽管这种天真的算法，在实际应用中有需要克服的困难，但大致思路就是这样。让我们先来看一个例子，假设为机器人的定位问题编写一个粒子滤波器。该机器人有转向和速度控制输入，同时有传感器观测到可见地标的距离。当然，传感器和控制机构都有噪声，现需要对机器人的位置进行跟踪。

在这里，我运行粒子滤波并绘制粒子的位置。左边的图是一次滤波循环后的图，右边的图是10次滤波循环后的图。红色的X表示机器人的实际位置，绿色的圆点表示粒子的估计位置。

pf_internal.show_two_pf_plots()

在这里插入图片描述

如果你在浏览器中查看，下面的动图将显示整个10次滤波循环的过程：

在这里插入图片描述

在第一次滤波循环之后，粒子基本上仍随机地散布在地图上，但是你可以看到一些粒子已经开始聚集在机器人的位置附近。粒子的位置估计还算比较接近机器人的位置，这是因为每个粒子的权重是基于它与观测值的匹配程度。简单一点说就是，机器人靠近 $(1, 1)$ ，所以靠近 $(1, 1)$ 的粒子会有较高的权重，因为它们与观测值非常匹配。远离机器人的粒子与观测值不匹配，因此权重较低。由于估计的位置是粒子位置的加权平均值，机器人附近的粒子对计算的贡献更大，因此估计是比较准确的。

几次滤波循环之后，可以看到几乎所有的粒子都聚集在机器人周围。这是因为重采样步骤。重采样丢弃非常不可能的粒子（非常低的权重），并用权重更高的粒子替换它们。

我既没有说明为什么这样做，也没有解释粒子权重的计算方法和重采样的算法，但它应该是直观的：制造一堆随机粒子，移动它们并让它们跟随机器人，根据它们与观测值的匹配程度对它们设置权重。同时，我们还通过一些策略，只让权重不是很低的的粒子存活。

蒙特卡罗概率分布

假设我们想知道区间 $\pi ]$ 内，曲线 $y = e^{sin(x)}$ 下的面积，面积可以用定积分 $\int_{0}^{\pi } e^{sin(x)} dx$ 计算。作为练习，你可以先手动算一下，别着急往下看。

如果你明智的话，你就不应该计算它，这是因为 $e^{sin(x)}$ 无法积分。世界上充满了我们无法积分的式子。

然而，如果使用蒙特卡罗技术，积分将会是微不足道的事情。例如：想要计算曲线下区域的面积，首先创建包含所求区域的边界框。在框内生成随机定位的点，并计算落在曲线下的点与总点数的比率。例如，如果 $40\%$ 的点位于曲线下，且边界框的面积为1，则曲线下的面积约为0.4。当点的个数趋向于无限时，你可以达到任意精度。在实践中，几千个点就可以给你一个相对准确的结果。

你可以用这种方法对任意难度的函数进行数值积分，包括不可积和非连续函数。这项技术是由Los Alamos国家实验室的Stanley Ulam发明的，目的是为了让他能够对无法在纸上解决的核反应进行计算。

让我们通过圆的面积来计算 $\pi$ 。我们定义一个半径为1的圆，并将它包含在一个正方形内。正方形的边长是2，所以它的面积是4。我们在正方形中生成一组均匀分布的随机点，并计算落在圆内的点的个数。圆的面积计算为正方形的面积乘以圆内点与总点数之比。最后，我们依靠圆的面积公式可以对 $\pi$ 的大小进行估算 $\pi = A / r^{2}$ ：

我们从创建点开始：

N = 20000
pts = uniform(-1, 1, (N, 2))

如果点与圆心的距离小于或等于半径，则该点位于圆内。我们用numpy.linalg.norm计算距离，它计算向量的大小。由于向量从 $(0, 0)$ 开始，调用范数将计算点与原点的距离。

dist = np.linalg.norm(pts, axis=1)

接下来，我们计算这些距离中的哪一个符合标准。如果满足条件 $d i s t <= 1$ ，则此代码返回包含True的bool数组：

in_circle = dist <= 1

剩下的就是计算圆内的点，计算圆周率，并绘制结果。我把它们都放在一个单元格里，这样你就可以实验 $N$ 的替代值，即点的数量。

import matplotlib.pyplot as plt
import numpy as np
from numpy.random import uniform 

N = 20000  # number of points
radius = 1.
area = (2*radius)**2

pts = uniform(-1, 1, (N, 2))

# distance from (0,0) 
dist = np.linalg.norm(pts, axis=1)
in_circle = dist <= 1

pts_in_circle = np.count_nonzero(in_circle)
pi = 4 * (pts_in_circle / N)

# plot results
plt.scatter(pts[in_circle,0], pts[in_circle,1], 
            marker=',', edgecolor='k', s=1)
plt.scatter(pts[~in_circle,0], pts[~in_circle,1], 
            marker=',', edgecolor='r', s=1)
plt.axis('equal')

print(f'mean pi(N={N})= {pi:.4f}')
print(f'err  pi(N={N})= {np.pi-pi:.4f}')

mean pi(N=20000)= 3.1684
err  pi(N=20000)= -0.0268

在这里插入图片描述

这种方法使我们认识到，我们可以使用蒙特卡罗来计算任何概率分布的概率密度。例如，假设我们有这个高斯分布：

from filterpy.stats import plot_gaussian_pdf

plot_gaussian_pdf(mean=2, variance=3)

在这里插入图片描述

概率密度函数（pdf）给出了随机值介于两个值之间的概率。例如，我们可能想知道上图中 $x$ 在 $0$ 和 $2$ 之间的概率。这是一个连续的函数，所以我们需要用积分来求曲线下的面积，因为面积等于出现该范围内值的概率。

$\le X \le b] = \int_{a}^{b} f_{x} (x) dx$

对于高斯分布，计算这种积分是很容易的，但现实情况并不总是这么容易。例如，下图显示了一个概率分布，连解析地描述该曲线都没有办法，更不用说积分曲线了。

pf_internal.plot_random_pd()

在这里插入图片描述

我们可以用蒙特卡罗方法来计算任何积分。pdf是用积分计算的，因此我们可以用蒙特卡罗法计算这条曲线的pdf。

粒子滤波

所有的这些都把我们带到了粒子滤波。为了保持一致性，我将使用EKF和UKF章节中的机器人定位问题。在这个问题中，我们追踪一个机器人的状态，同时它有一个传感器，可以观测到已知地标的距离和方位。

粒子滤波是一系列算法。我正在介绍的是其中的一种具体形式，它理解起来很直观，并且与我们研究的问题有关。初学的话，可能其中的一些步骤看起来有点神奇，因为我还没有提供一个完整的解释。这将在本文的后面介绍。

根据上文的讨论，我们首先创建几千个粒子。每个粒子都有一个状态量，表示机器人在场景中的可能位置，当然也可以有可能的方向和速度。假设我们并不知道机器人的初始位置，那么只好在整个场景中均匀地散列粒子。如果你认为所有的粒子都代表概率分布，那么粒子越多的位置代表概率越高，粒子越少的位置代表概率越低。如果在某个特定位置附近有一大团粒子，那就意味着我们更确定机器人就在那里。

每个粒子都需要一个权重——理想情况下，它代表机器人真实位置的概率。这个概率基本是不可计算的，但是我们只要求它和真实的那个概率值成比例，这是可计算的。在初始化时，我们没有理由偏爱一个粒子而不是另一个粒子，因此我们为每个粒子指定了一个权重 $1/ N$ 。我们使用 $1/ N$ ，使所有粒子的权重之和等于1。

粒子和权重的组合形成了问题的概率分布。回想一下离散贝叶斯章节，在那一章中，我们将走廊中的位置建模为离散的、统一的间隔。两者非常相似，只是在这里粒子随机分布在一个连续的空间，而不是局限于离散的空间。在这个问题中，机器人可以在一个任意尺寸的平面上运动，右下角为 $(0, 0)$ 。

为了跟踪我们的机器人，我们需要保持 $x$ ， $y$ 和朝向的状态。我们将 $N$ 个粒子存储在一个 $(N, 3)$ 形状的数组中。这三列依次包含 $x$ 、 $y$ 和朝向。

如果你是被动地跟踪某个东西（没有控制输入），那么你需要在状态量中包含速度。然而更高的维数，需要指数级别更多的粒子来形成一个好的估计，所以我们总是尽量减少状态量的数量。

此代码在区域上创建粒子的均匀高斯分布：

from numpy.random import uniform

def create_uniform_particles(x_range, y_range, hdg_range, N):
    particles = np.empty((N, 3))
    particles[:, 0] = uniform(x_range[0], x_range[1], size=N)
    particles[:, 1] = uniform(y_range[0], y_range[1], size=N)
    particles[:, 2] = uniform(hdg_range[0], hdg_range[1], size=N)
    particles[:, 2] %= 2 * np.pi
    return particles

def create_gaussian_particles(mean, std, N):
    particles = np.empty((N, 3))
    particles[:, 0] = mean[0] + (randn(N) * std[0])
    particles[:, 1] = mean[1] + (randn(N) * std[1])
    particles[:, 2] = mean[2] + (randn(N) * std[2])
    particles[:, 2] %= 2 * np.pi
    return particles

例如：

create_uniform_particles((0, 1), (0, 1), (0, np.pi * 2), 4)

array([[0.772, 0.336, 4.171],
       [0.333, 0.34 , 4.319],
       [0.6  , 0.274, 5.02 ],
       [0.054, 0.022, 5.034]])

预测步

贝叶斯算法中的预测步使用过程模型来更新状态量。我们如何处理粒子呢？每个粒子代表机器人的一个可能位置。假设我们向机器人发送一个命令，使其移动 $0.1 m$ ，同时转动 $0.007 R a d$ 。我们可以按照这个命令来移动每个粒子。如果我们这样做，很快就会遇到问题。机器人的控制并不完美，所以它不会完全按照指令移动。因此，我们需要在粒子的运动中加入噪声，以便有合理的机会捕捉到机器人的实际运动。如果你不对系统中的不确定性进行建模，粒子滤波将无法正确地对机器人位置的概率分布进行建模。

def predict(particles, u, std, dt=1.):
    """ move according to control input u (heading change, velocity)
    with noise Q (std heading change, std velocity)`"""
    N = len(particles)
    # update heading
    particles[:, 2] += u[0] + (randn(N) * std[0])
    particles[:, 2] %= 2 * np.pi

    # move in the (noisy) commanded direction
    dist = (u[1] * dt) + (randn(N) * std[1])
    particles[:, 0] += np.cos(particles[:, 2]) * dist
    particles[:, 1] += np.sin(particles[:, 2]) * dist

更新步

下一步我们得到一组观测值，即当前视图中的每个地标。这些观测值应该如何改变粒子所模拟的概率分布？

回想离散贝叶斯一章。在那一章中，我们将走廊中的位置建模为离散的、均匀分布的。我们给每个位置分配了一个概率，我们称之为先验。当一个新的观测值出现时，我们将当前位置的可能性（先验）乘以观测值与该位置匹配的可能性（似然）：

def update(likelihood, prior):
    posterior = prior * likelihood
    return normalize(posterior)

这是贝叶斯定理的一个实现：

$\frac{P(z|x) × P(x)}{P(z)} = \frac{likelihood × prior}{normalization}$

我们对粒子也是这样。每个粒子都有一个权重，用来估计它的位置与观测值之间的匹配程度。将权重标准化，使之和为一，将其转化为概率分布。通常情况下，距离机器人较近的粒子比距离机器人较远的粒子的权重要大。

def update(particles, weights, z, R, landmarks):
    for i, landmark in enumerate(landmarks):
        distance = np.linalg.norm(particles[:, 0:2] - landmark, axis=1)
        weights *= scipy.stats.norm(distance, R).pdf(z[i])

    weights += 1.e-300      # avoid round-off to zero
    weights /= sum(weights) # normalize

在文献中，这部分算法被称为序贯重要性抽样，或SIS。权重的方程称为重要性密度。我将在下面的内容中，给出相关的理论基础。但现在我希望这是直观的。如果我们根据粒子与观测值的匹配程度对粒子进行加权，那么在加入观测值后，它们可能是系统概率分布的一个很好的样本。理论证明确实如此，而权重是贝叶斯理论中的似然。不同的问题需要以稍微不同的方式来解决这一步，但这是总体思路。

计算状态估计

在大多数情况下，你希望知道每次更新后的状态估计，但粒子滤波器中包含一组粒子。假设我们正在跟踪一个物体（假设它是单峰的），我们可以将状态估计计算成粒子的加权值之和。

$\mu = \frac{1}{N} \sum_{i = 1}^{N} w^{i} x^{i}$

这里我采用符号 $x^{i}$ 来表示 $i^{th}$ 粒子。之所以使用上标，是因为我们经常需要用下标，来表示时间序列上的 $k^{th}$ 或 $k+1^{th}$ 粒子。

此函数计算粒子的均值和方差：

def estimate(particles, weights):
    """returns mean and variance of the weighted particles"""
    pos = particles[:, 0:2]
    mean = np.average(pos, weights=weights, axis=0)
    var  = np.average((pos - mean)**2, weights=weights, axis=0)
    return mean, var

如果我们在一个 $1\times 1$ 的正方形上用相等的权值建立一个均匀分布的点，我们就得到了一个正方形中心 $(0.5, 0.5)$ 附近的均值和一个很小的方差。

particles = create_uniform_particles((0,1), (0,1), (0, 5), 1000)
weights = np.array([.25]*1000)
estimate(particles, weights)

(array([0.494, 0.514]), array([0.083, 0.085]))

重采样

SIS算法存在退化问题。它从权重相等的均匀分布的粒子开始，到后来机器人附近可能只有少数粒子。当粒子滤波运行时，任何与观测值不匹配的粒子都将获得极低的权重。只有靠近机器人的粒子才会有可观的权重。可能会出现，我们有5000个粒子，但其中只有3个粒子点对状态估计有意义！这种情况下滤波器就退化了，通常可以通过某种形式的粒子重采样来解决。

权重很小的粒子不能有效地描述机器人的概率分布。重采样算法丢弃权重很低的粒子，用权重更高的新粒子代替。它通过复制权重相对较高的粒子来实现，这些粒子会被预测步骤中添加的微小噪声分散开来。这就产生了一组点，其中大部分粒子可以准确地表示概率分布。

重采样算法有很多种。现在让我们看一个最简单的：简单随机重采样，也称为多项式重采样。它从当前粒子集采样 $N$ 次，从样本中生成一组新粒子。选择任何给定粒子的概率应与其权重成正比。

我们用NumPy的cumsum()函数来实现这一点，cumsum()计算数组的累积和。也就是说，元素1是元素0和1的和，元素2是元素0、1和2的和，以此类推。然后我们生成0.0到1.0范围内的随机数，并进行二分搜索以找到与该数字最接近的权重：

def simple_resample(particles, weights):
    N = len(particles)
    cumulative_sum = np.cumsum(weights)
    cumulative_sum[-1] = 1. # avoid round-off error
    indexes = np.searchsorted(cumulative_sum, random(N))

    # resample according to indexes
    particles[:] = particles[indexes]
    weights.fill(1.0 / N)

我们不会在每次滤波循环中都采取重采样的操作。例如，如果没有收到新的观测结果，那就没有收到任何重采样可以受益的信息。我们可以通过使用有效 $N$ 来决定何时重采样，有效 $N$ 可以理解成对概率分布有意义的粒子数。有效 $N$ 的计算方式是：

$\hat{N} _{eff} = \frac{1}{\sum w^{2}}$

我们可以在Python中用：

def neff(weights):
    return 1. / np.sum(np.square(weights))

如果 $N e ff$ 低于某个阈值，则是需要进行重采样了。一个有用的阈值是 $N /2$ ，但这也要因问题而异。 $N e ff = N$ 也是可能的，这意味着粒子集已塌陷到一个点（每个点的权重相等）。理论上它可能并不能到达这一步，但如果真是这样的话，最好创造一个新的粒子分布，希望能产生更多多样性的粒子。如果你经常发生这种情况，可能需要增加粒子数，或者调整滤波器。我们稍后再谈。

SIR滤波器-完整示例

尽管还有更多的东西要学，但我们已经有足够的知识来实现一个完整的粒子滤波。我们将实现采样重要性重采样滤波器（SIR）。

我需要引入一种比上面给出的更复杂的重采样方法。FilterPy提供了几种重采样方法，我将在稍后来描述它们。它们获取一个权重数组，就将重采样选择的粒子的索引返回。因此，我们只需要编写一个函数，来对这些索引执行重采样操作即可：

def resample_from_index(particles, weights, indexes):
    particles[:] = particles[indexes]
    weights.resize(len(particles))
    weights.fill (1.0 / len(weights))

为了实现滤波器，我们需要创建粒子和地标。然后我们执行一个循环，依次调用predict()、update()、resampling()，然后用estimate()计算新的状态估计。

from filterpy.monte_carlo import systematic_resample
from numpy.linalg import norm
from numpy.random import randn
import scipy.stats

def run_pf1(N, iters=18, sensor_std_err=.1, 
            do_plot=True, plot_particles=False,
            xlim=(0, 20), ylim=(0, 20),
            initial_x=None):
    landmarks = np.array([[-1, 2], [5, 10], [12,14], [18,21]])
    NL = len(landmarks)
    
    plt.figure()
   
    # create particles and weights
    if initial_x is not None:
        particles = create_gaussian_particles(
            mean=initial_x, std=(5, 5, np.pi/4), N=N)
    else:
        particles = create_uniform_particles((0,20), (0,20), (0, 6.28), N)
    weights = np.ones(N) / N

    if plot_particles:
        alpha = .20
        if N > 5000:
            alpha *= np.sqrt(5000)/np.sqrt(N)           
        plt.scatter(particles[:, 0], particles[:, 1], 
                    alpha=alpha, color='g')
    
    xs = []
    robot_pos = np.array([0., 0.])
    for x in range(iters):
        robot_pos += (1, 1)

        # distance from robot to each landmark
        zs = (norm(landmarks - robot_pos, axis=1) + 
              (randn(NL) * sensor_std_err))

        # move diagonally forward to (x+1, x+1)
        predict(particles, u=(0.00, 1.414), std=(.2, .05))
        
        # incorporate measurements
        update(particles, weights, z=zs, R=sensor_std_err, 
               landmarks=landmarks)
        
        # resample if too few effective particles
        if neff(weights) < N/2:
            indexes = systematic_resample(weights)
            resample_from_index(particles, weights, indexes)
            assert np.allclose(weights, 1/N)
        mu, var = estimate(particles, weights)
        xs.append(mu)

        if plot_particles:
            plt.scatter(particles[:, 0], particles[:, 1], 
                        color='k', marker=',', s=1)
        p1 = plt.scatter(robot_pos[0], robot_pos[1], marker='+',
                         color='k', s=180, lw=3)
        p2 = plt.scatter(mu[0], mu[1], marker='s', color='r')
    
    xs = np.array(xs)
    #plt.plot(xs[:, 0], xs[:, 1])
    plt.legend([p1, p2], ['Actual', 'PF'], loc=4, numpoints=1)
    plt.xlim(*xlim)
    plt.ylim(*ylim)
    print('final position error, variance:\n\t', mu - np.array([iters, iters]), var)
    plt.show()

from numpy.random import seed
seed(2) 
run_pf1(N=5000, plot_particles=False)

final position error, variance:
	 [-0.106  0.106] [0.009 0.008]

在这里插入图片描述

代码看起来比较长，但其中大部分都用于初始化和打印，整个粒子滤波处理由以下几行组成：

# move diagonally forward to (x+1, x+1)
predict(particles, u=(0.00, 1.414), std=(.2, .05))

 # incorporate measurements
update(particles, weights, z=zs, R=sensor_std_err, 
       landmarks=landmarks)

# resample if too few effective particles
if neff(weights) < N/2:
    indexes = systematic_resample(weights)
    resample_from_index(particles, weights, indexes)

mu, var = estimate(particles, weights)

第一行代码预测粒子的位置，利用假设条件：机器人在直线上移动（ $u [0] == 0$ ），同时在 $x$ 轴和 $y$ 轴上移动1个单位（ $u [1] == 1.414$ ）。转弯误差的标准差为0.2，距离的标准差为0.05。当此调用返回时，粒子将全部向前移动，但权重不再正确，因为它们尚未更新。

下一行代码将观测值合并到滤波器中，但此时不会改变粒子的位置，只会改变权重。你可以回想一下，粒子的权重是根据它与传感器误差模型的高斯分布相匹配的概率来计算的。粒子离观测值距离越远，就越不可能是一个好的表现。

最后两行计算有效粒子数 $\hat{N} _{eff}$ 。如果它低于 $N /2$ ，我们将进行重采样，以确保我们的粒子是实际概率分布的良好表现。

现在让我们看看所有的粒子，以交互动画的方式看这种情况更有启发性。我将原始随机分布的点绘制成非常淡绿色的圆点，以帮助将它们与随后的滤波迭代区分开来。在滤波迭代的过程中，粒子是用黑色圆点绘制的。粒子的数量使得很难看到细节，所以我将滤波迭代次数限制为8次，这样我们可以放大并更仔细地观察。

seed(2)
run_pf1(N=5000, iters=8, plot_particles=True, 
        xlim=(0,8), ylim=(0,8))

final position error, variance:
	 [-0.019 -0.005] [0.005 0.006]

在这里插入图片描述

从图上看，在机器人的第一次滤波结果中似乎只有几个粒子。但这不是真的，有5000个粒子，但由于重采样，大多数粒子是彼此的副本。原因是传感器的高斯分布非常窄，这称为样本贫化，可能导致滤波器发散。我将在下面详细讨论这个问题。现在，看看 $x = 2$ 时的第二次滤波结果，我们可以看到粒子稍微有点分散了。这种分散是由运动模型噪声引起的。所有粒子都根据控制输入 $u$ 向前投射，但每个粒子都会加入噪声，噪声与机器人控制机构中的误差成比例。到了第三步，粒子已经分散到足以在机器人周围形成令人信服的粒子云。

粒子云的形状是椭圆，这不是巧合。由于传感器和机器人控制都是高斯分布，所以系统的概率分布也是高斯分布。粒子滤波是概率分布的抽样，所以云应该是一个椭圆。

必须认识到，粒子滤波算法不要求传感器或系统是高斯或线性的。因为我们用粒子云来表示概率分布，所以我们可以处理任何概率分布和强非线性问题，不连续或者存在硬极限的概率模型也是可以接受的。

传感器误差对滤波器的影响

滤波器的最初几次迭代产生了许多重复粒子，这是因为传感器的模型是高斯的，并且我们给它一个很小的标准差 $\sigma = 0.1$ 。一开始这是违反直觉的：当噪声较小时，卡尔曼滤波性能较好，而粒子滤波性能较差。

我们可以解释为什么这是真的。如果 $\sigma = 0.1$ ，则机器人位于 $(1, 1)$ ，粒子位于 $(2, 2)$ ，粒子距离机器人有14个标准差。这使得它的概率接近于零，因此它对均值的估计几乎没有任何贡献，并且在重采样之后它极不可能存活。如果 $\sigma = 1.4$ ，则粒子距离机器人仅与1个标准差相距，因此它将有助于平均值的估计。在重采样过程中，可能会复制一次或多次。

理解这一点非常重要：一个非常准确的传感器可能会导致表现较差的粒子滤波器，因为很少有粒子会是概率分布的好样本。我们有一些可用的规避方法。首先，我们可以人为地增加传感器噪声的标准差，这样粒子滤波器将接受更多的点来匹配机器人概率分布。这是非最佳的，因为其中一些点实际上是一个糟糕的状态。真正的问题是没有生成足够的点，导致也没有足够的点在机器人附近。增加粒子数 $N$ 通常可以解决这个问题。由于粒子数的增加会显著增加计算时间，因此此决定并非免费的。让我们看看使用100000个粒子的结果。

seed(2) 
run_pf1(N=100000, iters=8, plot_particles=True, 
        xlim=(0,8), ylim=(0,8))

final position error, variance:
	 [-0.17   0.084] [0.005 0.005]

在这里插入图片描述

在 $x = 1$ 处有更多的粒子，在 $x = 2$ 处就有了一个令人信服的粒子云。显然，滤波器的性能更好，但代价是占用大量内存，并且运行时间长。

另一种方法是更聪明地生成初始粒子云。假设我们猜测机器人初始位置在 $(0, 0)$ 附近。这并不精确，因为实际上机器人放置在 $(1, 1)$ 处，但也很接近了。如果我们在 $(0, 0)$ 附近创建一个正态分布的云，粒子匹配机器人位置的可能性就大得多。也就是说，初始粒子群不要大面积铺开。

run_pf1()有一个可选参数initial_x。使用此参数可以指定机器人的初始猜测位置。然后，代码使用create_gaussian_particles(mean, std, N)来在初始猜测位置创建正态分布的粒子群。我们将在下面的内容中使用它。

样本贫化引起的滤波器退化

上面所写的粒子滤波远非完美，下面是它使用不同的随机种子执行的情况。

seed(6) 
run_pf1(N=5000, plot_particles=True, ylim=(-20, 20))

final position error, variance:
	 [ -2.688 -31.47 ] [47.065 47.03 ]

在这里插入图片描述

由于随机种子的改变，此时，点的初始样本没有在机器人附近生成任何点。粒子滤波在重采样操作期间不创建新点，因此它最终复制的点都不是概率分布的代表性样本。如前所述，这被称为样本贫化。问题很快就失控了。这些粒子和观测值都不能很好的匹配，因此它们以高度非线性、曲线分布的方式分散，并且粒子滤波器发散。机器人附近没有可用的粒子，因此它永远无法收敛。

让我们使用create_gaussian_particles()方法来尝试在机器人附近生成更多点。我们可以通过使用initial_x参数来指定创建粒子的位置。

seed(6) 
run_pf1(N=5000, plot_particles=True, initial_x=(1,1, np.pi/4))

final position error, variance:
	 [ 0.035 -0.077] [0.007 0.009]

在这里插入图片描述

这很有效。如果有任何方法可以粗略估计初始位置，则应始终尝试在其附近创建粒子。不要太小心，如果生成的所有点都非常靠近一个位置，则粒子的分散程度可能不足以捕捉系统中的非线性。这是一个相当线性的系统，所以我们可以在分布上有一个较小的方差。显然这取决于你需要解决的问题。增加粒子的数量总是获得更好样品的好方法，但是处理成本可能比你愿意支付的要高。

重要性采样

先暂时把我们必须面对的困难抛在一旁。假设有一些概率分布描述了我们机器人的位置和运动，我们就可以直接从这个分布中抽取粒子样本，用蒙特卡罗方法计算积分。

但实际的困难是，在许多问题中我们压根就不知道分布。例如，被跟踪对象的移动可能与我们使用的状态模型预测的非常不同。如何从未知的概率分布中抽取样本？

统计学中有一个定理叫做重要性采样，它为我们提供了一种方法：从不同却已知的概率分布中抽取样本，并使用这些样本来计算未知概率的分布的特性。这是一个奇妙的定理。

这个想法很简单，我们已经用过了：我们从已知的概率分布中抽取样本，但根据我们感兴趣的分布对样本进行加权。例如：我们可以通过计算样本的加权均值和加权方差来计算均值和方差等特性。

对于机器人定位问题，我们从状态模型预测步中计算出来的概率分布中抽取样本。换句话说，我们推断机器人在那里，它可能正以这个方向和速度移动，因此它可能在这里。然而机器人可能做了完全不同的事情。它可能从悬崖上掉下来，或者被迫击炮弹击中。在每种情况下，预测步的概率分布都是不正确的。似乎我们被阻碍了，但我们不是，因为我们可以使用重要性采样。我们从可能不正确的概率分布中提取粒子，然后根据粒子与观测值的匹配程度对它们进行加权。这个加权是基于真实的概率分布，所以根据这个理论得出的均值、方差等都是正确的！

这怎么可能是真的？我会给你数学证明过程；如果你不打算超越机器人定位问题，你可以放心地跳过这个。然而，其他的粒子滤波问题需要不同的方法来进行重要性采样，这些数学知识可能会有所帮助。而且，网络上的文献和很多内容都使用了数学公式来支持我相当不精确的“想象那……”的论述。如果你想了解这些文献，你需要知道下列方程式。

我们想从一些概率分布 $\pi (x)$ 中取样，然而，我们不知道 $\pi (x)$ 是什么。相反，我们只知道另一种概率分布 $q (x)$ 。在机器人定位的背景下， $\pi (x)$ 是机器人的后验概率分布，但我们不知道；而 $q (x)$ 是我们机器人的先验概率分布，这我们知道。

概率分布为 $\pi (x)$ 的函数 $f (x)$ 的期望值为：

$E\left [ f(x) \right ] = \int f(x) \pi (x)dx$

我们不知道 $\pi (x)$ ，所以我们无法计算这个积分。我们知道另一种分布 $q (x)$ ，所以我们可以把它加到积分中，而不需要改变它的值：

$E\left [ f(x) \right ] = \int f(x) \pi (x) \frac{q(x)}{q(x)} dx$

现在我们重新排列和分组：

$E\left [ f(x) \right ] = \int f(x) q(x) \frac{\pi (x)}{q(x)} dx$

我们知道 $q (x)$ ，所以我们可以用蒙特卡罗积分计算 $\int f(x)q(x)dx$ ，剩下 $\pi (x) / q(x)$ 。这是一个比率，我们把它定义为一个权重。这给了我们：

$E\left [ f(x) \right ] = \sum_{i = 1}^{N} f(x^{i}) w(x^{i})$

也许这看起来有点抽象。如果我们要计算粒子的均值，我们会计算：

$\mu = \frac{1}{N} \sum_{i = 1}^{N} x^{i}w^{i}$

这就是我在本章前面给你的等式。

这要求权重与 $\pi (x) / q(x)$ 的比值成正比。我们通常不知道确切的值，所以在实践中，我们通过将它们除以 $\sum w(x^{i})$ 来对权重进行归一化。

当你制定一个粒子滤波算法时，你必须根据你的具体对于机器人定位，用于 $q (x)$ 的最佳分布是来自滤波器predict()步的粒子分布。让我们再看一遍代码：

def update(particles, weights, z, R, landmarks):
    for i, landmark in enumerate(landmarks):
        dist = np.linalg.norm(particles[:, 0:2] - landmark, axis=1)
        weights *= scipy.stats.norm(dist, R).pdf(z[i])

    weights += 1.e-300      # avoid round-off to zero
    weights /= sum(weights) # normalize

在这里，我们根据贝叶斯计算来计算权重 $\left | likelihood×prior \right |$ 。

当然，如果你能从先验知识中计算出后验概率分布，你可以直接这样做。如果你不能，那么重要性采样给你一个方法来解决这个问题。实际上，计算后验概率是非常困难的。卡尔曼滤波获得了惊人的成功，因为它利用了高斯函数的特性来寻找解析解。一旦我们放宽了卡尔曼滤波所要求的条件（马尔可夫性质、高斯观测、线性过程），那么重要性采样和蒙特卡罗方法使问题变得容易处理。

重采样

重采样算法也会影响滤波器的性能。例如，假设我们通过随机选取粒子来对粒子重采样。这将导致我们选择许多权重很低的粒子，由此产生的一组粒子将是概率分布的糟糕表现。

对这一主题的研究仍在继续，但实践中有少数算法在各种各样的情况下都能很好地工作。我们需要一个具有若干性质的算法。它应该优先选择概率更高的粒子。为避免样本贫化，应选择具有代表性的高概率粒子群。同时，它也应该包含足够的低概率粒子，使滤波器有机会检测到强非线性行为。

FilterPy实现了几种流行的算法。FilterPy不知道粒子滤波器是如何实现的，因此它无法直接生成新的样本。相反，算法创建了一个numpy.array来包含所选粒子的索引。如果你的代码需要执行重采样步骤，就可以直接使用了：

def resample_from_index(particles, weights, indexes):
    particles[:] = particles[indexes]
    weights.resize(len(particles))
    weights.fill(1.0 / len(weights))

多项式重采样

多项式重采样是我在开发机器人定位示例时使用的算法。想法很简单：计算归一化权重的累积和。这将提供一个从0到1递增的值数组。下面是一个图，它说明了如何将权重隔开。颜色是没有意义的，它们只是使分割更容易看到。

from kf_book.pf_internal import plot_cumsum

print('cumulative sume is', np.cumsum([.1, .2, .1, .6]))
plot_cumsum([.1, .2, .1, .6])

cumulative sume is [0.1 0.3 0.4 1. ]

在这里插入图片描述

为了选择一个权重，我们生成一个均匀分布在0和1之间的随机数，并使用二分法搜索来找到它在累积和数组中的位置。较大的权重比较小的权重占据更多的空间，因此它们更有可能被选中。

使用NumPy的ufunc支持，编写代码将非常容易。Ufuncs将函数应用于数组的每个元素，并返回一个结果数组。searchsorted()是NumPy的二分搜索算法。如果你为它提供一个搜索值数组，它将返回一个答案数组：每个搜索值对应一个答案。

def multinomal_resample(weights):
    cumulative_sum = np.cumsum(weights)
    cumulative_sum[-1] = 1.  # avoid round-off errors
    return np.searchsorted(cumulative_sum, random(len(weights)))

举个例子：

from kf_book.pf_internal import plot_multinomial_resample

plot_multinomial_resample([.1, .2, .3, .4, .2, .3, .1])

在这里插入图片描述

这是一个 $O (n l o g (n))$ 复杂度的算法。这并不可怕，但是仍然有仅 $O (n)$ 复杂度的重采样算法，却在样本的均匀性方面具有更好的性能。我之所以展示它，是因为你可以将其他算法理解为这个算法的变体。

使用从FilterPy导入函数：

from filterpy.monte_carlo import multinomal_resample

残差重采样

残差重采样既提高了多项式重采样的运行效率，又保证了采样在粒子总体上的均匀性。这是相当巧妙的：标准化的权重乘以 $N$ ，然后每个权重的整数值被用来定义该粒子的采样数量。例如，如果一个粒子的权重为 $0.0012$ 且 $N = 3000$ ，计算 $0.0012\times 3000=3.6$ ，因此将对该粒子采集3个样本。这将确保至少选择一次所有较高权重的粒子。运行时间为 $O (N)$ ，比多项式重采样快。

但是，由于小数点的存在，这可能不会正好生成所有 $N$ 个选择。为了选择剩下的部分，我们取残差：权重减去整数部分，剩下的是数字的小数部分。然后，我们使用一种更简单的采样方案，例如多项式，根据残差选择其余的粒子。在上述示例中，标度权重为3.6，因此残差将为0.6（ $3.6 - in t (3.6)$ ）。这个残差较大，所以粒子很可能会再次被取样。这是合理的，因为残差越大，舍入误差越大，因此在整数步中粒子的采样相对不足。

def residual_resample(weights):
    N = len(weights)
    indexes = np.zeros(N, 'i')

    # take int(N*w) copies of each weight
    num_copies = (N*np.asarray(weights)).astype(int)
    k = 0
    for i in range(N):
        for _ in range(num_copies[i]): # make n copies
            indexes[k] = i
            k += 1

    # use multinormial resample on the residual to fill up the rest.
    residual = w - num_copies     # get fractional part
    residual /= sum(residual)     # normalize
    cumulative_sum = np.cumsum(residual)
    cumulative_sum[-1] = 1. # ensures sum is exactly one
    indexes[k:N] = np.searchsorted(cumulative_sum, random(N-k))

    return indexes

你可能会尝试用切片索引替换内部for循环[k:k + num_copies[i]] = i，但是非常短的切片相对较慢，for循环通常运行得更快。

让我们看一个例子：

from kf_book.pf_internal import plot_residual_resample

plot_residual_resample([.1, .2, .3, .4, .2, .3, .1])

在这里插入图片描述

使用从FilterPy导入函数：

from filterpy.monte_carlo import residual_resample

分层重采样

该方案的目的是：保证采样得到的粒子之间相对均匀。它的工作原理是将累积和分成 $N$ 个相等的部分，然后从每个部分中随机选择一个粒子。这保证了每两个粒子的间隔在0到 $\frac{2}{n}$ 之间。

下图说明了这一点。彩色条表示数组的累积和，黑线表示 $N$ 等分，随机放置在每个小块中的黑色圆点表示粒子。

from kf_book.pf_internal import plot_stratified_resample

plot_stratified_resample([.1, .2, .3, .4, .2, .3, .1])

在这里插入图片描述

执行分层操作的代码非常简单：

def stratified_resample(weights):
    N = len(weights)
    # make N subdivisions, chose a random position within each one
    positions = (random(N) + range(N)) / N

    indexes = np.zeros(N, 'i')
    cumulative_sum = np.cumsum(weights)
    i, j = 0, 0
    while i < N:
        if positions[i] < cumulative_sum[j]:
            indexes[i] = j
            i += 1
        else:
            j += 1
    return indexes

使用从FilterPy导入函数：

from filterpy.monte_carlo import stratified_resample

系统重采样

与分层重采样一样，空间被划分为 $N$ 分区。不同的是，我们选择一个随机偏移量用于表示与分割线的偏移量，这样就确保了粒子之间正好相距 $\frac{1}{n}$ 。

from kf_book.pf_internal import plot_systematic_resample

plot_systematic_resample([.1, .2, .3, .4, .2, .3, .1])

在这里插入图片描述

看过前面的例子后，代码再简单不过了：

def systematic_resample(weights):
    N = len(weights)

    # make N subdivisions, choose positions 
    # with a consistent random offset
    positions = (np.arange(N) + random()) / N

    indexes = np.zeros(N, 'i')
    cumulative_sum = np.cumsum(weights)
    i, j = 0, 0
    while i < N:
        if positions[i] < cumulative_sum[j]:
            indexes[i] = j
            i += 1
        else:
            j += 1
    return indexes

使用从FilterPy导入函数：

from filterpy.monte_carlo import systematic_resample

选择合适的重采样算法

让我们同时看一下这四种重采样算法，以便比较它们。

a = [.1, .2, .3, .4, .2, .3, .1]
np.random.seed(4)
plot_multinomial_resample(a)
plot_residual_resample(a)
plot_systematic_resample(a)
plot_stratified_resample(a)