[发明专利]基于分层空间的RB化粒子滤波算法

摘要

本发明公开了基于分层空间的RB化粒子滤波算法，它涉及粒子滤波算法技术领域；本发明的算法步骤包含粒子初始化、重要性采样、粒子权值计算、状态参数更新、结果输出；本发明有益效果为：它运用Rao-Blackwell定理，通过引入边缘函数从而提高估计精度并实现无偏估计，然后利用分层空间的方法,与粒子滤波结合可以在满足同等精度要求时，提高粒子的使用率，大大减少粒子所需的粒子数目。

主权项

基于分层空间的RB化粒子滤波算法，其特征在于包括如下步骤：步骤一、粒子初始化根据目标运动模型先验概率p(m(O))选取N个粒子，记为m⁽ⁱ⁾(0)对应的权值根据目标的状态先验概率获得则初始状态集合可以定义为${\left\{m^{\left(i\right)}\left(0\right),\left({\hat{X}}^{\left(i\right)},P^{\left(i\right)}\left(0\right)\right),{\tilde{w}}^{\left(i\right)}\right\}}_{i=1}^N;$步骤二、重要性采样选取最优分布p(m(k)|m(k‑1)，Z_1：k)作为重要性函数：$\begin{array}{c}p\left(m\left(k\right)|m\left(k-1\right)=m^{\left(i\right)}\left(k-1\right),Z_{1:k}\right)\\ =\frac{p\left(Z\left(k\right)|m\left(k\right),Z_{1:k-1}\right)p\left(m\left(k\right)|m\left(k-1\right)=m^{\left(i\right)}\left(k-1\right)\right)}{p\left(Z\left(k\right)|Z_{1:k-1},m\left(k-1\right)=m^{\left(i\right)}\left(k-1\right)\right)}\end{array}$根据概率，可以得到根据重要性采样得到的新的粒子${\left\{m^{\left(i\right)}\left(k\right)\right\}}_{i=1}^N;$步骤三、粒子权值计算根据式更新粒子的权值得到w(k)∝w(k‑1)p(Z(k)|m(k‑1)，Z_1：k‑1)；步骤四、状态参数更新根据每个粒子运动模型，采用交互式多模型得到$\{{\hat{X}}^{\left(i\right)}(k-1),P^{\left(i\right)}(k-1)\}\→\{{\hat{X}}^{\left(j\right)}\left(k\right),P^{\left(i\right)}\left(k\right)\};$步骤五、结果输出根据式$\hat{x}\left(k\right)=\underset{i=1}{\overset{N}{\Σ}}{\tilde{w}}^{\left(i\right)}\left(k\right){\hat{X}}^{\left(i\right)}\left(k\right)$和式$P\left(k\right)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\tilde{w}}^{\left(i\right)}\left(P^{\left(i\right)}\right(k)+({\hat{X}}^{\left(i\right)}\left(k\right)-\hat{X}\left(k\right)\left){\left({\hat{X}}^{\left(i\right)}\left(k\right)-\hat{X}\left(k\right)\right)}^T\right),$输出状态的均值和方差。