粒子滤波小记

关于粒子滤波，我其实早就应该写篇文章来简单的聊一聊。但是由于拖延症加健忘症，一直就到了现在才开始写。

先说粒子滤波的推导和应用实例网上一搜就是一大堆，在搜索结果的中文内容中，有不少的是互相抄袭，这就很让人生气了。对，没错，就是说的csdn，博客互相抄袭，我都不想吐槽了。贺一佳博客是原创的，而且讲得很详细，最后也有实例程序。但是吧，详细是详细，就是太 “数学化”了，不够通俗易懂。

抛开繁杂的数学原理不谈，粒子滤波最基本的思想就是：如果我们不知道当前时刻的状态分布，那么我们就从上一时刻的状态分布中取出充分多的粒子，然后让这些粒子做一次状态转移函数相同的“运动”，然后我们再按照观测方程，对每个粒子做一次虚拟观测，最后我们比较每个粒子的“虚拟观测值”和真是观测值，根据相似的程度，给每个粒子分配一个合理的权重。接下来对所有的粒子加权求和，我们就得到了当前时刻的最优位置，进一步，我们还可通过密度提取、高斯近似、直方图近似、核密度估计等方法获得当前时刻的近似连续概率密度分布。

下面简要的说明一下数学原理，以下更多基于三维空间中机器人的运动推导，虽不及贺一家博士推导的普适性，但也一定程度上反应了算法的原理，以下f(x) 是我们的目标分布，g(x)是一个已知的分布，称之为建议分布，以下是求x在区间A的期望，其中(I(x \in A))是一个指示函数，如果(x \in A)，则返回1，否则返回0：

其中,记，是一个加权因子，他衡量了f(x)与g(x)之间的“不匹配”程度。通过上面的式子，我们就从f(x)的期望转化为求g(x)的期望，因为g(x)是我们可以自己指定的，从而使得我们可以从一个已知分布中去采样粒子来求未知分布f(x)的期望。

particle_filter1.png particle_filter2.png particle_filter3.png

如图所示，小竖线就是一个个粒子，竖线的高度就是其权重，在第一幅图中，f使我们希望获得的粒子分布。第二幅图是我们从g中抽样出粒子，带有均匀的权重，第三幅图，我们通过计算w，并归一化，将粒子权重重新分配。f较大的区域的粒子能获得较大的权重。

关键是我们要如何得到这个w(x)，这是一个麻烦的点。在机器人的运动中，我们下面的式子来表示后验分布，即目标分布：

另外，如果我们建议分布采用先验分布，也就是把上一时刻的最优分布通过状态转移方程后得到的分布，那建议分布就是：

那么，按照前面的f(x)/g(x)，目标分布除以建议分布就是。这里在强调一次，在这种序列滤波中，权重就是上一时刻的后验分布。

在粒子滤波的过程中，大部分的粒子权重会变得极低，从而对滤波过程不再有任何的贡献，我们需要将其剔除，并补充新的粒子进去，以保证粒子的总数不变。剔除和添加的方式，采用“轮盘赌”的方式来进行。通过这种方式，大权重的粒子会分身出多个子代粒子，小权重粒子就会被剔除。单这也带来了一些问题，比如粒子退化等等。粒子退化使得滤波器对“机器人绑架”问题无能为力，也使得算法的鲁棒性降低，这可以通过增加随机粒子来改善。为了改善完美的传感器在滤波中容易失效的问题，我们还可以改变建议分布，通过混合蒙特卡洛来提高定滤波的鲁棒性和准确性。另外，当机器人的位姿完全不确定的时候，粒子总数应该充分大，以覆盖尽可能多的位姿，但是当其位姿逐渐收敛的时候，我们就不再需要那么多的粒子，所以，固定粒子数目的粒子滤波也需要改进，比如通过“库尔贝克-莱布勒散度”采样来调节样本集合的大小。

这里我给出一个用python写的简单的粒子滤波的算法，运动模型采用

测量模型采用

import numpy as np
from matplotlib import pyplot as plt
import copy

#x = x/2 + 25*x/(1+x*x) + 8*np.cos(1.2*(i-1))+np.random.normal(0.0, x_Q) #forecast

N = 100
T = 100
R = 1.0 #measurement variance
Q = 1.0 #transform variance

x_initial = 0.1

x = x_initial
x_list =[x]

measurements_list =[x]

particles = [np.random.normal(scale=np.sqrt(5)) for i in range(N)]
w = [1.0/N for i in range(N)]

optimal_list =[np.mean([w[i]*particles[i] for i in range(N)])]

for t in range(T):
    x = x/2 + 25*x/(1+x*x) + 8*np.cos(1.2*(t-1)) + np.random.normal(scale = np.sqrt(Q))#transform in real world
    x_list.append(x)
    z = x + np.random.normal(scale=np.sqrt(R)) #measurement from noisy sensor

    for i in range(N):
        particles[i] = particles[i]/2 + 25*particles[i]/(1+particles[i] *particles[i]) + 8*np.cos(1.2*(t-1)) + np.random.normal(scale = np.sqrt(Q))#particles transform without noise
        z_ = particles[i] #measurement from particles without noise
        w[i] = 1.0/np.sqrt(2*np.pi*R)* np.exp(-(z_ - z)**2/(2*R))
    sum_ = np.sum(w)
    w = [item/sum_ for item in w] #

    #resample
    temp_p = []
    sample_base = np.random.uniform(0.0, 1.0/N)
    w_cumulation = [np.sum(w[:(i+1)]) for i in range(N)]
    for i in range(N):
        index =0 
        while w_cumulation[index]<sample_base:
            index +=1
        temp_p.append( particles[index])
        sample_base += 1.0/N
    for i in range(N):
        particles[i] = temp_p[i] 
    optimal = np.mean(particles)
    optimal_list.append(optimal)

plt.plot(range(len(x_list)), x_list)
plt.plot(range(len(x_list)), optimal_list, 'r')
plt.show()
print("end...")

my_particle_filter.png