一、概述
粒子滤波(Particle Filter)是动态模型的非线性,非高斯的版本,也就是说 z t z_t zt和 z t − 1 z_{t-1} zt−1、 x t x_t xt和 z t z_t zt的关系是非线性的,其噪声也是非高斯的:
z t = g ( z t − 1 , μ , ε ) x t = h ( z t , μ , δ ) z_{t}=g(z_{t-1},\mu ,\varepsilon )\\ x_{t}=h(z_{t},\mu ,\delta ) zt=g(zt−1,μ,ε)xt=h(zt,μ,δ)
对于卡尔曼滤波,可以通过高斯分布的性质直接解得其概率分布。但是对于粒子滤波,由于其状态转移概率和发射概率是任意的,所以没办法得到其概率分布,只能通过采样的方法来进行估计,通常我们在实际应用中更加关心的并非概率分布而是概率分布关于某函数的期望,因此本文主要介绍如何通过采样的方法来求某个函数依概率分布的期望。
二、序列重要性采样
对于分布 p ( z ) p(z) p(z),要求期望 E p ( z ) [ f ( z ) ] E_{p(z)}[f(z)] Ep(z)[f(z)],重要性采样(Importance Sampling)的方法是引入一个新的分布 q ( z ) q(z) q(z),然后对 q ( z ) q(z) q(z)进行采样,其做法如下:
E p ( z ) [ f ( z ) ] = ∫ f ( z ) ⋅ p ( z ) d z = ∫ f ( z ) ⋅ p ( z ) q ( z ) ⋅ q ( z ) d z ≈ 1 N ∑ i = 1 N f ( z i ) ⋅ p ( z i ) q ( z i ) ⏟ 权 重 z i ∼ q ( z ) , i = 1 , 2 , ⋯ , N E_{p(z)}[f(z)]=\int f(z)\cdot p(z)\mathrm{d}z\\ =\int f(z)\cdot \frac{p(z)}{q(z)}\cdot q(z)\mathrm{d}z\\ \approx \frac{1}{N}\sum_{i=1}^{N}f(z_{i})\cdot \underset{权重}{\underbrace{\frac{p(z_{i})}{q(z_{i})}}}\\ z_{i}\sim q(z),i=1,2,\cdots ,N Ep(z)[f(z)]=∫f(z)⋅p(z)dz=∫f(z)⋅q(z)p(z)⋅q(z)dz≈N1i=1∑Nf(zi)⋅权重 q(zi)p(zi)zi∼q(z),i=1,2,⋯,N
在滤波问题中,关心的概率分布是 p ( z t ∣ x 1 : t ) p(z_{t}|x_{1:t}) p(zt∣x1:t),权重就是:
w t ( i ) = p ( z t ( i ) ∣ x 1 : t ) q ( z t ( i ) ∣ x 1 : t ) , i = 1 , 2 , ⋯ , N w_{t}^{(i)}=\frac{p(z_{t}^{(i)}|x_{1:t})}{q(z_{t}^{(i)}|x_{1:t})},i=1,2,\cdots ,N wt(i)=q(zt(i)∣x1:t)p(zt(i)∣x1:t),i=1,2,⋯,N
使用重要性采样的一个问题就是计算量过大,这是因为在每个时刻都要采样 N N N次,然而权重的分子这一项计算一次是非常困难的,因此我们期望得到一个 w t ( i ) w_{t}^{(i)} wt(i)与 w t − 1 ( i ) w_{t-1}^{(i)} wt−1(i)之间的递推关系来简化这个过程,因此便有了序列重要性采样(Sequential Importance Sampling,SIS)。
在SIS中我们不直接使用 p ( z t ( i ) ∣ x 1 : t ) p(z_{t}^{(i)}|x_{1:t}) p(zt(i)∣x1:t)来进行采样,而是使用 p ( z 1 : t ( i ) ∣ x 1 : t ) p(z_{1:t}^{(i)}|x_{1:t}) p(z1:t(i)∣x1:t),这是为了计算上的简便,之所以可以这么用 p ( z 1 : t ( i ) ∣ x 1 : t ) p(z_{1:t}^{(i)}|x_{1:t}) p(z1:t(i)∣x1:t)来进行采样,是因为满足:
w t ( i ) ∝ p ( z 1 : t ( i ) ∣ x 1 : t ) q ( z 1 : t ( i ) ∣ x 1 : t ) w_{t}^{(i)}\propto \frac{p(z_{1:t}^{(i)}|x_{1:t})}{q(z_{1:t}^{(i)}|x_{1:t})} wt(i)∝q(z1:t(i)∣x1:t)p(z1:t(i)∣x1:t)
现在我们首先要找到 p ( z 1 : t ∣ x 1 : t ) p(z_{1:t}|x_{1:t}) p(z1:t∣x1:t)与 p ( z 1 : t − 1 ∣ x 1 : t − 1 ) p(z_{1:t-1}|x_{1:t-1}) p(z1:t−1∣x1:t−1)之间的递推关系:
p ( z 1 : t ∣ x 1 : t ) = p ( z 1 : t , x 1 : t ) p ( x 1 : t ) = 1 C p ( z 1 : t , x 1 : t ) = 1 C p ( x t ∣ z 1 : t , x 1 : t − 1 ) ⋅ p ( z 1 : t , x 1 : t − 1 ) = 1 C p ( x t ∣ z t ) ⋅ p ( z t ∣ z 1 : t − 1 , x 1 : t − 1 ) ⋅ p ( z 1 : t − 1 , x 1 : t − 1 ) = 1 C p ( x t ∣ z t ) ⋅ p ( z t ∣ z t − 1 ) ⋅ p ( z 1 : t − 1 ∣ x 1 : t − 1 ) ⋅ p ( x 1 : t − 1 ) ⏟ D = D C p ( x t ∣ z t ) ⋅ p ( z t ∣ z t − 1 ) ⋅ p ( z 1 : t − 1 ∣ x 1 : t − 1 ) {\color{Red}{p(z_{1:t}|x_{1:t})}}=\frac{p(z_{1:t},x_{1:t})}{p(x_{1:t})}\\ =\frac{1}{C}p(z_{1:t},x_{1:t})\\ =\frac{1}{C}p(x_{t}|z_{1:t},x_{1:t-1})\cdot p(z_{1:t},x_{1:t-1})\\ =\frac{1}{C}p(x_{t}|z_{t})\cdot p(z_{t}|z_{1:t-1},x_{1:t-1})\cdot p(z_{1:t-1},x_{1:t-1})\\ =\frac{1}{C}p(x_{t}|z_{t})\cdot p(z_{t}|z_{t-1})\cdot p(z_{1:t-1}|x_{1:t-1})\cdot \underset{D}{\underbrace{p(x_{1:t-1})}}\\ =\frac{D}{C}p(x_{t}|z_{t})\cdot p(z_{t}|z_{t-1})\cdot {\color{Red}{p(z_{1:t-1}|x_{1:t-1})}} p(z1:t∣x1:t)=p(x1:t)p(z1:t,x1:t)=C1p(z1:t,x1:t)=C1p(xt∣z1:t,x1:t−1)⋅p(z1:t,x1:t−1)=C1p(xt∣zt)⋅p(zt∣z1:t−1,x1:t−1)⋅p(z1:t−1,x1:t−1)=C1p(xt∣zt)⋅p(zt∣zt−1)⋅p(z1:t−1∣x1:t−1)⋅D p(x1:t−1)=CDp(xt∣zt)⋅p(zt∣zt−1)⋅p(z1:t−1∣x1:t−1)
由于 q ( z 1 : t ∣ x 1 : t ) q(z_{1:t}|x_{1:t}) q(z1:t∣x1:t)是我们指定的,因此可以指定:
q ( z 1 : t ∣ x 1 : t ) = q ( z t ∣ z 1 : t − 1 , x 1 : t ) ⋅ q ( z 1 : t − 1 ∣ x 1 : t − 1 ) {\color{Red}{q(z_{1:t}|x_{1:t})}}=q(z_{t}|z_{1:t-1},x_{1:t})\cdot {\color{Red}{q(z_{1:t-1}|x_{1:t-1})}} q(z1:t∣x1:t)=q(zt∣z1:t−1,x1:t)⋅q(z1:t−1∣x1:t−1)
所以有:
w t ( i ) ∝ p ( z 1 : t ( i ) ∣ x 1 : t ) q ( z 1 : t ( i ) ∣ x 1 : t ) ∝ p ( x t ∣ z t ( i ) ) ⋅ p ( z t ( i ) ∣ z t − 1 ( i ) ) ⋅ p ( z 1 : t − 1 ( i ) ∣ x 1 : t − 1 ) q ( z t ( i ) ∣ z 1 : t − 1 ( i ) , x 1 : t ) ⋅ q ( z 1 : t − 1 ( i ) ∣ x 1 : t − 1 ) = p ( x t ∣ z t ( i ) ) ⋅ p ( z t ( i ) ∣ z t − 1 ( i ) ) q ( z t ( i ) ∣ z 1 : t − 1 ( i ) , x 1 : t ) ⋅ w t − 1 ( i ) w_{t}^{(i)}\propto \frac{p(z_{1:t}^{(i)}|x_{1:t})}{q(z_{1:t}^{(i)}|x_{1:t})}\propto \frac{p(x_{t}|z_{t}^{(i)})\cdot p(z_{t}^{(i)}|z_{t-1}^{(i)})\cdot {\color{Red}{p(z_{1:t-1}^{(i)}|x_{1:t-1})}}}{q(z_{t}^{(i)}|z_{1:t-1}^{(i)},x_{1:t})\cdot {\color{Red}{q(z_{1:t-1}^{(i)}|x_{1:t-1})}}}=\frac{p(x_{t}|z_{t}^{(i)})\cdot p(z_{t}^{(i)}|z_{t-1}^{(i)})}{q(z_{t}^{(i)}|z_{1:t-1}^{(i)},x_{1:t})}\cdot w_{t-1}^{(i)} wt(i)∝q(z1:t(i)∣x1:t)p(z1:t(i)∣x1:t)∝q(zt(i)∣z1:t−1(i),x1:t)⋅q(z1:t−1(i)∣x1:t−1)p(xt∣zt(i))⋅p(zt(i)∣zt−1(i))⋅p(z1:t−1(i)∣x1:t−1)=q(zt(i)∣z1:t−1(i),x1:t)p(xt∣zt(i))⋅p(zt(i)∣zt−1(i))⋅wt−1(i)
总结一下,SIS的迭代过程如下:
前提: t − 1 t-1 t−1时刻完成采样并计算得到权重
t t t时刻:
①根据 q ( z t ∣ z 1 : t − 1 ( i ) , x 1 : t ) q(z_{t}|z_{1:t-1}^{(i)},x_{1:t}) q(zt∣z1:t−1(i),x1:t)完成采样得到 z t ( i ) z_{t}^{(i)} zt(i),并且计算权重 w t ( i ) w_{t}^{(i)} wt(i);
②对权重进行归一化, w t ( i ) → w ^ t ( i ) , ∑ i = 1 N w ^ t ( i ) = 1 w_{t}^{(i)}\rightarrow \hat{w}_{t}^{(i)},\sum_{i=1}^{N}\hat{w}_{t}^{(i)}=1 wt(i)→w^t(i),∑i=1Nw^t(i)=1。
三、重要性重采样
重要性重采样(Sampling Importance Resampling,SIR)是在上述SIS基础上进行改进的算法,与SIS相比其添加了两个部分,一个是重采样,另一个是特定的 q ( z t ∣ z 1 : t − 1 , x 1 : t ) q(z_{t}|z_{1:t-1},x_{1:t}) q(zt∣z1:t−1,x1:t)概率分布。
- 重采样
SIS算法会出现权值退化的现象,在一定时间后可能会出现大部分权重都逼近于 0 0 0的情况。简单解释一下,这主要还是由于维度灾难的问题,在高维空间中需要大量的样本。使用重采样可以缓解这一现象。
下面以 N = 3 N=3 N=3为例来阐述重采样的过程,在下表中展示了3个采样粒子的权重(归一化后的权重可以理解为pdf),并且计算了pdf的cdf:
z ( 1 ) z^{(1)} z(1) | z ( 2 ) z^{(2)} z(2) | z ( 3 ) z^{(3)} z(3) | |
---|---|---|---|
0.1 | 0.1 | 0.8 | |
cdf | 0.1 | 0.2 | 1 |
画出例子的cdf图像如下:
重采样是指在均匀分布 u ∼ U ( 0 , 1 ) u\sim U(0,1) u∼U(0,1)上进行采样,然后依照cdf找到对应的新的粒子 z ( i ) z^{(i)} z(i),重采样以后的新的权重 w ^ ^ = 1 N \hat{\hat{w}}=\frac{1}{N} w^^=N1。
在SIS的基础上加上重采样,就是基本的粒子滤波算法(Basic Particle Filter)。
- 选择合适的提议分布
选择恰当的提议分布也是一种解决权重衰减的方法。这里一个常用的选择就是:
q ( z t ∣ z 1 : t − 1 , x 1 : t ) = p ( z t ∣ z t − 1 ) q(z_{t}|z_{1:t-1},x_{1:t})=p(z_{t}|z_{t-1}) q(zt∣z1:t−1,x1:t)=p(zt∣zt−1)
则此时的权重 w t ( i ) w_{t}^{(i)} wt(i)的计算公式就变为了:
w t ( i ) = p ( x t ∣ z t ( i ) ) ⋅ p ( z t ( i ) ∣ z t − 1 ( i ) ) q ( z t ( i ) ∣ z 1 : t − 1 ( i ) , x 1 : t ) ⋅ w t − 1 ( i ) = p ( x t ∣ z t ( i ) ) ⋅ p ( z t ( i ) ∣ z t − 1 ( i ) ) p ( z t ( i ) ∣ z t − 1 ( i ) ) ⋅ w t − 1 ( i ) = p ( x t ∣ z t ( i ) ) ⋅ w t − 1 ( i ) w_{t}^{(i)}=\frac{p(x_{t}|z_{t}^{(i)})\cdot p(z_{t}^{(i)}|z_{t-1}^{(i)})}{q(z_{t}^{(i)}|z_{1:t-1}^{(i)},x_{1:t})}\cdot w_{t-1}^{(i)}=\frac{p(x_{t}|z_{t}^{(i)})\cdot {\color{Red}{p(z_{t}^{(i)}|z_{t-1}^{(i)})}}}{{\color{Red}{p(z_{t}^{(i)}|z_{t-1}^{(i)})}}}\cdot w_{t-1}^{(i)}=p(x_{t}|z_{t}^{(i)})\cdot w_{t-1}^{(i)} wt(i)=q(zt(i)∣z1:t−1(i),x1:t)p(xt∣zt(i))⋅p(zt(i)∣zt−1(i))⋅wt−1(i)=p(zt(i)∣zt−1(i))p(xt∣zt(i))⋅p(zt(i)∣zt−1(i))⋅wt−1(i)=p(xt∣zt(i))⋅wt−1(i)
这种方法叫做生成与测试方法(generate and test),生成过程指的是通过 p ( z t ∣ z t − 1 ( i ) ) p(z_{t}|z_{t-1}^{(i)}) p(zt∣zt−1(i))生成一个新的样本 z ( i ) z^{(i)} z(i)。然后计算权重 w t ( i ) = p ( x t ∣ z t ( i ) ) ⋅ w t − 1 ( i ) w_{t}^{(i)}=p(x_{t}|z_{t}^{(i)})\cdot w_{t-1}^{(i)} wt(i)=p(xt∣zt(i))⋅wt−1(i),这里通过将 w t − 1 ( i ) w_{t-1}^{(i)} wt−1(i)与 p ( x t ∣ z t ( i ) ) p(x_{t}|z_{t}^{(i)}) p(xt∣zt(i))相乘来得到新的权重 w t ( i ) w_{t}^{(i)} wt(i),当生成的样本 z ( i ) z^{(i)} z(i)对应的 p ( x t ∣ z t ( i ) ) p(x_{t}|z_{t}^{(i)}) p(xt∣zt(i))较大时,得到的新的权重 w t ( i ) w_{t}^{(i)} wt(i)也就越大,这就相当于测试过程,这就好比表明样本 z ( i ) z^{(i)} z(i)是比较合适的。
- 算法
在SIS的基础上加上重采样和上述特定的提议分布就得到了SIR算法,其迭代的流程如下:
前提: t − 1 t-1 t−1时刻完成采样并计算得到权重
t t t时刻:
①根据 q ( z t ∣ z 1 : t − 1 ( i ) , x 1 : t ) = p ( z t ∣ z t − 1 ( i ) ) q(z_{t}|z_{1:t-1}^{(i)},x_{1:t})=p(z_{t}|z_{t-1}^{(i)}) q(zt∣z1:t−1(i),x1:t)=p(zt∣zt−1(i))完成采样得到 z t ( i ) z_{t}^{(i)} zt(i),并且计算权重 w t ( i ) = p ( x t ∣ z t ( i ) ) ⋅ w t − 1 ( i ) w_{t}^{(i)}=p(x_{t}|z_{t}^{(i)})\cdot w_{t-1}^{(i)} wt(i)=p(xt∣zt(i))⋅wt−1(i);
②对权重进行归一化, w t ( i ) → w ^ t ( i ) , ∑ i = 1 N w ^ t ( i ) = 1 w_{t}^{(i)}\rightarrow \hat{w}_{t}^{(i)},\sum_{i=1}^{N}\hat{w}_{t}^{(i)}=1 wt(i)→w^t(i),∑i=1Nw^t(i)=1。
③重采样