伯努利滤波器(七)——伯努利粒子滤波器

强度模型下的伯努利粒子滤波:

输入: 存在概率 \(\varphi\), \(\{\omega^i_{k-1},\mathbf{m}^i_{k-1}\}^{N+N_B}_{i=1}\), \(\mathbf{z}_k\)

  1. 预测存在概率 \(q_{k|k-1} = p_b(1-q_{k-1})+p_sq_{k-1}\)
  2. 取样\(\mathbf{m}^i_{k|k-1} \sim \varrho _k(\mathbf{m}_k|\mathbf{m}^i_{k-1},\mathbf{z}_k)\) for \(i = 1,...,N+ N_B\)
  3. 预测概率 \(\omega^i_{k|k-1}\)\(\omega^i_{k|k-1} = \left \{ \begin{matrix} \frac{p_sq_{k-1}}{q_{k|k-1}}\frac{\pi_{k|k-1}(\mathbf{m}^i_{k|k-1}|\mathbf{m}^i_{k-1})}{\varrho_k(\mathbf{m}^i_{k|k-1}|\mathbf{m}^i_{k-1},\mathbf{Z}_k)}\omega^i_{k-1} & i = 1,...,N \\ \frac{p_b(1-q_{k-1})}{q_{k|k-1}}\frac{\pi_{k|k-1}(\mathbf{m}^i_{k|k-1}|\mathbf{m}^i_{k-1})}{\varrho_k(\mathbf{m}^i_{k|k-1}|\mathbf{m}^i_{k-1},\mathbf{Z}_k)}\frac{1}{B} & i = N+1,...,N+B \end{matrix} \right .\)
  4. 计算likelihood \(h_k(\mathbf{z}_k|\emptyset) = \Pi^n_{s=1} g^r_0(z^r_k)\)
  5. for \(i = 1,...,N+N_B\) do
    1. 计算likelihood \(h_k(\mathbf{z}_k|\mathbf{m}^i_{k|k-1}) =\Pi^n_{s=1} g^r_1(z^r_k |\mathbf{m}^i_{k|k-1})\),
    2. 计算likelihood 比率 \(l_k(\mathbf{z}_k|\mathbf{m})= \frac{h_k(\mathbf{z}_k|{\mathbf{m}{}})}{h_k(\mathbf{z}_k|\emptyset)}=\Pi^n_{r=1} \frac{g^r_1(\mathbf{z}^r_k|\mathbf{m})}{g^r_0(\mathbf{z}^r_k)}\)
  6. end for
  7. 近似积分\(I_k \approx\sum^{N+N_{B}}_{i=1}l_k(\mathbf{z}|\mathbf{m}^i_{k|k-1})\omega^i_{k|k-1}\)
  8. 更新存在概率 \(q_{k} = \frac{\mathbf{I}_kq_{k|k-1}}{1-q_{k|k-1}+q_{k|k-1}I_k}\)
  9. for \(i = 1,...,N+N_B\) do
    1. \(\hat {\omega}^i_k = l_k(\mathbf{z}_k|\mathbf{m}^i_{k|k-1})\omega^i_{k|k-1}\)
  10. end for
  11. 归一化\(\hat {\omega}^i_k-> {\omega}^i_k\)
  12. \(i = 1,.., N\) Resample, \(\mathbf{m}^i_{k|k-1}->\mathbf{m}^i_{k}\)
  13. 粒子正规化(MCMC move)
  14. \(i = 1,.., N\) \(\omega^i_k = 1/N\)
  15. 产生birth particles, \(\mathbf{m}^i_k \sim b_k(\mathbf{m};\mathbf{z}_k), i =N+1,...,N+B\)
  16. \(\mathbf{\omega}^i_k \sim 1/N_B, i =N+1,...,N+B\)
  17. 输出:\(q_{k}\), \(\{\omega^i_{k},\mathbf{m}^i_{k}\}^{N+N_B}_{i=1}\).

检测模型下的伯努利粒子滤波:

输入: 存在概率 \(\varphi\), \(\{\omega^i_{k-1},\mathbf{m}^i_{k-1}\}^{N+N_B}_{i=1}\), \(\mathbf{Z}_k\)

  1. 预测存在概率 \(q_{k|k-1} = p_b(1-q_{k-1})+p_sq_{k-1}\)
  2. 取样\(\mathbf{m}^i_{k|k-1} \sim \varrho _k(\mathbf{m}_k|\mathbf{m}^i_{k-1},\mathbf{z}_k)\) for \(i = 1,...,N+ N_B\)
  3. 预测概率 \(\omega^i_{k|k-1}\)\(\omega^i_{k|k-1} = \left \{ \begin{matrix} \frac{p_sq_{k-1}}{q_{k|k-1}}\frac{\pi_{k|k-1}(\mathbf{m}^i_{k|k-1}|\mathbf{m}^i_{k-1})}{\varrho_k(\mathbf{m}^i_{k|k-1}|\mathbf{m}^i_{k-1},\mathbf{Z}_k)}\omega^i_{k-1} & i = 1,...,N \\ \frac{p_b(1-q_{k-1})}{q_{k|k-1}}\frac{\pi_{k|k-1}(\mathbf{m}^i_{k|k-1}|\mathbf{m}^i_{k-1})}{\varrho_k(\mathbf{m}^i_{k|k-1}|\mathbf{m}^i_{k-1},\mathbf{Z}_k)}\frac{1}{B} & i = N+1,...,N+B \end{matrix} \right .\)
  4. 近似积分\(I_1 \approx \sum^{N+N_b}_{i=1}p_d(\mathbf{m}^i_{k})\omega^i_{k|k-1}\)
  5. 对于\(\mathbf{z}\in\mathbf{Z}_k\)计算近似积分\(I_{2}(\mathbf{z})\approx\sum^{N+N_B}_{i=1}p_d(\mathbf{m}^i_{k|k-1})h_k(\mathbf{z}|\mathbf{m}^i_{k|k-1})\omega^i_{k|k-1}\)
  6. \(\triangle_k \approx I_1-\sum_{\mathbf{z}\in\mathbf{Z}_k}\frac{I_2(\mathbf{z})}{\lambda c(\mathbf{z})}\)
  7. 更新存在概率\(q_k = \frac{1- \triangle_k}{1-\triangle_k q_{k|k-1}q_{k|k-1}}\)
  8. 更新权重\(\hat{\omega}^i_k = [1-p_d(\mathbf{m}^i_{k|k-1})+p_d(\mathbf{m}^i_{k|k-1})\sum_{\mathbf{z}\in\mathbf{Z}_k}\frac{h_k(\mathbf{z}|\mathbf{m}^i_{k|k-1})}{\lambda c(\mathbf{z})}]\omega^i_{k|k-1}\)
  9. 归一化\(\hat {\omega}^i_k-> {\omega}^i_k\)
  10. \(i = 1,.., N\) Resample, \(\mathbf{m}^i_{k|k-1}->\mathbf{m}^i_{k}\)
  11. 粒子正规化(MCMC move)
  12. \(i = 1,.., N\) \(\omega^i_k = 1/N\)
  13. 产生birth particles, \(\mathbf{m}^i_k \sim b_k(\mathbf{m};\mathbf{z}_k), i =N+1,...,N+B\)
  14. \(\mathbf{\omega}^i_k \sim 1/N_B, i =N+1,...,N+B\)
  15. 输出:\(q_{k}\), \(\{\omega^i_{k},\mathbf{m}^i_{k}\}^{N+N_B}_{i=1}\).