粒子滤波小记

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

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

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

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

$$E_f =[I(x \in A)]=\int f(x)I(x \in A)dx \\ =\int \frac{f(x)}{g(x)} g(x)I(x \in A)dx \\= E_g[w(x)I(x \in A)]$$

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

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

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

$$p(x_{0:t}|z_{1:t},u_{1:t} ) = \eta p(z_t | x_{0:t},z_{1:t-1},u_{1:t})p(x_{0:t}|z_{1:t-1},u_{1:t}) \\ =\eta p(z_t|x_t)p(x_{0:t}|z_{1:t-1},u_{1:t}) \\=\eta p(z_t|x_t) p(x_t|x_{0:t-1},z_{1:t-1},u_{1:t})p(x_{0:t-1}|z_{1:t-1},u_{1:t})\\=\eta p(z_t|x_t) p(x_t|x_{t-1},u_t) p(x_{0:t-1}|z_{0:t-1},u_{1:t-1})$$

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

$$p(x_t|x_{t-1},u_t)bel(x_{0:t-1}) = p(x_t|x_{t-1},u_t)p(x_{0:t-1}|z_{1:t-1},u_{1:t-1})$$

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

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

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

测量模型采用\(z_k = x_k + n_k\)

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...")

发表评论

电子邮件地址不会被公开。 必填项已用*标注