基于超球面采样无迹卡尔曼滤波的粒子滤波方法

摘要

本发明提供的是一种基于超球面采样无迹卡尔曼滤波的粒子滤波方法。初始化粒子及其权值；通过重要性采样产生粒子；更新粒子权值并对其进行归一化；再采样步骤；输出结果；进入下一时间步。本发明主要是对重要性采样步骤进行改进，采用基于超球面单形采样SSUT变换的SSUKF算法取得重要性概率密度。相比于粒子滤波PF、扩展卡尔曼粒子滤波EKPF以及标准的无迹粒子滤波，超球面采样无迹粒子滤波SSUPF可以取得与UPF相当的精度。另一方面，由于SSUT变换采用超球面分布的采样点(即sigma点)，采样点数量大大少于无迹变换UT变换，在计算效率方面的优势在高维系统中越发明显。

主权项

1.一种基于超球面采样无迹粒子滤波的粒子滤波方法，包括如下步骤：第一步，初始化粒子及其权值；第二步，通过重要性采样产生粒子；第三步，更新粒子权值并对其进行归一化；第四步，再采样步骤；第五步，输出结果；第六步，进入下一时间步；其特征是所述通过重要性采样产生粒子的方法为：(1)通过SSUT变换取得sigma点：选择零点对应的权值w₀，满足：0≤w₀≤1确定其它采样点对应的权值w_i：w_i＝(1-w₀)/(n+1)  i＝1，...，n+1初始化向量序列：$e_0^1=\left[0\right],$$e_1^1=[-1/\sqrt{{2w}_1}],$$e_2^1=[1/\sqrt{2w_1}]$扩展向量序列，j＝2，...，n：$e_i^j=\left\{\begin{array}{cc}\left[\begin{array}{c}{e}_0^{j-1}\\ 0\end{array}\right]& i=0\\ \left[\begin{array}{c}{e}_i^{j-1}\\ -1/\sqrt{j(j+1)w_j}\end{array}\right]& i=1,...,j\\ \left[\begin{array}{c}{0}^{j-1}\\ j/\sqrt{j(j+1)w_j}\end{array}\right]& i=j+1\end{array}\right.$式中，n为状态向量维数，e_i^j表示j维随机变量的第i个采样点；0^j表示j维零向量；通过步骤得到y的n+2个采样点e_iⁿ，i＝1，...，n+1；对于均值为，均方差为P_xx的n维随机变量x的超球面分布采样点由下式得到：${\chi }_i^n=\hat{x}+\sqrt{P_{\mathrm{xx}}}{e}_i^n$i＝0，...，n+1；(2)时间更新：χ_i，k|k+1＝F(χ_i,k-1，u_k-1，k-1)    i＝0，...，n+1${\hat{x}}_{k|k-1}=\underset{i=0}{\overset{n+1}{\Sigma }}{W}_i{\chi }_{i,k|k-1}$$P_{k|k-1}=\underset{i=0}{\overset{n+1}{\Sigma }}{W}_i({\chi }_{i,k|k-1}-{\hat{x}}_{k|k-1}){({\chi }_{i,k|k-1}-{\hat{x}}_{k|k-1})}^T+P_{\omega }$y_i，k|k-1＝H(χ_i，k|k-1，k)    i＝0，...，n+1${\hat{y}}_{k|k-1}=\underset{i=0}{\overset{n+1}{\Sigma }}{W}_i{y}_{i,k|k-1};$(3)量测更新$P_{{\tilde{y}}_k}=\underset{i=0}{\overset{n+1}{\Sigma }}{W}_i(y_{i,k|k-1}-{\hat{y}}_{k|k-1}){(y_{i,k|k-1}-{\hat{y}}_{k|k-1})}^T+P_v$$P_{x_k{y}_k}=\underset{i=0}{\overset{n+1}{\Sigma }}{W}_i({\chi }_{i,k|k-1}-{\hat{x}}_{k|k-1}){(y_{i,k|k-1}-{\hat{y}}_{k|k-1})}^T{K}_k=P_{x_k{y}_k}{P}_{{\tilde{y}}_k}^{-1}$${\hat{x}}_k={\hat{x}}_{k|k-1}+K_k(y_k-{\hat{y}}_{k|k-1})$$P_k=P_{k|k-1}-K_k{P}_{{\tilde{y}}_k}{K}_k^T;$(4)对每一个采样点x_k-1ⁱ，应用ssuKF得到粒子集的均值和方差P_kⁱ；(5)从SSUKF结果中产生N个粒子。