粒子滤波 particle filter —从贝叶斯滤波到粒子滤波—Part-III(重要性采样&序贯重要性采样SIS)
原创不易,路过的各位大佬请点个赞
机动目标跟踪/非线性滤波/传感器融合/导航等探讨代码联系WX: ZB823618313
粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-III(重要性采样&序贯重要性采样SIS)
在非线性条件下,贝叶斯滤波面临一个重要问题是状态分布的表达和积分式的求解,由前面章节中的分析可知,对于一般的非线性/非高斯系统,解析求解的途径是行不通的。在数值近似方法中,蒙特卡罗仿真是一种最为通用、有效的手段,粒子滤波就是建立在蒙特卡罗仿真基础之上的,它通过利用一组带权值的系统状态采样来近似状态的统计分布。由于蒙特卡罗仿真方法具有广泛的适用性,由此得到的粒子滤波算法也能适用于一般的非线性/非高斯系统。但是,这种滤波方法也面临几个重要问题,如有效采样(粒子)如何产生、粒子如何传递以及系统状态的序贯估计如何得到等。
简单的理解,粒子滤波就是使用了大量的随机样本,采用蒙特卡洛(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 ) \tag{1}
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为零均值高斯白噪声。
定义
1
1
1 ~
k
k
k时刻对状态
x
k
x_k
xk的所有测量数据为
z
k
=
[
z
1
T
,
z
2
T
,
⋯
,
z
k
T
]
T
z^k=[z_1^T,z_2^T,\cdots,z_k^T]^T
zk=[z1T,z2T,⋯,zkT]T
根据Part-I,
k
k
k时刻状态
x
k
x_k
xk的后验概率密度函数:
p
(
x
k
∣
z
k
)
=
p
(
z
k
∣
x
k
)
p
(
x
k
∣
z
k
−
1
)
p
(
z
k
∣
z
k
−
1
)
p(x_k |z^{k})=\frac{p(z_k |x_k)p(x_k |z^{k-1})}{p(z_k |z^{k-1})}
p(xk∣zk)=p(zk∣zk−1)p(zk∣xk)p(xk∣zk−1)
通过后验分布
p
(
x
k
∣
z
k
)
p(x_k |z^{k})
p(xk∣zk)可以得到
x
k
x_k
xk的最小均方误差(MMSE)估计为
x
^
k
=
E
[
x
k
∣
z
k
]
=
∫
x
k
p
(
x
k
∣
z
k
)
d
x
k
(2)
\hat{x}_k=E[x_k|z_k]=\int x_kp(x_k |z^{k}) dx_k \tag{2}
x^k=E[xk∣zk]=∫xkp(xk∣zk)dxk(2)
2、 蒙特卡洛方法MC
**蒙特卡洛近似方法细节 见Part-I**
根据Part-II蒙特卡洛方法,
x
k
(
i
)
,
i
=
1
,
2
,
⋯
,
N
x_k^{(i)}, i=1,2,\cdots,N
xk(i),i=1,2,⋯,N表示从后验概率分布函数
p
(
x
k
∣
z
k
)
p(x_k |z^{k})
p(xk∣zk)采样得到的
N
N
N个独立同分布的样本,则状态的后验概率密度可以通过如下经验公式近似得到:
p
(
x
k
∣
z
k
)
=
1
N
∑
i
=
1
N
δ
(
x
k
−
x
k
(
i
)
)
p(x_k |z^{k})=\frac{1}{N}\sum_{i=1}^N\delta(x_k-x_k^{(i)})
p(xk∣zk)=N1i=1∑Nδ(xk−xk(i))
同时后验条件期望可近似表示为
x
^
k
=
E
[
x
k
∣
z
k
]
≈
E
^
[
x
k
∣
z
k
]
≈
1
N
∑
i
=
1
N
x
k
(
i
)
,
E
[
g
(
x
k
)
∣
z
k
]
≈
E
^
[
g
(
x
k
)
∣
z
k
]
≈
1
N
∑
i
=
1
N
g
(
x
k
(
i
)
)
(3)
\hat{x}_k=E[x_k|z^{k}]\approx\hat{E}[x_k|z^{k}]\approx\frac{1}{N}\sum_{i=1}^Nx_k^{(i)}, \tag{3}\\ E[g(x_k)|z^{k}]\approx\hat{E}[g(x_k)|z^{k}]\approx\frac{1}{N}\sum_{i=1}^Ng(x_k^{(i)})
x^k=E[xk∣zk]≈E^[xk∣zk]≈N1i=1∑Nxk(i),E[g(xk)∣zk]≈E^[g(xk)∣zk]≈N1i=1∑Ng(xk(i))(3)
蒙特卡洛方法是实现的贝叶斯滤波,得到粒子滤波的桥梁。
3、 重要性采样
需要指出的是,从后验滤波分布 p ( x k ∣ z k ) p(x_k |z^{k}) p(xk∣zk)进行采样近似只是蒙特卡罗仿真技术的一种特殊形式,实际上更一般的情况是从完整的后验分布 p ( x 1 : k ∣ z 1 : k ) p(x_{1:k} |z_{1:k}) p(x1:k∣z1:k)中进行采样、即全状态采样。(这里, z 1 : k z_{1:k} z1:k和 z k z^{k} zk等价)
如果我们能够对后验概率密度函数 p ( x 0 : k ∣ z 1 : k ) p(x_{0:k} |z_{1:k}) p(x0:k∣z1:k)进行随机采样,便可通过对样本进行加权求和来近似随机变量 x k x_k xk以及 g ( x k ) g(x_k) g(xk)的数学期望。
然而通常情况下,对于一般化的非线性系统,无法从后验概率密度函数
p
(
x
0
:
k
∣
z
1
:
k
)
p(x_{0:k} |z_{1:k})
p(x0:k∣z1:k)中直接进行采样,常规的解决方法是从某一已知的、 容易采样的函数
q
(
x
0
:
k
∣
z
1
:
k
)
q(x_{0:k} |z_{1:k})
q(x0:k∣z1:k)中间接采样,这种函数称为重要性密度函数(Importance DensityFunction,IDF), 而这种采样方法称为重要性采样(Importance Sampling, IS)。 为此可以得到
其中
w
(
x
0
:
k
)
w(x_{0:k})
w(x0:k)称之为重要性权值,
w
(
x
0
:
k
)
=
p
(
z
1
:
k
∣
x
0
:
k
)
p
(
x
0
:
k
)
q
(
x
0
:
k
∣
z
1
:
k
)
w(x_{0:k})=\frac{p(z_{1:k} |x_{0:k})p(x_{0:k})}{q(x_{0:k} |z_{1:k})}
w(x0:k)=q(x0:k∣z1:k)p(z1:k∣x0:k)p(x0:k)
由于
p
(
z
1
:
k
)
p(z_{1:k})
p(z1:k)是未知的,
E
[
g
(
x
0
:
k
)
]
E[g(x_{0:k})]
E[g(x0:k)]可以进一步表示为
即从概率分布
q
(
x
0
:
k
∣
z
1
:
k
)
q(x_{0:k} |z_{1:k})
q(x0:k∣z1:k)中采样,得到一组粒子
x
0
:
k
(
i
)
,
i
=
1
,
2
,
⋯
,
N
x_{0:k}^{(i)}, i=1,2,\cdots,N
x0:k(i),i=1,2,⋯,N。将重要性权值
w
(
x
0
:
k
)
w(x_{0:k})
w(x0:k)简记为
w
k
w_k
wk,则后验期望的估计表示为:
E
[
g
(
x
0
:
k
)
∣
z
k
]
≈
E
^
[
g
(
x
0
:
k
)
∣
z
k
]
≈
1
N
∑
i
=
1
N
g
(
x
0
:
k
(
i
)
)
w
k
(
i
)
1
N
∑
i
=
1
N
w
k
(
i
)
=
∑
i
=
1
N
g
(
x
0
:
k
(
i
)
)
w
~
k
(
i
)
E[g(x_{0:k})|z^{k}]\approx\hat{E}[g(x_{0:k})|z^{k}]\approx\frac{\frac{1}{N}\sum_{i=1}^Ng(x_{0:k}^{(i)})w_k^{(i)}}{\frac{1}{N}\sum_{i=1}^Nw_k^{(i)}} = \sum_{i=1}^Ng(x_{0:k}^{(i)})\tilde{w}_k^{(i)}
E[g(x0:k)∣zk]≈E^[g(x0:k)∣zk]≈N1∑i=1Nwk(i)N1∑i=1Ng(x0:k(i))wk(i)=i=1∑Ng(x0:k(i))w~k(i)
其中KaTeX parse error: Expected '}', got 'EOF' at end of input: …tilde{w}_k^{(i)为归一化重要性权值:
w
~
k
(
i
)
=
w
k
(
i
)
∑
j
=
1
N
w
k
(
j
)
(4)
\tilde{w}_k^{(i)}=\frac{w_k^{(i)}}{\sum_{j=1}^Nw_k^{(j)}} \tag{4}
w~k(i)=∑j=1Nwk(j)wk(i)(4)
式(4)的估计结果是有偏的,因为其包括-部分估计值。但可以通过以下两个假设获得近收敛和 E ^ [ g ( x 0 : k ) ∣ z k ] \hat{E}[g(x_{0:k})|z^{k}] E^[g(x0:k)∣zk]的中心极限定理。
- x 0 : k ( i ) x_{0:k}^{(i)} x0:k(i)是一组从建议分布中采样得到的粒子, E ^ [ g ( x 0 : k ) ∣ z k ] \hat{E}[g(x_{0:k})|z^{k}] E^[g(x0:k)∣zk]存在并且有限。
- 通过后验分布得到的 w k w_k wk和 w k g 2 ( x 0 : k ) w_kg^2(x_{0:k}) wkg2(x0:k)的期望存在且有限。
- 验证第二个假设成立的一一个充分条件是
g
(
x
0
:
k
)
g(x_{0:k})
g(x0:k)的方差和重要性权值有界。因此, 随着
N
N
N趋于无穷大,后验密度函数可以用点估计近似,即
p ( x 0 : k ∣ z 1 : k ) = 1 N ∑ i = 1 N δ ( x 0 : k − x 0 : k ( i ) ) w ~ k ( i ) p(x_{0:k} |z_{1:k})=\frac{1}{N}\sum_{i=1}^N\delta(x_{0:k}-x_{0:k}^{(i)})\tilde{w}_k^{(i)} p(x0:k∣z1:k)=N1i=1∑Nδ(x0:k−x0:k(i))w~k(i)
其中 δ \delta δ为狄克里函数。
只要得到了粒子 x 0 : k ( i ) x_{0:k}^{(i)} x0:k(i) 及其对应的归一化的重要性权重 w ~ k ( i ) \tilde{w}_k^{(i)} w~k(i),便可通过上式近似估计期望 E [ g ( x 0 : k ) ∣ z k ] E[g(x_{0:k})|z^{k}] E[g(x0:k)∣zk]。简言之,重要性采样要解决的是原分布难以采样甚至无法采样的问题。不同的粒子都有它们相应的权重,如果粒子权重大,说明信任该粒子比较多。
到这里已经解决了不能从后验概率直接采样的问题,但是上面这种每个粒子的权重都直接计算的方法,效率低,因为每增加一个采样, p ( x 0 : k ∣ z 1 : k ) p(x_{0:k} |z_{1:k}) p(x0:k∣z1:k)都得重新计算,并且还不好计算这个式子。每次计算过程都需要从 1 1 1到 k k k时刻的多有测量,不仅时间靠前的测量数据被重复利用,而且随着时间测量增多计算复杂度会急剧上升。
所以求权重时能否避开计算 p ( x 0 : k ∣ z 1 : k ) p(x_{0:k} |z_{1:k}) p(x0:k∣z1:k)?而最佳的形式是能够以递推的方式去计算权重,这就是所谓的序贯重要性采样(SIS),粒子滤波的原型。
4、 序贯重要性采样SIS
前面的贝叶斯重要性采样是一种常用的蒙特卡洛积分方法,但其不能直接用来进行递推估计,主要因为估计
p
(
x
0
:
k
∣
z
1
:
k
)
p(x_{0:k} |z_{1:k})
p(x0:k∣z1:k)的过程需要用到所有的量测信息
k
k
k,然而每次在
k
+
1
k+1
k+1时刻更新量测信息
z
k
+
1
z_{k+1}
zk+1时,则需要重新计算整个状态序列的重要性权值,所以其计算量将随时间的推移而大量增加。为了解决这一问题, 序贯重要性采样( Sequential Importance Sampling,SIS) 方法得以提出。这种方法在
k
k
k时刻采样时不会改动过去的状态序列xx_,而重要性密度函数分解为
q
(
x
0
:
k
∣
z
1
:
k
)
=
q
(
x
k
∣
x
0
:
k
−
1
,
z
1
:
k
)
q
(
x
0
:
k
−
1
∣
z
1
:
k
−
1
)
(5)
q(x_{0:k} |z_{1:k})=q(x_{k} |x_{0:k-1},z_{1:k})q(x_{0:k-1} |z_{1:k-1})\tag{5}
q(x0:k∣z1:k)=q(xk∣x0:k−1,z1:k)q(x0:k−1∣z1:k−1)(5)
系统模型满足以下假设。
- (1)系统状态符合- -阶马尔可夫过程。
- (2)在给定状态下,不同量测值是相互条件独立的。
- (3)初始的先验密度为
p
(
x
0
)
p(x_0)
p(x0).
即满足
p ( x 0 : k ) = p ( x 0 ) ∏ j = 1 k p ( x j ∣ x j − 1 ) p ( z 1 : k ∣ x 0 : k ) = ∏ j = 1 k p ( z j ∣ x j ) p(x_{0:k}) =p(x_0)\prod_{j=1}^k p(x_{j} |x_{j-1}) \\ p(z_{1:k}|x_{0:k} ) = \prod_{j=1}^k p(z_{j} |x_{j}) p(x0:k)=p(x0)j=1∏kp(xj∣xj−1)p(z1:k∣x0:k)=j=1∏kp(zj∣xj)
将上式代入到(5)中,得到重要性权值的递推形式
在给定合适的重要性密度函数
q
(
x
k
∣
x
0
:
k
−
1
,
z
1
:
k
)
q(x_{k} |x_{0:k-1},z_{1:k})
q(xk∣x0:k−1,z1:k)的条件下,利用上式可以通.
过递归方法简化重要性权值的计算。
此外,如果重要性密度函数满足
q
(
x
k
∣
x
0
:
k
−
1
,
z
1
:
k
)
=
q
(
x
k
∣
x
k
−
1
,
z
k
)
q(x_{k} |x_{0:k-1},z_{1:k})=q(x_{k} |x_{k-1},z_{k})
q(xk∣x0:k−1,z1:k)=q(xk∣xk−1,zk)
只依赖
x
k
−
1
,
z
k
x_{k-1},z_{k}
xk−1,zk。
实际上上式很容易满足:因为这是马尔科过程的基本性质。而贝叶斯滤波的非线性系统就为马尔可夫过程。因此他是满足的。
由该函数采样得到粒子:
x
k
(
i
)
∼
q
(
x
k
∣
x
k
−
1
,
z
1
:
k
)
x_k^{(i)} \sim q(x_{k} |x_{k-1},z_{1:k})
xk(i)∼q(xk∣xk−1,z1:k)
并得到归一化重要性权值为:
w
~
k
(
i
)
=
w
~
k
−
1
(
i
)
p
(
x
k
(
i
)
∣
x
k
−
1
(
i
)
)
p
(
z
k
∣
x
k
(
i
)
)
q
(
x
k
(
i
)
∣
x
k
−
1
(
i
)
,
z
k
)
(6)
\tilde{w}_k^{(i)}=\tilde{w}_{k-1}^{(i)}\frac{p(x_k^{(i)} |x_{k-1}^{(i)})p(z_{k}|x_k^{(i)})}{q(x_k^{(i)} |x_{k-1}^{(i)}, z_{k})}\tag{6}
w~k(i)=w~k−1(i)q(xk(i)∣xk−1(i),zk)p(xk(i)∣xk−1(i))p(zk∣xk(i))(6)
其中归一化权值可表示为
w
~
k
(
i
)
=
w
k
(
i
)
∑
j
=
1
N
w
k
(
j
)
\tilde{w}_k^{(i)}=\frac{w_k^{(i)}}{\sum_{j=1}^Nw_k^{(j)}}
w~k(i)=∑j=1Nwk(j)wk(i)
从而得到后验密度函数的估计为
p
(
x
k
∣
z
1
:
k
)
=
1
N
∑
i
=
1
N
δ
(
x
k
−
x
k
(
i
)
)
w
~
k
(
i
)
p(x_{k} |z_{1:k})=\frac{1}{N}\sum_{i=1}^N\delta(x_{k}-x_{k}^{(i)})\tilde{w}_k^{(i)}
p(xk∣z1:k)=N1i=1∑Nδ(xk−xk(i))w~k(i)
其中
δ
\delta
δ为狄克里函数。
序贯重要性采样算法根据每一步接收到新的量测信息,逐次进行采样粒子和重要性权值的递推,算法步骤如下:
算法:序贯重要性采样SIS
For i = 1 : N i=1:N i=1:N
Step 1: 初始时刻粒子重要性采样 x 0 ( i ) ∼ p ( x 0 ) x_0^{(i)} \sim p(x_{0}) x0(i)∼p(x0)
Step 2: 计算初始时刻重要性权值 w ~ 0 ( i ) = 1 N \tilde{w}_0^{(i)}=\frac{1}{N} w~0(i)=N1
End For
SIS核心步骤START:
For i = 1 : N i=1:N i=1:N
Step 3: 采样粒子 x k ( i ) ∼ q ( x k ∣ x k − 1 , z 1 : k ) x_k^{(i)} \sim q(x_{k} |x_{k-1},z_{1:k}) xk(i)∼q(xk∣xk−1,z1:k)
Step 4: 根据 w ~ k ( i ) = w ~ k − 1 ( i ) p ( x k ( i ) ∣ x k − 1 ( i ) ) p ( z k ∣ x k ( i ) ) q ( x k ( i ) ∣ x k − 1 ( i ) , z k ) \tilde{w}_k^{(i)}=\tilde{w}_{k-1}^{(i)}\frac{p(x_k^{(i)} |x_{k-1}^{(i)})p(z_{k}|x_k^{(i)})}{q(x_k^{(i)} |x_{k-1}^{(i)}, z_{k})} w~k(i)=w~k−1(i)q(xk(i)∣xk−1(i),zk)p(xk(i)∣xk−1(i))p(zk∣xk(i))
计算立在的权值 w ~ k ( i ) \tilde{w}_k^{(i)} w~k(i)
End For
Step 5: 粒子重要性权重归一化
w ~ k ( i ) = w k ( i ) ∑ j = 1 N w k ( j ) \tilde{w}_k^{(i)}=\frac{w_k^{(i)}}{\sum_{j=1}^Nw_k^{(j)}} w~k(i)=∑j=1Nwk(j)wk(i)
SIS核心步骤END.
Step 6: 后验期望估计
E [ g ( x k ) ∣ z k ] ≈ ∑ i = 1 N g ( x k ( i ) ) w ~ k ( i ) E[g(x_{k})|z^{k}]\approx \sum_{i=1}^Ng(x_{k}^{(i)})\tilde{w}_k^{(i)} E[g(xk)∣zk]≈i=1∑Ng(xk(i))w~k(i)
5. PF的目标跟踪应用:
5.1. 仿真参数
**一、目标模型:CV(细节见另一个博客) **
X k + 1 = [ 1 T 0 0 0 1 0 0 0 0 1 T 0 0 0 1 ] X k + [ T 2 / 2 0 T 0 0 T 2 / 2 0 T ] W k X_{k+1}=\begin{bmatrix}1&T&0&0\\0&1&0&0\\0&0&1&T\\0&0&0&1 \end{bmatrix}X_{k} + \begin{bmatrix}T^2/2&0\\T&0\\0&T^2/2\\0&T\end{bmatrix}W_k Xk+1=⎣⎢⎢⎡1000T100001000T1⎦⎥⎥⎤Xk+⎣⎢⎢⎡T2/2T0000T2/2T⎦⎥⎥⎤Wk
CV CT 模型的具体方程形式见另一个博客
二、测量模型:2D主动雷达
在二维情况下,雷达量测为距离和角度
r
k
m
=
r
k
+
r
~
k
b
k
m
=
b
k
+
b
~
k
{r}_k^m=r_k+\tilde{r}_k\\ b^m_k=b_k+\tilde{b}_k
rkm=rk+r~kbkm=bk+b~k
其中
r
k
=
(
x
k
−
x
0
)
+
(
y
k
−
y
0
)
2
)
b
k
=
tan
−
1
y
k
−
y
0
x
k
−
x
0
r_k=\sqrt{(x_k-x_0)^+(y_k-y_0)^2)}\\ b_k=\tan^{-1}{\frac{y_k-y_0}{x_k-x_0}}\\
rk=(xk−x0)+(yk−y0)2)bk=tan−1xk−x0yk−y0
[
x
0
,
y
0
]
[x_0,y_0]
[x0,y0]为雷达坐标,一般情况为0。雷达量测为
z
k
=
[
r
k
,
b
k
]
′
z_k=[r_k,b_k]'
zk=[rk,bk]′。雷达量测方差为
R
k
=
cov
(
v
k
)
=
[
σ
r
2
0
0
σ
b
2
]
R_k=\text{cov}(v_k)=\begin{bmatrix}\sigma_r^2 & 0 \\0 & \sigma_b^2 \end{bmatrix}
Rk=cov(vk)=[σr200σb2]
5.2. 跟踪轨迹
5.3. 跟踪误差(RMSE)
PF的妈妈贝叶斯滤波、基于标准的粒子滤波见Part-I和Part-V
原创不易,路过的各位大佬请点个赞