[发明专利]一种基于集合卡尔曼滤波的粒子滤波方法

摘要

本发明提出一种基于集合卡尔曼滤波的粒子滤波方法，包括初始化采样粒子、对k时刻的背景集合进行预测、计算卡尔曼增益、融合最近观测信息，对背景集合进行更新、重新计算分析集合、构建高斯建议分布函数和权值进行归一化处理等步骤。本发明提出的基于集合卡尔曼滤波的粒子滤波方法无须对非线性系统进行线性化，避免了Jacobian矩阵的计算，使用采样法近似非线性分布，提高了计算精度。且本发明的粒子滤波方法中采样点数是启发式的，可以灵活设定，计算量不会随维数增大发生激增，实时性得到有效控制。

主权项

1.一种基于集合卡尔曼滤波的粒子滤波方法，其特征在于：包括以下几个步骤：步骤一：在k＝0时刻初始化粒子，利用先验概率p(x₀)进行随机采样，获得初始采样粒子初始化每个采样粒子的权值为1/N，i代表粒子序号，i＝1∶N，N为采样粒子个数，k为非线性系统的运行时刻，为k＝0时刻第i个采样粒子的状态值；对于k＝0时刻每一个采样粒子定义初始的分析集合为其中表示k＝0时刻第i个粒子对应的分析集合中第j个样本，j代表分析集合中样本序号，表示以k＝0时刻第i个粒子状态值为均值，以系统过程噪声协方差Q为方差的高斯分布；步骤二：(1)在k＝1时刻，对每一个采样粒子分别定义一个背景集合为和一个分析集合为其中上标a和b分别代表分析集合和背景集合，j代表分析集合或背景集合中样本序号，n表示分析集合或背景集合中样本的个数，表示k时刻背景集合中第j个样本，表示k时刻分析集合中第j个样本；(2)利用非线性系统的状态方程由k-1时刻的分析集合对当前背景集合中每个样本进行预测，得到公式(3)xk,j(i),b=f(xk-1,j(i),a)---(3)]]>为非线性系统状态方差，表示k-1时刻分析集合中第j个样本。计算k时刻背景集合的中所有样本的均值非线性系统关于状态变量的协方差以及观测方程的协方差得到公式(4)、(5)和(6)：x^k(i),b=1nΣj-1nxk,j(i),b---(4)]]>P^xh(i),k=1n-1Σj=1n(xk,j(i),b-x^k(i),b)(h(xk,j(i),b)-h(x^k(i),b))T---(5)]]>P^hh(i)k=1n-1Σj=1n(h(xk,j(i),b)-h(x^k(i),b))(h(xk,j(i),b)-h(x^k(i),b))T---(6)]]>h(·)表示非线性系统的观测方程函数；(3)根据公式(7)计算卡尔曼增益Kk(i)=P^xh(i),k(P^hh(i),k+Rk)-1---(7)]]>其中为卡尔曼增益，R_k为观测噪声的协方差矩阵；(4)融合最近观测信息，对k时刻背景集合进行更新，计算得到k时刻的分析集合：xk,j(i),a=xk,j(i),b+Kk(i)(yk,j-h(xk,j(i),b))---(8)]]>其中y_k，j是以观测值y_k为均值、R_k为方差的高斯分布的采样；根据公式(9)和(10)，计算分析集合的均值和协方差x^k(i)a=1nΣj-1nxk,j(i)a---(9)]]>P^k(i)a=1n-1Σj=1n(xk,j(i)a-x^k(i)a)(xk,j(i)a-x^k(i)a)T---(10)]]>这个过程利用上一时刻的状态变量的值和当前时刻的最近观测信息，通过集合卡尔曼滤波方法对建议分布更新后的分析集合均值和方差进行传递，得到该时刻的高斯建议分布函数其中表示k时刻第i个粒子，表示k-1时刻第i个粒子，z_k表示k时刻观测向量；步骤三：从步骤二得到的高斯建议分布函数中进行采样，根据公式(11)得到当前时刻的二次采样粒子，表示k时刻第i个样本的二次采样样本，按照公式(12)计算每个二次采样粒子的权值对权值进行归一化处理，得到归一化后的权值x^k(i)~q(xk(i)|xk-1(i),zk)=N(x^k(i),a,P^k(i),a)]]>ωk(i)=ωk-1(i)p(zk|x^k(i))p(x^k(i)|xk-1(i))q(x^k(i)|x0:k-1(i),z1:k)---(12)]]>步骤四：依照二次采样粒子的权值大小对步骤三中得到的二次粒子进行重采样，得到三次采样粒子；步骤五：待估计非线性离散系统的运行时间为M，若k＝M则输出滤波结果，若k＜M，则迭代过程未完成，进入k+1时刻，重复步骤二至步骤四，直至非线性系统运行结束，使用采样加权平均值E(x_0:k)作为每一时刻算法输出值：E(x0:k)=∫x0:k·p(x0:k|z1:k)dx0:k≈1NΣi=1Nx0:k(i)---(13)]]>输出值E(x_0:k)是对非线性系统进行滤波后的状态变量的估计值，其中0:k表示从0时刻到k时刻；x_0:k表示非线性系统从0到k时刻的状态值；p(x_0:k|z_1:k)表示非线性系统从0到k时刻的后验概率密度；表示第i个样本从0到k时刻的状态变量的值。