[发明专利]一种改进的粒子滤波方法

摘要

本发明提供的是一种改进的粒子滤波方法。包括（1）运载体启动后，选取运载体状态函数及量测函数；（2）选取渐消因子和弱化因子；（3）采用STSRCKF设计重要性密度函数；（4）重新产生粒子；（5）计算粒子重要性权值并归一化；（6）重采样；（7）状态估计；（8）时刻迭代更新，判断当前时刻k是否为迭代终止时刻，若当前时刻k不是迭代终止时刻，则由当前时刻k更新到下一时刻k+1，重复执行步骤（2）；若当前时刻k是迭代终止时刻，则结束，控制运载体停止运动。

主权项

一种用于运载体导航过程的改进的粒子滤波方法，其特征是：(1)运载体启动后，选取运载体状态函数及量测函数；所述选取运载体状态函数及量测函数包括：xk=f(xk-1)+wk-1zk=h(xk)+vk]]>其中，xk为状态向量，zk为量测向量，f(xk‑1)和h(xk)分别为运载体状态函数和量测函数，wk‑1为随机系统噪声，且wk‑1～N(0,Q)，vk为随机量测噪声，且vk～N(0,R)；在初始时刻，即k＝0时刻，设定运载体的初始状态x0＝[0 0 1 0 ‑0.1 1.5 0.03 0]T，初始状态协方差P0＝[0]8×8，系统噪声Q，量测噪声R，并设定粒子数N，重采样阈值Nthreshold，用于求取渐消因子中间参数C0,k的初始值C0,0和弱化因子ρ；从先验分布P(x0)中选取粒子其中i＝1,2,…,N：式中，P(x0)为先验分布，为所选取的粒子，为所选取的第i个粒子的均值，为所选取的第i个粒子的协方差；(2)选取渐消因子和弱化因子；所述选取渐消因子和弱化因子包括：选取渐消因子λk和弱化因子β如下：其中，λk为渐消因子，当模型较准确时，λk＝1，STSRCKF退化为普通SRCKF；tr(·)为求矩阵的迹；β≥1为弱化因子；Szz,k|k‑1为新息协方差矩阵的平方根因子；γk是残差；0＜ρ≤1为遗忘因子；(3)采用STSRCKF设计重要性密度函数；所述采用STSRCKF设计重要性密度函数包括：采用STSRCKF方法的时间更新和量测更新更新每个粒子：①时间更新若k‑1时刻的后验概率为分解因式得：Sk‑1|k‑1＝Chol{Pk‑1|k‑1}计算容积点，i＝1,2,…,m；m＝2n：通过状态方程传播容积点：估计k时刻的状态预测值：计算k时刻状态协方差预测值的平方根因子：②量测更新，融入最新量测数据：计算容积点，i＝1,2,…,m；m＝2n：通过量测方程传播容积点：zi,k|k‑1＝h(Xi,k|k‑1)估计k时刻的量测预测值：计算新息协方差矩阵的平方根因子：Szz,k|k‑1＝Tria([ζk|k‑1 SR,k])计算互相关协方差矩阵：引入渐消因子λk实时估计滤波增益：计算k时刻的状态估计值：计算k时刻状态协方差估计值的平方根因子：Sk|k＝Tria([ηk|k‑1‑Kkζk|k‑1 KkSR,k])其中，Pk‑1|k‑1分别为k‑1时刻的状态估计值和协方差估计值；Sk‑1|k‑1为Pk‑1|k‑1的乔里斯基因子；[1]i表示集合[1]的第i列，[1]＝[eye(n) ‑eye(n)]，eye(n)＝diag([1 1 1 1 1 1 1 1])；m＝2n为容积点个数，n＝8为UUV状态向量的维数；Xi,k‑1|k‑1、Xi,k|k‑1分别为k‑1时刻和k时刻的容积点；f(·)、h(·)分别为状态方程和量测方程；zi,k|k‑1分别为通过状态方程和量测方程传播后的容积点；SQ,k‑1、SR,k分别为Qk‑1和Rk的平方根因子；分别为k时刻的状态预测值和量测预测值；Sk|k‑1、Sk|k分别为k时刻状态协方差预测值和估计值的平方根因子；Szz,k|k‑1为信息协方差矩阵的平方根因子；Pxz,k|k‑1为互相关协方差矩阵；Kk为滤波增益；zk为量测向量；(4)重新产生粒子；所述重新产生粒子包括：重新产生粒子：其中，表示采用重要性密度函数重新抽取粒子表示新产生的粒子所服从的分布；(5)计算粒子重要性权值并归一化；所述计算粒子重要性权值并归一化包括：对于i＝1,2,…,N，计算重要性权值并归一化计算重要性权值：归一化：其中，为k时刻第i个粒子未经过归一化的重要性权值，为k时刻第i个粒子归一化后的重要性权值，为采用STSRCKF设计的重要性密度函数，为似然函数，为预测密度函数；(6)重采样；所述重采样的方法为：计算有效粒子数Neff，并将有效粒子数与设定的重采样阈值进行比较，若Neff＜Nthreshold，则进行重采样过程，得到新的粒子集j＝1,2,…N，N个粒子的权值相等，均为其中，Neff为有效粒子数，Nthreshold为重采样阈值，为重采样后获得的粒子；(7)状态估计；所述状态估计包括：计算状态估计值：计算协方差估计值：其中，为k时刻的状态估计值，Pk|k为协方差估计值；(8)时刻迭代更新，判断当前时刻k是否为迭代终止时刻，若当前时刻k不是迭代终止时刻，则由当前时刻k更新到下一时刻k+1，重复执行步骤(2)；若当前时刻k是迭代终止时刻，则结束，控制运载体停止运动。