particle filtering---粒子滤波
原文来自:
particle filtering—粒子滤波(讲的很通俗易懂)
Particle Filter Tutorial 粒子滤波:从推导到应用(一)
Particle Filter Tutorial 粒子滤波:从推导到应用(二)
Particle Filter Tutorial 粒子滤波:从推导到应用(三)
Particle Filter Tutorial 粒子滤波:从推导到应用(四)
文章架构:
由最基础的贝叶斯估计开始介绍,再引出蒙特卡罗采样,重要性采样,SIS粒子滤波,重采样,基本粒子滤波Generic Particle Filter,SIR粒子滤波,这些概念的引进,都是为了解决上一个概念中出现的问题而环环相扣的。最后给出几个在matlab和python中的应用,例程包括图像跟踪,滤波,机器人定位。
再往下看之前,也可以看看《卡尔曼滤波:从推导到应用》,好对这种通过状态方程来滤波的思路有所了解。
一、贝叶斯滤波
假设有一个系统,我们知道它的状态方程,和测量方程如下:
如:
其中x为系统状态,y为测量到的数据,f,h是状态转移函数和测量函数,v,n为过程噪声和测量噪声,噪声都是独立同分布的。上面对应的那个例子将会出现在程序中。
从贝叶斯理论的观点来看,状态估计问题(目标跟踪、信号滤波)就是根据之前一系列的已有数据 y 1 : k y_{1:k} y1:k(后验知识)递推的计算出当前状态 x k x_k xk的可信度。这个可信度就是概率公式 p ( x k ∣ y 1 : k ) p(x_k|y_{1:k}) p(xk∣y1:k),它需要通过预测和更新两个步奏来递推的计算。
预测过程是利用系统模型(状态方程1)预测状态的先验概率密度,也就是通过已有的先验知识对未来的状态进行猜测,即p( x(k)|x(k-1) )。
更新过程则利用最新的测量值对先验概率密度进行修正,得到后验概率密度,也就是对之前的猜测进行修正。
在处理这些问题时,一般都先假设系统的状态转移服从一阶马尔科夫模型,即当前时刻的状态x(k)只与上一个时刻的状态x(k-1)有关。这是很自然的一种假设,就像小时候玩飞行棋,下一时刻的飞机跳到的位置只由当前时刻的位置和骰子决定。
同时,假设k时刻测量到的数据y(k)只与当前的状态x(k)有关,如上面的状态方程2。
为了进行递推,不妨假设已知k-1时刻的概率密度函数 p ( x k − 1 ∣ y 1 : k − 1 ) p(x_{k-1}|y_{1:k-1}) p(xk−1∣y1:k−1)
预测: 由上一时刻的概率密度 p ( x k − 1 ∣ y 1 : k − 1 ) p(x_{k-1}|y_{1:k-1}) p(xk−1∣y1:k−1)得到 p ( x k ∣ y 1 : k − 1 ) p(x_k|y_{1:k-1}) p(xk∣y1:k−1),这个公式的含义是既然有了前面1:k-1时刻的测量数据,那就可以预测一下状态x(k)出现的概率。
1
2
求 P ( A , B ∣ C ) P(A,B|C) P(A,B∣C)?
令 T = ( A , B ) T=(A,B) T=(A,B)
则 P ( A , B ∣ C ) = P ( T ∣ C ) = P ( C ∣ T ) P ( T ) P ( C ) = P ( C ∣ A , B ) P ( A , B ) P ( C ) P(A,B|C)=P(T|C)=\frac{P(C|T)P(T)}{P(C)}=\frac{P(C|A,B)P(A,B)}{P(C)} P(A,B∣C)=P(T∣C)=P(C)P(C∣T)P(T)=P(C)P(C∣A,B)P(A,B)
= P ( C ∣ A , B ) P ( A ∣ B ) P ( B ) P ( C ) =\frac{P(C|A,B)P(A|B)P(B)}{P(C)} =P(C)P(C∣A,B)P(A∣B)P(B)
又因为 P ( A ∣ B , C ) = P ( A ) P ( B ∣ A ) P ( C ∣ A , B ) P ( B ) P ( C ∣ B ) P(A|B,C)=\frac{P(A)P(B|A)P(C|A,B)}{P(B)P(C|B)} P(A∣B,C)=P(B)P(C∣B)P(A)P(B∣A)P(C∣A,B)
= P ( B ) P ( A ∣ B ) P ( C ∣ A , B ) P ( C ) P ( B ∣ C ) =\frac{P(B)P(A|B)P(C|A,B)}{P(C)P(B|C)} =P(C)P(B∣C)P(B)P(A∣B)P(C∣A,B)
因此
P ( A , B ∣ C ) = P ( A ∣ B , C ) P ( B ∣ C ) P(A,B|C)=P(A|B,C)P(B|C) P(A,B∣C)=P(A∣B,C)P(B∣C)
计算推导如下:
等式的第一行到第二行纯粹是贝叶斯公式的应用。
第二行得到第三行是由于一阶马尔科夫过程的假设,状态x(k)只由x(k-1)决定。
楼主看到这里的时候想到两个问题:
第一:既然 ,x(k)都只由x(k-1)决定了,即 p ( x k ∣ x k − 1 , y 1 : k − 1 ) = p ( x k ∣ x k − 1 ) p(x_k|x_{k-1},y_{1:k-1})=p(x_k|x_{k-1}) p(xk∣xk−1,y1:k−1)=p(xk∣xk−1),在这里弄一个 p ( x k ∣ y 1 : k − 1 ) p(x_k|y_{1:k-1}) p(xk∣y1:k−1)是什么意思?
这两个概率公式含义是不一样的,
前一个是纯粹根据模型进行预测,x(k)实实在在的由x(k-1)决定,
后一个是既然测到的数据和状态是有关系的,现在已经有了很多测量数据 y 了,那么我可以根据已有的经验对你进行预测,只是猜测x(k),而不能决定x(k)。
第二:上面公式的最后一行 p ( x k − 1 ∣ y 1 : k − 1 ) p(x_{k-1}|y_{1:k-1}) p(xk−1∣y1:k−1)是假设已知的,但是 p ( x k ∣ x k − 1 ) p(x_k|x_{k-1}) p(xk∣xk−1)怎么得到呢?
其实
p
(
x
k
∣
x
k
−
1
)
p(x_k|x_{k-1})
p(xk∣xk−1)它由系统状态方程决定,它的概率分布形状和系统的过程噪声形状一模一样。如何理解呢?观察状态方程(1)式我们知道,x(k) = Constant( x(k-1) ) + v(k-1) 也就是x(k)由一个通过x(k-1)计算出的常数叠加一个噪声得到。看下面的图:
如果没有噪声,x(k)完全由x(k-1)计算得到,也就没由概率分布这个概念了,由于出现了噪声,所以x(k)不好确定,他的分布就如同图中的阴影部分,实际上形状和噪声是一样的,只是进行了一些平移。理解了这一点,对粒子滤波程序中,状态x(k)的采样的计算很有帮助,要采样x(k),直接采样一个过程噪声,再叠加上 f(x(k-1)) 这个常数就行了。
我们知道了一个变量的分布,要生成一批样本服从这个分布,这个过程就叫采样
为什么要采样
为什么要采样呢?有人可能会这样想,样本一般是用来估计分布参数的,现在我都知道分布函数了,还采样干嘛呢?其实采样不只是可以用来估计分布参数,还有其他用途,比如说用来估计分布的期望、高阶动量等。其实在机器学习中,采样的主要用途是用来估计某个函数在某个分布上的期望 [公式] ,比如在EM算法中,计算E步的时候,已知隐变量的分布,用采样的方法估计对数似然的期望。
基本采样方法
- 变换法
大概思想是这样的,比如我们要对分布a采样,但是只有分布b的采样器,我们可以通过对b采样的结果进行一些变换,使变换后的结果服从分布a,举个例子,对0到1的均匀分布的结果进行tan函数变换得到的结果就服从柯西分布,大家可以试想下怎么变换能得到指数分布和高斯分布。- 接受-拒绝采样(accept-rejct sampling)
接受-拒绝采样基于这样一个前提: 对一个随机变量取样, 等价于从这个变量所服从的密度函数下方的区域均匀取样。当然, 取样的结果是变量值,如果是一元变量, 取样后得到的是一元变量的值。
上图显示了采样的过程。我们需要生成符合概率分布 p ( z ) p(z) p(z)的样本。 q ( z ) q(z) q(z)表示选择的建议分布(proposal distribution)。建议分布应选择简单的分布(如正态分布),目的是容易直接进行采样。整个计算过程可以概括如下:
- 选择一个易于采样的建议分布 q ( z ) q(z) q(z)
- 寻找一个常数 k k k,使 k q ( z ) ≥ p ( z ) kq(z)≥p(z) kq(z)≥p(z)对于 任意的 z z z均成立
- 从建议分布 q ( z ) q(z) q(z)中进行一次采样,得到样本 z 0 z_0 z0
- 在区间 [ 0 , k q ( z 0 ) ] [0,kq(z0)] [0,kq(z0)]上进行均匀采样,得到 u 0 u_0 u0
- 如果 u 0 > p ( z 0 ) u_0>p(z_0) u0>p(z0)(落在灰色区域)则拒绝采样 z 0 z_0 z0,否则,接受采样
- 重复步骤 3~5,直至生成了足够多的样本。
这种采样方法的效率与样本的接受率(步骤5)密切相关。而样本的接受率是受建议分布 q ( z ) q(z) q(z)影响的。因此,选择合适的建议分布式非常关键的。如果选择的建议分布 q ( z ) q(z) q(z)与 目标分布 p ( z ) p(z) p(z)差异太大,则会导致大量的样本被拒绝,从而降低算法的效率。另外 k 值得选择,也与样本接受率相关。
3. 重要性采样(importance sampling)
看看这里更清楚重要性采样(Importance Sampling)
Importance Sampling 也是借助了容易采样的建议分布 q ( z ) q(z) q(z)来解决问题,如下所示:
其中 p ( z l ) q ( z l ) \frac{p(z^l)}{q(z^l)} q(zl)p(zl) 可以看作 important weight,修正了由于从错误的建议分布中采样进入的偏差。
常见的情形是概率分布 p ( z ) p(z) p(z)是未归一化的: p ~ ( z ) = p ( z ) Z p \tilde{p}(z)=\frac{p(z)}{Z_p} p~(z)=Zpp(z) ,其中 Z p Z_p Zp 未知。类似的,我们采用建议分布 q ( z ) = q ~ ( z ) Z q q(z)=\frac{\tilde{q}(z)}{Z_q} q(z)=Zqq~(z) ,它具有相同的性质。于是有:
其中 r ~ l = p ~ ( z ) q ~ ( z ) \tilde{r}_l=\frac{\tilde{p}(z)}{\tilde{q}(z)} r~l=q~(z)p~(z) 。我们可以使用同样的样本集合来计算比值 Z p Z q \frac{Z_p}{Z_q} ZqZp,结果为:
与接受-拒绝采样的情形相同,重要性采样方法的效果严重依赖于建议分布 q(z) 与 目标分布 p(z) 的匹配程度。重要性采样⽅法的⼀个主要的缺点是它具有产⽣任意错误的结果的可能性,并且这种错误⽆法检测。这也强调了建议分布 q(z) 的⼀个关键的要求,即它不应该 在 p(z) 可能较⼤的区域中取得较⼩的值或者为零的值。接受-拒绝采样和重要性采样的关键在于建议分布的选取,选择一个合适的建议分布是往往比较困难。另外这两种采样方法通常只适用于单变量的情形。对于多元随机变量的采样,通常是采用MCMC (Markov chain Monte Carlo)或者 Gibbs采样的方法。
4. 马尔可夫链蒙特卡洛采样(MCMC)
马尔科夫链蒙特卡洛采样(MCMC)入门 之二
更新: 由 p ( x k ∣ y 1 : k − 1 ) p(x_k|y_{1:k-1}) p(xk∣y1:k−1)得到后验概率 p ( x k ∣ y 1 : k ) p(x_k|y_{1:k}) p(xk∣y1:k)。这个后验概率才是真正有用的东西,上一步还只是预测,这里又多了 k k k时刻的测量,对上面的预测再进行修正,就是滤波了。这里的后验概率也将是代入到下次的预测,形成递推。
推导:
其中归一化常数:
等式第一行到第二行是因为测量方程知道, y(k)只与x(k)有关,
p
(
y
k
∣
x
k
)
p(y_k|x_k)
p(yk∣xk)也称之为似然函数,由量测方程决定。
也和上面的推理一样,
y
k
=
h
(
x
k
)
+
n
k
y_k=h(x_k)+n_k
yk=h(xk)+nk,
x
(
k
)
x(k)
x(k)部分是常数,
p
(
y
k
∣
x
k
)
p(y_k|x_k)
p(yk∣xk)也是只和量测噪声
n
(
k
)
n(k)
n(k)的概率分布有关,注意这个也将为SIR粒子滤波里权重的采样提供编程依据。
贝叶斯滤波到这里就告一段落了。但是,请注意上面的推导过程中需要用到积分,这对于一般的非线性,非高斯系统,很难得到后验概率的解析解。为了解决这个问题,就得引进蒙特卡洛采样。
reference:
- M. Sanjeev Arulampalam 《A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking》
- ZHE CHEN 《Bayesian Filtering: From Kalman Filters to Particle Filters, and Beyond》
- Sebastian THRUN 《Probabilistic Robotics》
- 百度文库 《粒子滤波理论》
二、蒙特卡洛采样
假设我们能从一个目标概率分布p(x)中采样到一系列的样本(粒子)
x
1
,
.
.
.
,
x
N
x_1,...,x_N
x1,...,xN,(至于怎么生成服从
p
(
x
)
p(x)
p(x)分布的样本,这个问题先放一放),那么就能利用这些样本去估计这个分布的某些函数的期望值。譬如:
上面的式子其实都是计算期望的问题,只是被积分的函数不同。
蒙特卡洛采样的思想就是用平均值来代替积分,求期望:
这可以从大数定理的角度去理解它。我们用这种思想去指定不同的f(x)以便达到估计不同东西的目的。比如:要估计一批同龄人的体重,不分男女,在大样本中男的有100个,女的有20个,为了少做事,我们按比例抽取10个男的,2个女的,测算这12个人的体重求平均就完事了。注意这里的按比例抽取,就可以看成从概率分布p(x)中进行抽样。
下面再看一个稍微学术一点的例子:
假设有一粒质地均匀的骰子。规定在一次游戏中,连续四次抛掷骰子,至少出现一次6个点朝上就算赢。现在来估计赢的概率。
我们用
n
k
(
n
)
n_{k}^{(n)}
nk(n)来表示在第n次游戏中,第k次投掷的结果,k=1…4。对于分布均匀的骰子,每次投掷服从均匀分布,即:
这里的区间是取整数,1,2,3,4,5,6,代表6个面。由于每次投掷都是独立同分布的,所以这里的目标分布p(x)也是一个均匀分布
χ
=
{
1
,
2
,
.
.
.
,
6
}
4
\chi={\{1,2,...,6\}}^4
χ={1,2,...,6}4。一次游戏就是
χ
\chi
χ空间中的一个随机点。
为了估计取胜的概率,在第n次游戏中定义一个指示函数:
其中,指示函数I{x }是指,若x的条件满足,则结果为1,不满足结果为0。回到这个问题,这里函数 f()的意义就是单次游戏中,若四次投掷中只要有一个6朝上,f()的结果就会是1。由此,就可以估计在这样的游戏中取胜的期望,也就是取胜的概率:
当抽样次数N足够大的时候,上式就逼近真实取胜概率了,看上面这种估计概率的方法,是通过蒙特卡洛方法的角度去求期望达到估计概率的目的。是不是就跟我们抛硬币的例子一样,抛的次数足够多就可以用来估计正面朝上或反面朝上的概率了。
当然可能有人会问,这样估计的误差有多大,对于这个问题,有兴趣的请去查看我最下面列出的参考文献2。
(啰嗦一句:管的太多太宽,很容易让我们忽略主要问题。博主就是在看文献过程中,这个是啥那个是啥,都去查资料,到头来粒子滤波是干嘛完全不知道了,又重新看资料。个人感觉有问题还是先放一放,主要思路理顺了再关注细节。)
接下来,回到我们的主线上,在滤波中蒙特卡洛又是怎么用的呢?
由上面我们知道,它可以用来估计概率,而在上一节中,贝叶斯后验概率的计算里要用到积分,为了解决这个积分难的问题,可以用蒙特卡洛采样来代替计算后验概率。
假设可以从后验概率中采样到N个样本,那么后验概率的计算可表示为:
其中,在这个蒙特卡洛方法中,我们定义
f
(
x
)
=
δ
(
x
n
−
x
n
(
i
)
)
f(x)=\delta(x_n-x^{(i)}_n)
f(x)=δ(xn−xn(i)),是狄拉克函数(dirac delta function),跟上面的指示函数意思差不多。
看到这里,既然用蒙特卡洛方法能够用来直接估计后验概率,现在估计出了后验概率,那到底怎么用来做图像跟踪或者滤波呢?要做图像跟踪或者滤波,其实就是想知道当前状态的期望值:
也就是用这些采样的粒子的状态值直接平均就得到了期望值,也就是滤波后的值,这里的 f(x) 就是每个粒子的状态函数。这就是粒子滤波了,只要从后验概率中采样很多粒子,用它们的状态求平均就得到了滤波结果。
思路看似简单,但是要命的是,后验概率不知道啊,怎么从后验概率分布中采样!所以这样直接去应用是行不通的,这时候得引入重要性采样这个方法来解决这个问题。
三、重要性采样
无法从目标分布中采样,就从一个已知的可以采样的分布里去采样如 q(x|y),这样上面的求期望问题就变成了:
上面的期望计算都可以通过蒙特卡洛方法来解决它,也就是说,通过采样N个样本
{
x
k
(
i
)
}
∼
q
(
x
k
∣
y
1
:
k
)
\{x^{(i)}_k\} \sim q(x_k|y_{1:k})
{xk(i)}∼q(xk∣y1:k),用样本的平均来求它们的期望,所以上面的(3)式可以近似为:
这就是归一化以后的权重,而之前在(2)式中的那个权重是没有归一化的。
注意上面的(4)式,它不再是(1)式中所有的粒子状态直接相加求平均了,而是一种加权和的形式。不同的粒子都有它们相应的权重,如果粒子权重大,说明信任该粒子比较多。
到这里已经解决了不能从后验概率直接采样的问题,但是上面这种每个粒子的权重都直接计算的方法,效率低,因为每增加一个采样,p( x(k) |y(1:k))都得重新计算,并且还不好计算这个式子。
所以求权重时能否避开计算p(x(k)|y(1:k))?而最佳的形式是能够以递推的方式去计算权重,这就是所谓的序贯重要性采样(SIS),粒子滤波的原型。
下面开始权重w递推形式的推导:
假设重要性概率密度函数
q
(
x
0
:
k
∣
y
1
:
k
)
q(x_{0:k}|y_{1:k})
q(x0:k∣y1:k),这里x的下标是0:k,也就是说粒子滤波是估计过去所有时刻的状态的后验。假设它可以分解为:
后验概率密度函数的递归形式可以表示为:
P ( A , B ∣ C ) = P ( A ∣ B , C ) P ( B ∣ C ) P(A,B|C)=P(A|B,C)P(B|C) P(A,B∣C)=P(A∣B,C)P(B∣C)
其中,为了表示方便,将 y(1:k) 用 Y(k) 来表示,注意 Y 与 y 的区别。同时,上面这个式子和上一节贝叶斯滤波中后验概率的推导是一样的,只是之前的x(k)变成了这里的x(0:k),就是这个不同,导致贝叶斯估计里需要积分,而这里后验概率的分解形式却不用积分。
粒子权值的递归形式可以表示为:
注意,这种权重递推形式的推导是在前面(2)式的形式下进行推导的,也就是没有归一化。而在进行状态估计的公式为
这个公式中的的权重是归一化以后的,所以在实际应用中,递推计算出w(k)后,要进行归一化,才能够代入(4)式中去计算期望。
同时,上面(5)式中的分子是不是很熟悉,在上一节贝叶斯滤波中我们都已经做了介绍,p( y|x ),p( x(k)|x(k-1) )的形状实际上和状态方程中噪声的概率分布形状是一样的,只是均值不同了。因此这个递推的(5)式和前面的非递推形式相比,公式里的概率都是已知的,权重的计算可以说没有编程方面的难度了。
权重也有了以后,只要进行稍微的总结就可以得到SIS Filter。
四、Sequential Importance Sampling (SIS) Filter
在实际应用中我们可以假设重要性分布q()满足:
这个假设说明重要性分布只和前一时刻的状态x(k-1)以及测量y(k)有关了,那么(5)式就可以转化为:
在做了这么多假设和为了解决一个个问题以后,终于有了一个像样的粒子滤波算法了,他就是序贯重要性采样滤波。
下面用伪代码的形式给出这个算法:
----------------------pseudo code-----------------------------------
粒子权值归一化。粒子有了,粒子的权重有了,就可以由(4)式,对每个粒子的状态进行加权去估计目标的状态了。
-----------------------end -----------------------------------------------
这个算法就是粒子滤波的前身了。只是在实际应用中,又发现了很多问题,如粒子权重退化的问题,因此就有了重采样( resample ),就有了基本的粒子滤波算法。还有就是重要性概率密度q()的选择问题,等等。都留到下一章 去解决。
在这一章中,我们是用的重要性采样这种方法去解决的后验概率无法采样的问题。实际上,关于如何从后验概率采样,也就是如何生成特定概率密度的样本,有很多经典的方法(如拒绝采样,Markov Chain Monte Carlo,Metropolis-Hastings 算法,Gibbs采样),这里面可以单独作为一个课题去学习了,有兴趣的可以去看看《统计之都 的一篇博文》,强烈推荐,参考文献里的前几个也都不错。
reference:
- Gabriel A. Terejanu 《Tutorial on Monte Carlo Techniques》
- Taylan Cemgil 《A Tutorial Introduction to Monte Carlo methods, Markov Chain Monte Carlo and Particle Filtering》
- M. Sanjeev Arulampalam 《A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking》
- ZHE CHEN 《Bayesian Filtering: From Kalman Filters to Particle Filters, and Beyond》
5.百度文库《粒子滤波理论》- Haykin 《Neural Networks and learning Machines 》Chapter 14
- 统计之都 <LDA-math-MCMC 和 Gibbs Sampling>
五、重采样
在应用SIS 滤波的过程中,存在一个退化的问题。就是经过几次迭代以后,很多粒子的权重都变得很小,可以忽略了,只有少数粒子的权重比较大。并且粒子权值的方差随着时间增大,状态空间中的有效粒子数较少。随着无效采样粒子数目的增加,使得大量的计算浪费在对估计后验滤波概率分布几乎不起作用的粒子上,使得估计性能下降,如图所示。
通常采用有效粒子数来衡量粒子权值的退化程度,即
这个式子的含义是,有效粒子数越小,即权重的方差越大,也就是说权重大的和权重小的之间差距大,表明权值退化越严重。在实际计算中,有效粒子数可以近似为:
在进行序贯重要性采样时,若上式小于事先设定的某一阈值,则应当采取一些措施加以控制。
克服序贯重要性采样算法权值退化现象最直接的方法是增加粒子数,而这会造成计算量的相应增加,影响计算的实时性。因此,一般采用以下两种途径:
(1)选择合适的重要性概率密度函数;
(2)在序贯重要性采样之后,采用重采样方法。
对于第一种方法:选取重要性概率密度函数的一个标准就是使得粒子权值的方差最小。关于这部分内容,还是推荐百度文库的那篇文章《粒子滤波理论》,他在这里也引申出来了几种不同的粒子滤波方法。
这里着重讲第二种方法,重采样。
重采样的思路是:既然那些权重小的不起作用了,那就不要了。要保持粒子数目不变,得用一些新的粒子来取代它们。找新粒子最简单的方法就是将权重大的粒子多复制几个出来,至于复制几个?那就在权重大的粒子里面让它们根据自己权重所占的比例去分配,也就是老大分身分得最多,老二分得次多,以此类推。
下面以数学的形式来进行说明。
前面已经说明了求某种期望问题变成了这种加权和的形式:
通过重采样以后,希望表示成:
,注意对比(1)和(2)。
x
k
(
i
)
x^{(i)}_k
xk(i)是第k时刻的粒子。是k时刻重采样以后的粒子。其中n(i)是指粒子在产生新的粒子集
x
k
(
i
)
x^{(i)}_k
xk(i)时被复制的次数。(2)式中第一个等号说明重采样以后,所有的粒子权重一样,都是1/N,只是有的粒子多出现了n(i)次。
思路有了,就看具体的操作方法了。
在《On resampling algorithms for particle filters》 这篇paper里讲了四种重采样的方法。
这四种方法大同小异。如果你接触过遗传算法的话,理解起来就很容易,就是遗传算法中那种轮盘赌的思想。
在这里用个简单的例子来说明:
假设有3个粒子,在第k时刻的时候,他们的权重分别是0.1, 0.1 ,0.8,
然后计算他们的概率累计和(matlab 中为cumsum() )得到: [0.1, 0.2, 1]。
接着,我们用服从[0,1]之间的均匀分布随机采样3个值,假设为0.15 , 0.38 和 0.54。
也就是说,第二个粒子复制一次,第三个粒子复制两次。
累积分布函数(Cumulative Distribution Function),又叫分布函数,是概率密度函数的积分,能完整描述一个实随机变量X的概率分布。一般以大写CDF标记,,与概率密度函数probability density function(小写pdf)相对。即累积分布函数表示:对离散变量而言,所有小于等于a的值出现概率的和.
可以把概率密度看成是纵坐标,区间看成是横坐标,概率密度对区间的积分就是面积,而这个面积就是事件在这个区间发生的概率,所有面积的和为1。
所以单独分析一个点的概率密度是没有任何意义的,它必须要有区间作为参考和对比。
在MATLAB中一句命令就可以方便的实现这个过程:
[~, j] = histc(rand(N,1), [0 cumsum(w')]); 关于histc的用法可以点击histc用法。
对于上面的过程,还可以对着下面的图加深理解:
将重采样的方法放入之前的SIS算法中,便形成了基本粒子滤波算法。
重采样的思路很简单,但是当你仔细分析权重的计算公式时:
会有疑问,权重大的就多复制几次,这一定是正确的吗?
权重大,如果是分子大,即后验概率大,那说明确实应该在后验概率大的地方多放几个粒子。但权重大也有可能是分母小造成的,这时候的分子也可能小,也就是实际的后验概率也可能小,这时候的大权重可能就没那么优秀了。何况,这种简单的重采样会使得粒子的多样性丢失,到最后可能都变成了只剩一种粒子的分身。
在遗传算法中好歹还引入了变异来解决多样性的问题。
当然,粒子滤波里也有专门的方法:正则粒子滤波,有兴趣的可以查阅相关资料。
至此,整个粒子滤波的流程已经清晰明朗了,在实际应用中还有一些不确定的就是重要性概率密度的选择。
在下一章中,首先引出 SIR 粒子滤波,接着用SIR滤波来进行实践应用。
reference:
- N. J. Gordon 《Beyond the Kalman Filter:Particle filters for tracking applications》
- 百度文库《粒子滤波理论》
- Jeroen D. Hol《On resampling algorithms for particle fi lters》
- Particle filters: How to do resampling?
- Gabriel A. Terejanu 《Tutorial on Monte Carlo Techniques》
六、Sampling Importance Resampling Filter (SIR)
SIR滤波器很容易由前面的基本粒子滤波推导出来,只要对粒子的重要性概率密度函数做出特定的选择即可。在SIR中,选取:
p( x(k)|x(k-1) )这是先验概率,在第一章贝叶斯滤波预测部分已经说过怎么用状态方程来得到它。将这个式子代入到第二章SIS推导出的权重公式中:
得到:
(1)式
由之前的重采样我们知道,实际上每次重采样以后,有 w k − 1 ( i ) = 1 N w^{(i)}_{k-1}=\frac{1}{N} wk−1(i)=N1。
所以(1)式可以进一步简化成:
这里又出来一个概率采样 p ( y k ∣ x k i ) p(y_k|x^i_k) p(yk∣xki),实际怎么得到这个概率,程序里面又怎么去采样呢?
先搞清这个概率
p
(
y
k
∣
x
k
i
)
p(y_k|x^i_k)
p(yk∣xki)的含义,它表示在状态x出现的条件下,测量y出现的概率。
在机器人定位里面就是,在机器人处于位姿x时,此时传感器数据y出现的概率。更简单的例子是,我要找到一个年龄是14岁的男孩(状态x),身高为170(测量y)的概率。要知道y出现的概率,需要知道此时y的分布。这里以第一篇文章的状态方程为例,由系统状态方程可知,测量是在真实值附近添加了一个高斯噪声。因此,y的分布就是以真实测量值为均值,以噪声方差为方差的一个高斯分布。因此,权重的采样过程就是:当粒子处于x状态时,能够得到该粒子的测量y。要知道这个测量y出现的概率,就只要把它放到以真实值为均值,噪声方差为方差的高斯分布里去计算就行了:
到这里,就可以看成SIR只和系统状态方程有关了,不用自己另外去设计概率密度函数,所以在很多程序中都是用的这种方法。
下面以伪代码的形式给出SIR滤波器:
----------------------SIR Particle Filter pseudo code-----------------------------------
-----------------------end -----------------------------------------------
在上面算法中,每进行一次,都必须重采样一次,这是由于权重的计算方式决定的。
分析上面算法中的采样,发现它并没有加入测量y(k)。只是凭先验知识p( x(k)|x(k-1) )进行的采样,而不是用的修正了的后验概率。所以这种算法存在效率不高和对奇异点(outliers)敏感的问题。但不管怎样,SIR确实简单易用。
七、粒子滤波的应用
在这里主要以第一章的状态方程作为例子进行演示。
在这个存在过程噪声和量测噪声的系统中,估计状态x(k)。
clear all
close all
clc
%%初始化相关变量
x = 0.1; % 真值
x_N = 1; % 系统过程噪声的协方差 (由于是一维的,这里就是方差)
x_R = 1; % 测量的协方差
T = 75; % 共进行75次
N = 1000; % 粒子数,越大效果越好,计算量也越大
%%初始化粒子,粒子是符合以x为均值,V为方差的高斯分布
V = 2; %初始分布的方差
x_P = []; % 粒子
% 用一个高斯分布随机的产生初始的粒子
for i = 1:N
x_P(i) = x + sqrt(V) * randn;
end
z_out = [x^2 / 20 + sqrt(x_R) * randn]; %实际测量值
x_out = [x]; %the actual output vector for measurement values.
x_est = [x]; % time by time output of the particle filters estimate
x_est_out = [x_est]; % the vector of particle filter estimates.
%%T为迭代次数
for t = 1:T
%根据状态方程求出先验
x = 0.5*x + 25*x/(1 + x^2) + 8*cos(1.2*(t-1)) + sqrt(x_N)*randn;
%根据观测方程求出似然
z = x^2/20 + sqrt(x_R)*randn;
%N为粒子总数
for i = 1:N
%从先验p(x(k)|x(k-1))中采样
x_P_update(i) = 0.5*x_P(i) + 25*x_P(i)/(1 + x_P(i)^2) + 8*cos(1.2*(t-1)) + sqrt(x_N)*randn;
%计算采样粒子的值,为后面根据似然去计算权重做铺垫
z_update(i) = x_P_update(i)^2/20;
%对每个粒子计算其权重,这里假设量测噪声是高斯分布。所以 w = p(y|x)对应下面的计算公式
P_w(i) = (1/sqrt(2*pi*x_R)) * exp(-(z - z_update(i))^2/(2*x_R));
end
% 权重归一化.
P_w = P_w./sum(P_w);
%% Resampling这里没有用博客里之前说的histc函数,不过目的和效果是一样的
for i = 1 : N
% find( ,1) 返回第一个符合前面条件的数的下标
% 简单说,就是权重大的被选中的概率大
x_P(i) = x_P_update(find(rand <= cumsum(P_w),1)); % 粒子权重大的将多得到后代
end
%状态估计,重采样以后,每个粒子的权重都变成了1/N
x_est = mean(x_P);
% 保存数据以便作图,x为状态方程得到的先验值,z为观测方程得到的似然,x_est为经过粒子滤波后的预估值
x_out = [x_out x];
z_out = [z_out z];
x_est_out = [x_est_out x_est];
end
t = 0:T;
figure(1);
clf
plot(t, x_out, '.-b', t, x_est_out, '-.r','linewidth',3);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('time step'); ylabel('flight position');
legend('True flight position', 'Particle filter estimate');
滤波后的结果如下: