一种基于高斯扰动的重采样粒子滤波算法的制作方法

技术特征：

1.一种基于高斯扰动的重采样粒子滤波算法，其特征在于，所述方法的步骤为

步骤1、初始化，在初始时刻k＝0时，从系统状态的先验概率密度分布函数p(S₀)～U(Λ)中采样得到粒子集合其中，U表示均匀分布函数，Λ表示系统状态的均匀分布区间，N为采样粒子数目，表示在k＝0时刻采样得到的第i个粒子，为第i个粒子的权值，此时所有粒子重要性权值相同，均为初始化后进入粒子滤波迭代过程，令k＝1；

步骤2、从重要性概率密度分布函数中采样：对粒子依据系统状态模型进行预测；式中表示在k时刻采样得到的第i个粒子、p(S_k|S_k-1)为重要性概率密度分布函数；

步骤3、根据由观测模型确定的似然函数计算每个粒子权值并对权值归一化，归一化后粒子的权值为式中，Z_k为观测值，为k时刻第i个粒子的权值；此时系统状态后验概率密度分布函数近似为式中S_k为待估计的k时刻的系统状态，δ为单位脉冲函数；

步骤4、计算有效粒子的数目

步骤5、当有效粒子数目N_eff大于等于设定阈值时，输出系统状态估计值否则当有效粒子数目小于设定阈值时，进入步骤6；

步骤6、启动重采样过程进行重采样，对粒子集进行重采样得到新的粒子集

步骤7、比较k是否到达结束时刻T，若k＜T,则k＝k+1跳至步骤2；若k≥T则结束。

2.根据权利要求1所述的一种基于高斯扰动的重采样粒子滤波算法，其特征在于，所述步骤6中的重采样的过程为

601、产生[0,1]上均匀分布的随机数{u_i}_i＝1…N；

602、产生权值累积函数wc,满足

603、找到满足wc(m-1)≤u_i≤wc(m)的整数m生成重采样粒子下标m，第m个粒子经重采样后将被复制在第i个位置,即为k时刻重采样后第i个粒子，为k时刻重采样前第m个粒子；

604、统计第m个粒子被采样的次数

605、若C_m＞1,则计算采样邻域的大小α²,α²与粒子集中的有效粒子目数和系统状态的方差ε²有关，进入606；

606、对由多次重采样后的粒子添加高斯扰动为重采样后，粒子集中粒子的重要性权值相等，均为重采样得到新的粒子集

607、计算输出k时刻系统状态估计期望值