粒子滤波 particle filter—从贝叶斯滤波到粒子滤波—Part-V(粒子滤波 PF)
Posted 脑壳二
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了粒子滤波 particle filter—从贝叶斯滤波到粒子滤波—Part-V(粒子滤波 PF)相关的知识,希望对你有一定的参考价值。
粒子滤波—从贝叶斯滤波到粒子滤波—Part-V(粒子滤波)
原创不易,路过的各位大佬请点个赞
机动目标跟踪/非线性滤波/传感器融合/导航等探讨代码联系WX: ZB823618313
粒子滤波 —从贝叶斯滤波到粒子滤波—Part-V(粒子滤波 PF)
本博客主要给出基于系统重采样的SIR粒子滤波PF算法流程及代码
针对基于随机重采样、残差重采样以及多项式重采样的SIR粒子滤波PF只给出运行结果
在非线性条件下,贝叶斯滤波面临一个重要问题是状态分布的表达和积分式的求解,由前面章节中的分析可知,对于一般的非线性/非高斯系统,解析求解的途径是行不通的。在数值近似方法中,蒙特卡罗仿真是一种最为通用、有效的手段,粒子滤波就是建立在蒙特卡罗仿真基础之上的,它通过利用一组带权值的系统状态采样来近似状态的统计分布。由于蒙特卡罗仿真方法具有广泛的适用性,由此得到的粒子滤波算法也能适用于一般的非线性/非高斯系统。但是,这种滤波方法也面临几个重要问题,如有效采样(粒子)如何产生、粒子如何传递以及系统状态的序贯估计如何得到等。
简单的理解,粒子滤波就是使用了大量的随机样本,采用蒙特卡洛(MonteCarlo,MC)仿真技术完成贝叶斯递推滤波(Recursive Bayesian Filter)过程。因此本博客从贝叶斯滤波出发,简单介绍粒子滤波PF的出生、即应用
1、贝叶斯滤波
**贝叶斯滤波细节 见Part-I**
考虑离散时间非线性系统动态模型,
x
k
=
f
(
x
k
−
1
,
w
k
−
1
)
z
k
=
h
(
x
k
,
v
k
)
(1)
x_k=f(x_k-1,w_k-1) \\\\ z_k=h(x_k,v_k ) \\tag1
xk=f(xk−1,wk−1)zk=h(xk,vk)(1)
其中
x
k
x_k
xk为
k
k
k时刻的目标状态向量,
z
k
z_k
zk为
k
k
k时刻量测向量(传感器数据)。这里不考虑控制器
u
k
u_k
uk。
w
k
w_k
wk和
v
k
v_k
vk分别是过程噪声序列和量测噪声序列。
w
k
w_k
wk和
v
k
v_k
vk为零均值高斯白噪声。
2、 蒙特卡洛方法MC
**蒙特卡洛近似方法细节 见Part-II**
蒙特卡洛方法是实现的贝叶斯滤波,得到粒子滤波的桥梁。
3、 序贯重要性采样SIS
**序贯重要性采样SIS 见Part-III**
4、 粒子重采样
**序贯重要性采样SIS 见Part-IV**
重采样的思路是:既然那些权重小的不起作用了,那就不要了。要保持粒子数目不变,得用一些新的粒子来取代它们。找新粒子最简单的方法就是将权重大的粒子多复制几个出来,至于复制几个?那就在权重大的粒子里面让它们根据自己权重所占的比例去分配,也就是老大分身分得最多,老二分得次多,以此类推。下面以数学的形式来进行说明。
常用的重采样方法:
系统重采样
多项式重采样
残差重采样
随机重采样
5、 标准的粒子滤波PF
核心思想:是使用一组具有相应权值的随机样本(粒子)来表示状态的后验分布。该方法的基本思路是选取一个重要性概率密度并从中进行随机抽样,得到一些带有相应权值的随机样本后,在状态观测的基础上调节权值的大小。和粒子的位置,再使用这些样本来逼近状态后验分布,最后将这组样本的加权求和作为状态的估计值。粒子滤波不受系统模型的线性和高斯假设约束,采用样本形式而不是函数形式对状态概率密度进行描述,使其不需要对状态变量的概率分布进行过多的约束,因而在非线性非高斯动态系统中广泛应用。尽管如此,粒子滤波目前仍存在计算量过大、粒子退化等关键问题亟待突破。
通常情况下选择先验分布作为重要性密度函数、即
q
(
x
k
∣
x
k
−
1
(
i
)
,
z
k
)
=
p
(
x
k
∣
x
k
−
1
(
i
)
)
q(x_k |x_k-1^(i), z_k)=p(x_k |x_k-1^(i))
q(xk∣xk−1(i),zk)=p(xk∣xk−1(i))
对该函数取重要性权值为
w
k
(
i
)
=
w
k
−
1
(
i
)
p
(
z
k
∣
x
k
(
i
)
)
w_k^(i)=w_k-1^(i)p(z_k |x_k^(i))
wk(i)=wk−1(i)p(zk∣xk(i))
同样
w
k
(
i
)
w_k^(i)
wk(i)需要归一化得到
w
~
k
(
i
)
\\tildew_k^(i)
w~k(i)。
标准的粒子滤波算法步骤为:
粒子滤波PF:
Step 1: 根据 p ( x 0 ) p(x_0) p(x0)采样得到 N N N个粒子 x 0 ( i ) ∼ p ( x 0 ) x_0^(i) \\sim p(x_0) x0(i)∼p(x0)
For i = 2 : N i=2:N i=2:N
Step 2: 根据状态转移函数产生新的粒子为:$ x k ( i ) ∼ p ( x k ∣ x k − 1 ( i ) ) x_k^(i) \\sim p(x_k |x_k-1^(i)) xk(i)∼p(xk∣xk−1(i))
Step 3: 计算重要性权值: w k ( i ) = w k − 1 ( i ) p ( z k ∣ x k ( i ) ) w_k^(i)=w_k-1^(i)p(z_k |x_k^(i)) wk(i)=wk−1(i)p(zk∣xk(i))
Step 4: 归一化重要性权值: w ~ k ( i ) = w k ( i ) ∑ j = 1 N w k ( j ) \\tildew_k^(i)=\\fracw_k^(i)\\sum_j=1^Nw_k^(j) w~k(i)=∑j=1Nwk(j)wk(i)
Step 5: 使用重采样方法对粒子进行重采样(以系统重采样为例)
Step 6: 得到 k k k时刻的后验状态估计:
E [ x ^ k ] = ∑ i = 1 N x k ( i ) w ~ k ( i ) E[\\hatx_k]= \\sum_i=1^Nx_k^(i)\\tildew_k^(i) E[x^k]=i=1∑Nxk(i)w~k(i)
End For
粒子滤波PF算法结构图
6、粒
以上是关于粒子滤波 particle filter—从贝叶斯滤波到粒子滤波—Part-V(粒子滤波 PF)的主要内容,如果未能解决你的问题,请参考以下文章
粒子滤波 particle filter —从贝叶斯滤波到粒子滤波——Part-I(贝叶斯滤波)
粒子滤波 particle filter —从贝叶斯滤波到粒子滤波——Part-I(贝叶斯滤波)
粒子滤波 particle filter—从贝叶斯滤波到粒子滤波—Part-V(粒子滤波 PF)
粒子滤波 particle filter—从贝叶斯滤波到粒子滤波—Part-V(粒子滤波 PF)