Particle Filter 粒子滤波
人都喜欢自以为是。心外无事,就没是了。
本文抄自:https://blog.csdn.net/heyijia0327/article/details/40899819
贝叶斯滤波
假设有一个系统,我们知道它的状态方程和测量方程如下
状态方程:
x k = f k ( x k − 1 , v k − 1 ) ( 1 ) x_k=f_k(x_{k-1},v_{k-1}) \qquad (1) xk=fk(xk−1,vk−1)(1)
如: x k = x k − 1 2 + 25 x k 1 1 + x k − 1 2 + 8 c o s ( 1.2 ( k − 1 ) ) + v k − 1 x_k=\frac{x_{k-1}}{2}+\frac{25x_{k_1}}{1+x^2_{k-1}}+8cos(1.2(k-1))+v_{k-1} xk=2xk−1+1+xk−1225xk1+8cos(1.2(k−1))+vk−1
测量方程:
y k = h k ( x k , n k ) y_k=h_k(x_k,n_k) yk=hk(xk,nk)
如: y k = x k 2 20 + n k ( 2 ) y_{k}=\frac{x^2_{k}}{20}+n_{k} \qquad (2) yk=20xk2+nk(2)
其中 x x x为系统状态, y y y为测量到的数据, f f f, h h h分别是状态转移函数和测量函数, v v v, n n 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 ) p(x_k|x_{k-1}) p(xk∣xk−1)。更新过程则利用最新的测量值对先验概率密度进行修正,得到后验概率密度,也就是对之前的猜测进行修正。
在处理这些问题时,一般都先假设系统的状态转移服从一阶马尔科夫模型,即当前时刻的状态 x k x_k xk只与上一个时刻的状态 x k − 1 x_{k-1} xk−1有关。同时,假设 k k k时刻测量到的数据 y k y_k yk只与当前的状态 x k x_k xk有关,如上面的测量方程2。
假设已知 k − 1 k-1 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 1:k-1 1:k−1时刻的测量数据,那么可以预测下一状态 x k x_k xk出现的概率。
计算推到如下:
p ( x k ∣ y 1 : k − 1 ) = ∫ p ( x k , x k − 1 ∣ y 1 : k − 1 ) d x k − 1 = ∫ p ( x k ∣ x k − 1 , y 1 : k − 1 ) p ( x k − 1 ∣ y 1 : k − 1 ) d x k − 1 = ∫ p ( x k ∣ x k − 1 ) p ( x k − 1 ∣ y 1 : k − 1 ) d x k − 1 \begin{aligned} p\left(x_{k} \mid y_{1: k-1}\right) &=\int p\left(x_{k}, x_{k-1} \mid y_{1: k-1}\right) d x_{k-1} \\ &=\int p\left(x_{k} \mid x_{k-1}, y_{1: k-1}\right) p\left(x_{k-1} \mid y_{1: k-1}\right) d x_{k-1} \\ &=\int p\left(x_{k} \mid x_{k-1}\right) p\left(x_{k-1} \mid y_{1: k-1}\right) d x_{k-1} \end{aligned} p(xk∣y1:k−1)=∫p(xk,xk−1∣y1:k−1)dxk−1=∫p(xk∣xk−1,y1:k−1)p(xk−1∣y1:k−1)dxk−1=∫p(xk∣xk−1)p(xk−1∣y1:k−1)dxk−1
等式的第一行到第二行是贝叶斯公式的应用。第二行得到第三行是由于一阶马尔科夫过程的假设,状态 x k x_k xk只由 x k − 1 x_{k-1} xk−1决定 。
对上式的理解:
(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),表明 x k x_k xk只由 x k − 1 x_{k-1} xk−1决定
而这里 p ( x k ∣ y 1 : k − 1 ) p(x_k|y_{1:k-1}) p(xk∣y1:k−1)表示在已经有了很多测量数据 y y y的情况下,那么可以根据已有的经验进行预测,只是猜测 x k x_k xk, 而不能决定 x k x_k xk。
(2)、公式的最后一行 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)是由系统的状态方程决定的, x k x_k xk由 x k − 1 x_{k-1} xk−1和噪声 v k − 1 v_{k-1} vk−1叠加得到的。
更新:由 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时刻的测量,对上面的预测再进行修正,这就是滤波。这里的后验概率也将是代入到下次的预测,形成递推。
推导:
p ( x k ∣ y 1 : k ) = p ( y k ∣ x k , y 1 : k − 1 ) p ( x k ∣ y 1 : k − 1 ) p ( y k ∣ y 1 : k − 1 ) = p ( y k ∣ x k ) p ( x k ∣ y 1 : k − 1 ) p ( y k ∣ y 1 : k − 1 ) \begin{aligned} p\left(x_{k} \mid y_{1: k}\right) &=\frac{p\left(y_{k} \mid x_{k}, y_{1: k-1}\right) p\left(x_{k} \mid y_{1: k-1}\right)}{p\left(y_{k} \mid y_{1: k-1}\right)} \\ &=\frac{p\left(y_{k} \mid x_{k}\right) p\left(x_{k} \mid y_{1: k-1}\right)}{p\left(y_{k} \mid y_{1: k-1}\right)} \end{aligned} p(xk∣y1:k)=p(yk∣y1:k−1)p(yk∣xk,y1:k−1)p(xk∣y1:k−1)=p(yk∣y1:k−1)p(yk∣xk)p(xk∣y1:k−1)
其中归一化常数:
p ( y k ∣ y 1 : k − 1 ) = ∫ p ( y k ∣ x k ) p ( x k ∣ y 1 : k − 1 ) d x k p(y_k|y_{1:k-1})=\int p(y_k|x_k)p(x_k|y_{1:k-1})dx_k p(yk∣y1:k−1)=∫p(yk∣xk)p(xk∣y1:k−1)dxk
等式第一行到第二行是因为, y k y_k yk只与 x k x_k xk有关, p ( y k ∣ x k ) p(y_k|x_k) p(yk<