若由本篇博文增加关注,就解封本篇博文的VIP权限哈,记得在下方留言哈
本周进行了PF-TBD的研究。Firstly,先别说什么是基于粒子滤波的检测前跟踪,你要先知道什么是粒子滤波法吧。下面我将把基本的粒子滤波步骤说一下。
首先,把下面的几句话读一下:
- 先耐着性子把我的博文完完整整地读一遍,有些我没让你去看而你又不知道的知识点,别忙着去查。因为粒子滤波算法推导说麻烦还是有点麻烦的,如果你刚看了几句话就去查这个,又刚看了几句话就去查那个,很快你就会把前面你刚看的粒子滤波的本身的内容搞忘掉了。在推导过程中一定要抓住主线。
- 要了解蒙特卡洛思想,如果你不知道,先看这篇博文:一句话理解到底什么是蒙特卡洛思想。
- 要理解并记住下面这个图,以下的推导都是按照这个图来的哈。
x行,我们称为状态例如物体的速度,位置啥的,满足一阶马尔可夫假设;
z行,我们称为量测,也即观测的东西,例如幅度啥的,某一时刻的量测只与当时时刻的状态有关。 - 本博文按照以下序列进行讲述:卡尔曼滤波——》贝叶斯估计——》蒙特卡罗采样——》重要性采样——》SIS粒子滤波——》重采样。
让我们正式进入正文:
- 讲粒子滤波那就肯定要提一下卡尔曼滤波了,因为粒子滤波可以说是卡尔曼滤波的升华,当然,你不知道卡尔曼滤波也没关系,看看下面这个故事,你就懂了。
一片绿油油的草地上有一条曲折的小径,通向一棵大树。一个要求被提出:从起点沿着小径走到树下。
“很简单。” A说,于是他丝毫不差地沿着小径走到了树下。
现在,难度被增加了:蒙上眼。
“也不难,我当过特种兵。” B说,于是他歪歪扭扭地走到了树 ………. 旁。“唉,好久不练,生疏了。”
“看我的,我有 DIY 的 GPS!” C说,于是他像个醉汉似地走到了树………. 旁。“唉,这个 GPS 软件没做好,漂移太大。”
“我来试试。” 旁边一人拿过 GPS, 蒙上眼,居然沿着小径走到了树下。
“这么厉害!你是什么人?”
“卡尔曼 ! ”
“卡尔曼?!你是卡尔曼?”众人大吃一惊。
“我是说这个 GPS 卡而慢。”
此段引用自highgear的《授之以渔:卡尔曼滤波器…大泄蜜…》。这个小笑话指出了卡尔曼滤波的核心:预测+测量反馈,同时也是粒子滤波的核心。那为什么说粒子滤波是卡尔曼滤波的升华呢?
让我们来看看下面的一组公式:
-
x
k
=
f
(
x
k
−
1
,
w
k
−
1
)
(1)
\boldsymbol{x}_{k}=f\left(\boldsymbol{x}_{k-1}, \boldsymbol{w}_{k-1}\right)\tag{1}
xk=f(xk−1,wk−1)(1)
z k = h ( x k , v k ) (2) z_{k}=h\left(\boldsymbol{x}_{k}, \boldsymbol{v}_{k}\right)\tag{2} zk=h(xk,vk)(2)
ω k : \omega_{k}: ωk:过程噪声,就是在状态转移的过程中产生的噪声
v k : v_{k}: vk:量测噪声,就是观察时引进的噪声,记住这里牵扯到信噪比的概念哈。 - 对于卡尔曼滤波
f
(
)
f()
f()和
h
(
)
h()
h()均是线性的,即斜率为常数,并且过程噪声与量测噪声服从高斯分布。
对于粒子滤波 f ( ) f() f()和 h ( ) h() h()可以是非线性的,并且过程噪声与量测噪声不一定服从高斯分布。 - 那为什么卡尔曼滤波就不能解决非线性非高斯的问题呢?因为在非线性非高斯的情况下,通过计算你会发现,你根本解不出方程。(这里回忆一下我让你看的)一句话理解到底什么是蒙特卡洛思想,你知道该怎么办了吧。
- 咱们再来观察一下 x k x_{k} xk与 z k z_{k} zk的概率分布的问题,我直接给出结论吧: w k w_{k} wk的PDF就是 x k x_{k} xk的PDF( p ( x k ∣ x k − 1 ) p\left(x_{k} | x_{k-1}\right) p(xk∣xk−1)),而 v k v_{k} vk的PDF就是 z k z_{k} zk的PDF( p ( y k ∣ x k ) p\left(y_{k} | x_{ k}\right) p(yk∣xk)),为啥子呢?因为由 x k = f ( x k − 1 , w k − 1 ) \boldsymbol{x}_{k}=f\left(\boldsymbol{x}_{k-1}, \boldsymbol{w}_{k-1}\right) xk=f(xk−1,wk−1)可得,正常情况下 x k x_{k} xk是与 x k − 1 x_{k-1} xk−1一一对应的,但就是出来个 w k w_{k} wk加在了正常情况下的 x k x_{k} xk身上。
- 接下来就开始进行大规模的公式推导了,要有心理准备哈,记得要自己跟着我的思路推一下。
贝叶斯估计:
先上个贝叶斯公式吧: P ( B i ∣ A ) = P ( B i ) P ( A ∣ B i ) ∑ j = 1 n P ( B j ) P ( A ∣ B j ) P\left(B_{i} | A\right)=\frac{P\left(B_{i}\right) P\left(A | B_{i}\right)}{\sum_{j=1}^{n} P\left(B_{j}\right) P\left(A | B_{j}\right)} P(Bi∣A)=∑j=1nP(Bj)P(A∣Bj)P(Bi)P(A∣Bi)
- 从贝叶斯理论的角度来看,状态估计的问题是基于一系列现有数据(后验知识)z_{1: k}来递归计算当前状态的可信度。 这种可信度是概率公式 p ( x k ∣ z 1 : k ) p\left(x_{k} | z_{1: k}\right) p(xk∣z1:k),需要通过预测和更新的两个步骤来递归计算。
- 预测则是根据公式(1)由上一时刻的状态进行当前时刻的状态的预测得到先验概率。而更新过程则是根据最新时刻的观测值来修正我们得到的先验概率,进而得到后验概率。
- 假设已知k-1时刻的概率密度函数
p
(
x
k
−
1
∣
z
1
:
k
−
1
)
p\left(x_{k-1} | z_{1: k-1}\right)
p(xk−1∣z1:k−1),这是上一时刻更新的后验概率。
**预测:**由于我们目前相当于只知道公式(1)和(2),以及这个假设,所以下面我们推导预测的时候肯定是想往 p ( x k − 1 ∣ z 1 : k − 1 ) p\left(x_{k-1} | z_{1: k-1}\right) p(xk−1∣z1:k−1)以及公式(1)(2)上面靠拢(因为我们只知道这个假设以及公式(1)(2),你往别处靠,咱也不知道呀。)
p ( x k ∣ z 1 : k − 1 ) = ∫ p ( x k , x k − 1 ∣ z 1 : k − 1 ) d x k − 1 = ∫ p ( x k ∣ x k − 1 , z 1 : k − 1 ) p ( x k − 1 ∣ z 1 : k − 1 ) d x k − 1 = ∫ p ( x k ∣ x k − 1 ) p ( x k − 1 ∣ z 1 : k − 1 ) d x k − 1 \begin{aligned} p\left(x_{k} | z_{1: k-1}\right) &=\int p\left(x_{k}, x_{k-1} | z_{1: k-1}\right) d x_{k-1} \\ &=\int p\left(x_{k} | x_{k-1},z_{1: k-1}\right) p\left(x_{k-1} | z_{1: k-1}\right) d x_{k-1} \\ &=\int p\left(x_{k} | x_{k-1}\right) p\left(x_{k-1} | z_{1: k-1}\right) d x_{k-1} \end{aligned} p(xk∣z1:k−1)=∫p(xk,xk−1∣z1:k−1)dxk−1=∫p(xk∣xk−1,z1:k−1)p(xk−1∣z1:k−1)dxk−1=∫p(xk∣xk−1)p(xk−1∣z1:k−1)dxk−1
我先说一下从第一步到第二步是应用贝叶斯公式,而从第二步到第三步则是满足一阶马尔可夫假设。有人该问为什么要这样推呢?上面说过了,就是为了往 p ( x k − 1 ∣ z 1 : k − 1 ) p\left(x_{k-1} | z_{1: k-1}\right) p(xk−1∣z1:k−1)以及公式(1)(2)上面靠拢。 - 更新:
p ( x k ∣ z 1 : k ) = p ( z k ∣ x k , z 1 : k − 1 ) p ( x k ∣ z 1 : k − 1 ) p ( z k ∣ z 1 : k − 1 ) = p ( z k ∣ x k ) p ( x k ∣ z 1 : k − 1 ) p ( z k ∣ z 1 : k − 1 ) = p ( z k ∣ x k ) p ( x k ∣ z 1 : k − 1 ) ∫ p ( z k ∣ x k ) p ( x k ∣ z 1 : k − 1 ) d x k \begin{aligned} p\left(x_{k} | z_{1: k}\right) &=\frac{p\left(z_{k} | x_{k}, z_{1: k-1}\right) p\left(x_{k} | z_{1: k-1}\right)}{p\left(z_{k} |z_{1: k-1}\right)} \\ &=\frac{p\left(z_{k} | x_{k}\right) p\left(x_{k} |z_{1: k-1}\right)}{p\left(z_{k} | z_{1: k-1}\right)} \\&=\frac{p\left(z_{k} | x_{k}\right) p\left(x_{k} | z_{1: k-1}\right)}{\int p\left(z_{k} | x_{k}\right) p\left(x_{k} | z_{1: k-1}\right) d x_{k}} \end{aligned} p(xk∣z1:k)=p(zk∣z1:k−1)p(zk∣xk,z1:k−1)p(xk∣z1:k−1)=p(zk∣z1:k−1)p(zk∣xk)p(xk∣z1:k−1)=∫p(zk∣xk)p(xk∣z1:k−1)dxkp(zk∣xk)p(xk∣z1:k−1)
- 蒙特卡洛采样
你发现在第2点中,已经把粒子滤波讲完了呀,但是由于非线性非高斯,导致上面的公式你根本就解不出来,所以我们要采用粒子滤波的思想(再所以一下,你编程不应该从上一步开始想着咋编,而是从这一步开始想哈)。再次提一下博文一句话理解到底什么是蒙特卡洛思想,别嫌我烦,要理解粒子滤波,蒙特卡洛思想你必须要理解,而不仅仅限于知道。我们要进行滤波,其实就是求当前状态的期望值:
E [ f ( x n ) ] ≈ ∫ f ( x n ) p ^ ( x n ∣ z 1 : k ) d x n = 1 N ∑ i = 1 N ∫ f ( x n ) δ ( x n − x n ( i ) ) d x n = 1 N ∑ i = 1 N f ( x n ( i ) ) \begin{aligned} E\left[f\left(x_{n}\right)\right] & \approx \int f\left(x_{n}\right) \hat{p}\left(x_{n} |z_{1: k}\right) d x_{n} \\ &=\frac{1}{N} \sum_{i=1}^{N} \int f\left(x_{n}\right) \delta\left(x_{n}-x_{n}^{(i)}\right) d x_{n} \\ &=\frac{1}{N} \sum_{i=1}^{N} f\left(x_{n}^{(i)}\right) \end{aligned} E[f(xn)]≈∫f(xn)p^(xn∣z1:k)dxn=N1i=1∑N∫f(xn)δ(xn−xn(i))dxn=N1i=1∑Nf(xn(i))
其中 p ^ ( x n ∣ z 1 : k ) \hat{p}\left(x_{n} | z_{1: k}\right) p^(xn∣z1:k)为经蒙特卡洛采样后的后验概率, p ^ ( x n ∣ z 1 : k ) = 1 N ∑ i = 1 N δ ( x n − x n ( i ) ) ≈ p ( x n ∣ z 1 : k ) \hat{p}\left(x_{n} | z_{1: k}\right)=\frac{1}{N} \sum_{i=1}^{N} \delta\left(x_{n}-x_{n}^{(i)}\right) \approx p\left(x_{n} |z_{1: k}\right) p^(xn∣z1:k)=N1∑i=1Nδ(xn−xn(i))≈p(xn∣z1:k),这里 x n − x n ( i ) x_{n}-x_{n}^{(i)} xn−xn(i)还记不记得上面我说的第1点的最后一点。如果你实在理解不了,可以直接看 E [ f ( x n ) ] = 1 N ∑ i = 1 N f ( x n ( i ) ) E\left[f\left(x_{n}\right)\right] =\frac{1}{N} \sum_{i=1}^{N} f\left(x_{n}^{(i)}\right) E[f(xn)]=N1∑i=1Nf(xn(i)),这个公式能看懂吧,期望依概率收敛均值。记住采样是按照后验概率采样的哈。