Particle Filters

Particle Filters，从1993年提出来就作为一种应用很广的最优化估计算法，用于解决非线性非高斯分布的状态空间问题求解，通过在线的数值估计的方式解决问题。现在也应用在像计算机视觉，计量经济学，机器人学和导航等各种各样的场景中。¹并且随着改进方案的不断提出，Particle Filters也可以应用在更加高维，更加复杂的场景中。²

背景

Particle Filters可以作为解决两个机器人领域全局定位和kidnapped robot的问题，用于帮助机器人在不确定状况下求解自身的全局位置的问题。卡尔曼滤波（Kalman filter）假设目标为线性模型，噪声为高斯分布，返回一个假设值。但是当目标对象运动非常快速，或者不太容易预测（比如大风天随风飘动的树叶）那就会失效，但是Particle Filters可以返回很多的假设（每个Particle代表一个假设）目标的状态可以是位置可以是模型的形状等。

粒子滤波可以在跟踪问题上表现出较好的效果，在实际情况中，我们需要应用的场景都比较复杂，简单地总结一下应用场景的特征就是：

• 多模型：我们需要跟踪的目标不止一个
• 震荡：有些目标可能会被遮挡
• 非线性行为：飞行器会受到风的影响，球会收到各种摩擦力的影响，人的行走也会被人群影响
• 非线性的测量：像雷达测量的结果，转换到3维坐标是要开平方根的
• 非高斯噪声：当目标穿过背景的时候，有时候计算机视觉会错误识别背景和前景
• 连续性：目标的位置和目标的速度等可以做到在时间上平滑
• 多变量：我们要跟踪多个指数，包括位置，速度，变换速率等
• 未知过程模型：甚至不知道系统内部的工作规律，比如某些动物的运动

概念

粒子滤波（Particle Filters）是一种常见的函数最优化的算法，它的搜索空间是通过粒子（采样）去搜索，然后验证粒子所代表的解，根据解的优劣赋予不同的权重，权重越高的采样会被增加，验证后结果较差的采样会被减少。然后所有剩下的粒子会被带入到另一轮的重新采样中去，不断循环直到找到最优解。

蒙特卡洛逼近

通过数值方法计算积分最简单的方法是通过求小矩边梯形的面积再累加，但是如果函数的维度太高，变量的数量太多的情况下，简单的方法就难以适用了，而蒙特卡洛方法可以从概率学的角度出发近似地计算这些积分。

对于形式如 \(I=\int^{b}_{a}h(x)g(x)dx\) 的积分形式，如果能够转换用密度函表示的形式就可以通过采样函数进行模拟。也就是对于随机变量\(X\)的函数\(f(x)\)，其期望值可以通过下面的公式计算。

其中，\(p(x)\)为变量\(X\)的密度函数，在定义域上的积分为1。

这种平均值我们可以根据大数定理知道，对于一个已知分布的随机序列，当取样数趋于无穷时，其均值趋向于期望。所以上面的公式可以再写成如下形式：

其中\(x_s\sim p(X)\)，这是一个很重要的条件，即生成的随机数要符合密度函数为\(p(X)\)的分布，这样计算得到的平均值才是积分的近似值。

粒子滤波跟踪

粒子滤波跟踪通常可以简单地分为以下几个步骤：

1. 随机生成一堆粒子：每个粒子可以包括位置，指向，或者任何你需要的状态信息，每个粒子都有它的权重，这个权重表示了它和真实系统状态的吻合程度。刚开始所有的粒子的权重都是相同的。
2. 预测粒子的下一个阶段：通过对于真实系统行为的预测，移动粒子
3. 更新：基于测量结果更新粒子的权重，接近测量结果的粒子权重将被增加
4. 重采样：丢弃那些不相关的粒子，复制那些更加可信的粒子
5. 计算估计：可选操作，计算粒子的加权平均和方差得到状态估计

基于粒子滤波的目标跟踪是一种生成式跟踪方法，所以有初始化的阶段。对于一帧图像，首先人为给定它的初始化，在特定的区域提出特征。

其中\(r\)是区域到中心的距离，

初始化

如果我们是被动地跟踪目标（我们不知道它的运动方式，也就是没有运动控制的输入信息），就还需要将各个维度的速度也加入到状态空间中去，维度越高就需要指数增长的粒子数量来进行跟踪。³针对一个机器人追踪问题，我们可以定义粒子模型

重采样

在重采样的过程中，可能性低的粒子会被舍弃，可能性高的粒子会被复制，但是也不是原原本本的复制，这些原本粒子的副本会在预测阶段再增加一些噪声。这样在新得到的点集中，粒子中的绝大多数可以准确地表示出概率分布。

重采样有很多种对应的算法，最简单的一种是随机采样算法，随机采样是根据权重分配被采样到的的概率，权重越大的粒子就越容易被采到。

重采样的频率也是通过一种规则确定的，因为如果没有收到新的观测值，也就不能从重采样过程中获得新的信息。所以可以根据一个影响因子\(N\)来进行估计是否要进行重采样，这个影响因子大致衡量了那些对于概率分布有贡献的粒子的数量。

一个比较有效的点大概是在\(N/2\)这个值左右，但是也要依据情况而定，甚至还会出现\(\hat{N}_{eff}=N\)的情况，这是因为所有的点都模式坍塌到一个点去了（每个点都有相同的权重），这不是一个好的现象，有时可以通过增加粒子数量来避免，但是如果经常出现，你就需要调整你自己的算法了。

在开始的几轮，滤波器会带来很多重复的粒子，这是因为传感器的建模方式是基于高斯的，而且只有很小的标准差，这种较小的噪声对于卡尔曼滤波器而言是一件好事，但是对于粒子滤波器会让它表现地更差。这是因为在重采样的过程中，离观察结果稍微远一点的粒子就会被抛弃了，导致留下的粒子同质化比较严重。

同时，粒子滤波的效果也与初始化设置的种子有很大的关系，有些初始化所用的种子会导致在后来重采样的过程中不收敛，甚至与我们的预期远远不相符合。因此，也要尽量生成一些粒子在初始状态的周围（想办法去估计这些粒子的状态信息），不过也没有必要把点估计的太精准，因为太过单一准确的初始估计也会使滤波器难以把握系统中的非线性。并且对于这种非线性的问题，我们还是要避免分布中的方差太小的问题。

重要性采样

我们需要从一些分布中获得机器人的位置和某些信息，从这些分布中得到粒子的采样，并且通过蒙特卡洛采样获得对应的积分。

很多问题是没法获得对应的分布的，比如你要追踪一个目标，但是根本就不知道它将往何处去，预测阶段也无法实现。重要性采样的用处就在于，通过一个不同但已知的分布来获得对应未知分布的性质。

思路也是很简单的，我们从已知的分布中抽取样本，然后用我们感兴趣的分布来确定样本的权重，然后就可以通过这种方式可以得到这些加权的样本均值和方差这些代表分布的特性。

假设我们有一个机器人在过道中，根据上一次的估计结果，我们知道，在下一时刻，它将可能保持哪种速度，将会出现在什么位置，这些是可以预测的。但是，如果它掉到坑里了，又或者被人撞了一下，那原来的根据预计的速度和位置估计的下一时刻的状态也就不准确了。所以我们还是要根据测量结果来给粒子赋予权重，这些权重是根据真实的分布得出来的。

\(\pi(x)\)：是我们感兴趣的分布，但是是未知的 \(q(x)\)：这是一个我们已经知道的分布，是测量结果的分布

我们可以对下面的积分进行改写：

但是我们不知道\(\pi(x)\)的分布，带入一项：

改写这条公式的形式不影响结果：

这样我们就可以通过已知的分布用蒙特卡洛积分的办法来解决问题，也就生息的是\(\pi(x)/q(x)\)，这个比例也就是我们在测量阶段所说的的权重。因此，积分改写成：

更加通俗地可以表示成计算粒子的加权平均值：

所以简单地解释一下下面的权重的更新代码：

def update(particles, weights, z, R, landmarks):
    weights.fill(1.)
    # 遍历状态空间，将各个要素的分布都叠加在一起
    for i, landmark in enumerate(landmarks):
        dist = np.linalg.norm(particles[:, 0:2] - landmark, axis=1)
        # 这里的dist是分布的均值，R表示分布的区间
        weights *= scipy.stats.norm(dist, R).pdf(z[i])
    weights += 1.e-300 # avoid round-off to zero
    weights /= sum(weights) # normalize

多项式重采样

得到各个粒子的权重集合之后，需要根据这些权重进行采样，我们期望权重越大则该粒子更容易被采集到。通过对概率分布函数进行积分，得到累计概率函数，针对采样问题就是对概率分布数列进行累加，生成均匀分布的随机数，通过二分查找确定这个随机数所在的区间，这个区间即对应一个粒子，这种采样方式的复杂度为\(O(N\log N)\)。

残差重采样

残差重采样可以将复杂度降低到\(O(N)\)，通过残差重采样实现高效的采样过程。残差重采样不仅可以提高运行效率还能保证权重较大的粒子至少能被采样到一次。

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):
        # 插一句题外话使用小循环比使用小切片要更快
        # indexes[k:k+num_copies[i]] = i (小切片索引真的很慢)
        for _ in range(num_copies[i]):
            indexes[k] = i
            k += 1

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

    return indexes

分层采样

分层采样方式会先将累计密度分布分成\(N\)个相同长度的段，然后从每个段中随机选取一个粒子。这种采样方式保证了每个样本之间有\(0\sim \frac{2}{N}\)的间隔。

系统性重采样

前面的分层采样把累计分布函数分成等长的段，在每个段中随机采样，而这里是随机选择一个偏移量，每个段中都采集这个偏移位置的粒子。

重采样测试

在测试过程中，多项式采样的表现比较差，很多权重较大的粒子都被忽略了，而权重小的粒子却被采样了多次。而残差重采样确实比较好的达到了它的目的，但是还是有很多比较合适的粒子没有被采集到。分层采样和系统性采样的表现都相对比较好，

总结

粒子滤波（Particle Filters）有时候就像是一种集成方法，这和卡尔曼滤波等有比较不同的区别，卡尔曼滤波是一个优化的针对线性、高斯噪声的模型，它非常高效但是一旦情况变得复杂和不可预测就会失效。而粒子滤波可以在这种复杂的情形下做倒比较好的效果，前提是粒子的数量足够多的情况下。重要性采样让我们可以在不知道目标分布的情况下获得比较好的估计，蒙特卡洛方法让我们对可以求解那些可以滤波器中要求的积分。

但是粒子滤波的能力是通过比较高额的计算开销达成的，大量的粒子需要很高的内存开销，同时还需要考虑粒子的退化等问题。