一种导向粒子滤波方法与流程

技术特征：

1.一种导向粒子滤波方法，其特征在于，步骤如下：

S1.初始化粒子滤波参数，粒子滤波参数包括粒子个数N，时间t，第i个粒子在初始时刻的权值粒子有效值N_off，粒子有效值N_off的权值ω_off，其中i＝1，2，……，N；

S2.计算并归一化粒子的加权值，具体为过程：

S21.结合导向滤波计算其他时刻粒子的权值，计算方式如下式所示，

其中，表示第i个粒子在t时刻的权值，表示第i个粒子在t-1时刻的权值，a_t和b_t为自定义系数，且有

其中，表示第i个粒子在t-1时刻的加权值，x_0：t-1为0到t-1时刻的状态值，是的方差，ε是修正因子；

S22.根据步骤S21得到的粒子的权值计算粒子的加权值具体方法为：

S23.对步骤S22计算得到的粒子的加权值进行归一化处理，具体方法为：

S3.粒子重采样，具体过程为：

S31.比较步骤S2中计算得到的粒子的加权值与步骤S1中粒子有效值N_off的权值ω_off，根据比较结果对粒子进行分组，并计数获得有效值个数N_e；其中，

若将第i个粒子分入组A，组A粒子表示为其中表示第i个粒子t时刻的状态值；

若将第i个粒子分入组B，组B粒子表示为

S32.将S31得到的粒子滤波的有效值个数N_e与步骤S1中粒子有效值N_off进行比较，判断是否需要重采样；

若N_e＜N_off，则对步骤S31中组B中的粒子按照步骤S33所示方法进行重采样处理；若N_e≥N_off，则直接转至步骤S4；

S33.设定组B中的粒子数目为L，滤波器的阶数也为L，滤波器的系数为1/L，对组B中的粒子做线性滤波，使权值大的粒子减小，得到粒子集合 然后返回步骤S31；

S4.根据公式所示方法，输出t时刻所有粒子的估计状态返回步骤S2，进行下一时刻粒子加权值的计算。