粒子滤波原理与公式推导
粒子滤波原理原理假设有一个系统，服从一阶马尔科夫模型，我们知道它的状态方程，和观测方程如下：(1)xk=f(xk−1,Qk)yk=h(xk,Rk)x_k=f(x_{k-1},Q_k) \tag{1}\\y_k=h(x_k,R_k)xk=f(xk−1,Qk)yk=h(xk,Rk)(1)设已知k−1k-1k−1时刻的概率密度函数p(xk∣y1:k−1)p(x_k|y_{...
原理
假设有一个系统，服从一阶马尔科夫模型，我们知道它的状态方程，和观测方程如下：
x k = f ( x k − 1 , Q k ) y k = h ( x k , R k ) (1) x_k=f(x_{k-1},Q_k) \tag{1}\\ y_k=h(x_k,R_k) xk=f(xk−1,Qk)yk=h(xk,Rk)(1)
设已知 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)，
1，预测，根据状态方程求 f : x k − 1 → x k f:x_{k-1}\to x_k f:xk−1→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(x_k|y_{1:k-1}) &= \int p(x_k,x_{k-1}|y_{1:k-1})dx_{k-1} \\ &= \int p(x_k|x_{k-1},y_{1:k-1})p(x_{k-1}|y_{1:k-1})dx_{k-1} \\ &= \int p(x_k|x_{k-1})p(x_{k-1}|y_{1:k-1})dx_{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
采样过程噪声 Q k Q_k Qk再叠加 f ( x k − 1 ) f(x_{k-1}) f(xk−1)就可以得到 x k x_k xk的分布；
2，更新，根据观测方程求 h : x k → y k h:x_k\to y_k h:xk→yk
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 ) = p ( y k ∣ x k ) p ( x k ∣ y 1 : k − 1 ) ∫ p ( y k ∣ x k ) p ( x k ∣ y 1 : k − 1 ) d x k \begin{aligned} p(x_k|y_{1:k})&=\frac {p(y_k|x_k,y_{1:k-1})p(x_k|y_{1:k-1})}{p(y_k|y_{1:k-1})} \\ &=\frac {p(y_k|x_k)p(x_k|y_{1:k-1})}{p(y_k|y_{1:k-1})} \\ &=\frac {p(y_k|x_k)p(x_k|y_{1:k-1})}{\int p(y_k|x_k)p(x_k|y_{1:k-1})dx_k} \\ \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(yk∣xk)p(xk∣y1:k−1)dxkp(yk∣xk)p(xk∣y1:k−1)
p ( y k ∣ x k ) p(y_k|x_k) p(yk∣xk)与观测噪声 R k R_k Rk的概率分布有关，利用蒙特卡洛采样计算过上式中的分母积分；
蒙特卡洛采样
蒙特卡洛采样解决概率积分难的问题：从概率分布 p ( x ) p(x) p(x)中采样到样本 x 1 , x 2 , ⋯ , x N x_1,x_2,\cdots,x_N x1,x2,⋯,xN,利用这些样本去估计这个分布的某些函数的期望与方差，如下，
E [ f ( x ) ] = ∫ f ( x ) p ( x ) d x ≈ f ( x 1 ) + f ( x 2 ) + ⋯ + f ( x N ) N E[f(x)]=\int f(x)p(x)dx\approx \frac{f(x_1)+f(x_2)+\cdots+f(x_N)}{N} E[f(x)]=∫f(x)p(x)dx≈Nf(x1)+f(x2)+⋯+f(xN)
重要性采样
重要性采样解决分布难预知的问题（分布转换技巧）
E [ f ( x k ) ] = ∫ f ( x k ) p ( x k ∣ y 1 : k ) d x k = ∫ f ( x k ) p ( x k ∣ y 1 : k ) q ( x k ∣ y 1 : k ) q ( x k ∣ y 1 : k ) d x k = ∫ f ( x k ) p ( y 1 : k ∣ x k ) p ( x k ) p ( y 1 : k ) q ( x k ∣ y 1 : k ) q ( x k ∣ y 1 : k ) d x k = ∫ f ( x k ) W k ( x k ) p ( y 1 : k ) q ( x k ∣ y 1 : k ) d x k = 1 p ( y 1 : k ) ∫ f ( x k ) W k ( x k ) q ( x k ∣ y 1 : k ) d x k = ∫ f ( x k ) W k ( x k ) q ( x k ∣ y 1 : k ) d x k ∫ p ( y 1 : k ∣ x k ) p ( x k ) d x k = ∫ f ( x k ) W k ( x k ) q ( x k ∣ y 1 : k ) d x k ∫ W k ( x k ) q ( x k ∣ y 1 : k ) d x k = E q ( x k ∣ y 1 : k ) [ W k ( x k ) f ( x k ) ] E q ( x k ∣ y 1 : k ) [ W k ( x k ) ] ≈ 1 N ∑ i = 1 N W k ( x k ( i ) ) f ( x k ( i ) ) 1 N ∑ i = 1 N W k ( x k ( i ) ) = ∑ i = 1 N W k ( x k ( i ) ) ∑ i = 1 N W k ( x k ( i ) ) f ( x k ( i ) ) = ∑ i = 1 N W ~ k ( x k ( i ) ) f ( x k ( i ) ) \begin{aligned} E[f(x_k)]&=\int f(x_k)p(x_k|y_{1:k})dx_k \\ &=\int f(x_k)\frac{p(x_k|y_{1:k})}{q(x_k|y_{1:k})}q(x_k|y_{1:k})dx_k \\ &=\int f(x_k)\frac{p(y_{1:k}|x_k)p(x_k)}{p(y_{1:k})q(x_k|y_{1:k})}q(x_k|y_{1:k})dx_k \\ &=\int f(x_k)\frac{W_k(x_k)}{p(y_{1:k})}q(x_k|y_{1:k})dx_k \\ &=\frac{1}{p(y_{1:k})}\int f(x_k)W_k(x_k)q(x_k|y_{1:k})dx_k \\ &=\frac{\int f(x_k)W_k(x_k)q(x_k|y_{1:k})dx_k}{\int p(y_{1:k}|x_k)p(x_k)dx_k} \\ &=\frac{\int f(x_k)W_k(x_k)q(x_k|y_{1:k})dx_k}{\int W_k(x_k)q(x_k|y_{1:k})dx_k} \\ &=\frac{E_{q(x_k|y_{1:k})}[W_k(x_k)f(x_k)]}{E_{q(x_k|y_{1:k})}[W_k(x_k)]} \\ &\approx \frac{\frac{1}{N}\sum_{i=1}^{N} W_k(x_k^{(i)})f(x_k^{(i)})}{\frac{1}{N}\sum_{i=1}^{N} W_k(x_k^{(i)})} \\ &=\sum_{i=1}^{N} \frac{W_k(x_k^{(i)})}{\sum_{i=1}^{N} W_k(x_k^{(i)})}f(x_k^{(i)}) \\ &=\sum_{i=1}^{N} \tilde{W}_k(x_k^{(i)})f(x_k^{(i)}) \end{aligned} E[f(xk)]=∫f(xk)p(xk∣y1:k)dxk=∫f(xk)q(xk∣y1:k)p(xk∣y1:k)q(xk∣y1:k)dxk=∫f(xk)p(y1:k)q(xk∣y1:k)p(y1:k∣xk)p(xk)q(xk∣y1:k)dxk=∫f(xk)p(y1:k)Wk(xk)q(xk∣y1:k)dxk=p(y1:k)1∫f(xk)Wk(xk)q(xk∣y1:k)dxk=∫p(y1:k∣xk)p(xk)dxk∫f(xk)Wk(xk)q(xk∣y1:k)dxk=∫Wk(xk)q(xk∣y1:k)dxk∫f(xk)Wk(xk)q(xk∣y1:k)dxk=Eq(xk∣y1:k)[Wk(xk)]Eq(xk∣y1:k)[Wk(xk)f(xk)]≈N1∑i=1NWk(xk(i))N1∑i=1NWk(xk(i))f(xk(i))=i=1∑N∑i=1NWk(xk(i))Wk(xk(i))f(xk(i))=i=1∑NW~k(xk(i))f(xk(i))
权重的递推公式 W k ( i ) W_k^{(i)} Wk(i)
W k ( i ) = W k ( x k ( i ) ) = p ( y 1 : k ∣ x k ) p ( x k ) q ( x k ∣ y 1 : k ) ∝ p ( x k ∣ y 1 : k ) q ( x k ∣ y 1 : k ) ≡ p ( x 0 : k ∣ y 1 : k ) q ( x 0 : k ∣ y 1 : k ) = p ( y k ∣ x 0 : k , y 1 : k − 1 ) p ( x 0 : k ∣ y 1 : k − 1 ) / p ( y k ∣ y 1 : k − 1 ) q ( x 0 : k − 1 ∣ y 1 : k − 1 ) q ( x k ∣ x 0 : k − 1 , y 1 : k ) = p ( y k ∣ x 0 : k , y 1 : k − 1 ) p ( x k ∣ x 0 : k − 1 , y 1 : k − 1 ) p ( x 0 : k − 1 ∣ y 1 : k − 1 ) / p ( y k ∣ y 1 : k − 1 ) q ( x 0 : k − 1 ∣ y 1 : k − 1 ) q ( x k ∣ x 0 : k − 1 , y 1 : k ) = p ( y k ∣ x k ) p ( x k ∣ x k − 1 ) p ( x 0 : k − 1 ∣ y 1 : k − 1 ) / p ( y k ∣ y 1 : k − 1 ) q ( x 0 : k − 1 ∣ y 1 : k − 1 ) q ( x k ∣ x 0 : k − 1 , y 1 : k ) ∝ p ( y k ∣ x k ) p ( x k ∣ x k − 1 ) p ( x 0 : k − 1 ∣ y 1 : k − 1 ) q ( x 0 : k − 1 ∣ y 1 : k − 1 ) q ( x k ∣ x 0 : k − 1 , y 1 : k ) = W k − 1 ( i ) p ( y k ∣ x k ( i ) ) p ( x k ( i ) ∣ x k − 1 ( i ) ) q ( x k ( i ) ∣ x 0 : k − 1 ( i ) , y 1 : k ) \begin{aligned} W_k^{(i)}&=W_k(x_k^{(i)}) \\ &=\frac{p(y_{1:k}|x_k)p(x_k)}{q(x_k|y_{1:k})} \\ &\propto \frac{p(x_k|y_{1:k})}{q(x_k|y_{1:k})} \\ &\equiv \frac{p(x_{0:k}|y_{1:k})}{q(x_{0:k}|y_{1:k})} \\ &=\frac{p(y_k|x_{0:k},y_{1:k-1})p(x_{0:k}|y_{1:k-1})/p(y_k|y_{1:k-1})}{q(x_{0:k-1}|y_{1:k-1})q(x_k|x_{0:k-1},y_{1:k})} \\ &=\frac{p(y_k|x_{0:k},y_{1:k-1})p(x_k|x_{0:k-1},y_{1:k-1})p(x_{0:k-1}|y_{1:k-1})/p(y_k|y_{1:k-1})}{q(x_{0:k-1}|y_{1:k-1})q(x_k|x_{0:k-1},y_{1:k})} \\ &=\frac{p(y_k|x_k)p(x_k|x_{k-1})p(x_{0:k-1}|y_{1:k-1})/p(y_k|y_{1:k-1})}{q(x_{0:k-1}|y_{1:k-1})q(x_k|x_{0:k-1},y_{1:k})} \\ &\propto \frac{p(y_k|x_k)p(x_k|x_{k-1})p(x_{0:k-1}|y_{1:k-1})}{q(x_{0:k-1}|y_{1:k-1})q(x_k|x_{0:k-1},y_{1:k})} \\ &=W_{k-1}^{(i)}\frac{p(y_k|x_k^{(i)})p(x_k^{(i)}|x_{k-1}^{(i)})}{q(x_k^{(i)}|x_{0:k-1}^{(i)},y_{1:k})} \\ \end{aligned} Wk(i)=Wk(xk(i))=q(xk∣y1:k)p(y1:k∣xk)p(xk)∝q(xk∣y1:k)p(xk∣y1:k)≡q(x0:k∣y1:k)p(x0:k∣y1:k)=q(x0:k−1∣y1:k−1)q(xk∣x0:k−1,y1:k)p(yk∣x0:k,y1:k−1)p(x0:k∣y1:k−1)/p(yk∣y1:k−1)=q(x0:k−1∣y1:k−1)q(xk∣x0:k−1,y1:k)p(yk∣x0:k,y1:k−1)p(xk∣x0:k−1,y1:k−1)p(x0:k−1∣y1:k−1)/p(yk∣y1:k−1)=q(x0:k−1∣y1:k−1)q(xk∣x0:k−1,y1:k)p(yk∣xk)p(xk∣xk−1)p(x0:k−1∣y1:k−1)/p(yk∣y1:k−1)∝q(x0:k−1∣y1:k−1)q(xk∣x0:k−1,y1:k)p(yk∣xk)p(xk∣xk−1)p(x0:k−1∣y1:k−1)=Wk−1(i)q(xk(i)∣x0:k−1(i),y1:k)p(yk∣xk(i))p(xk(i)∣xk−1(i))
前提假设重要性分布满足 q ( x k ∣ x 0 : k − 1 , y 1 : k ) = q ( x k ∣ x k − 1 , y k ) q(x_k|x_{0:k-1},y_{1:k})=q(x_k|x_{k-1},y_k) q(xk∣x0:k−1,y1:k)=q(xk∣xk−1,yk)
伪代码
[ { x k ( i ) , w k ( i ) } i = 1 N ] = S I S [ { x k − 1 ( i ) , w k − 1 ( i ) } i = 1 N , y 1 : k ] [\{x_k^{(i)},w_k^{(i)}\}_{i=1}^N]=SIS[\{x_{k-1}^{(i)},w_{k-1}^{(i)}\}_{i=1}^N,y_{1:k}] [{xk(i),wk(i)}i=1N]=SIS[{xk−1(i),wk−1(i)}i=1N,y1:k]
F o r i = 1 : N For\quad i=1:N Fori=1:N
(1)采样： x k ( i ) ∼ q ( x k ( i ) ∣ x k − 1 ( i ) , y k ) x_k^{(i)}\sim q(x_k^{(i)}|x_{k-1}^{(i)},y_k) xk(i)∼q(xk(i)∣xk−1(i),yk)
(2)根据 w k ( i ) ∝ w k − 1 ( i ) p ( y k ∣ x k ( i ) ) p ( x k ( i ) ∣ x k − 1 ( i ) ) q ( x k ( i ) ∣ x k − 1 ( i ) , y k ) w_{k}^{(i)}\propto w_{k-1}^{(i)}\frac{p(y_k|x_k^{(i)})p(x_k^{(i)}|x_{k-1}^{(i)})}{q(x_k^{(i)}|x_{k-1}^{(i)},y_k)} wk(i)∝wk−1(i)q(xk(i)∣xk−1(i),yk)p(yk∣xk(i))p(xk(i)∣xk−1(i))递推计算各个粒子的权重；
重采样
重采样解决粒子权重退化的问题
有“AI”的1024 = 2048，欢迎大家加入2048 AI社区
更多推荐
5
0- 0
已为社区贡献15条内容
所有评论(0)