[发明专利]一种基于变换采样点的强跟踪UKF的滤波方法

摘要

本发明提供的是一种基于变换采样点的强跟踪UKF滤波方法。本发明包括（1）对系统进行初始参数设置；（2）根据正交变换采样点的方法对Sigma点进行采样，求出相应的预测方程，进行时间更新和量测更新；（3）计算渐消因子；（4）利用渐消因子计算新的一步预测协方差，重新计算Sigma点，通过非线性量测函数传播，得到引入渐消因子后的自协方差和互协方差；（4）进行滤波更新，直至结束。本发明既有效的解决系统非局部采样问题，在提高了系统精度，又使系统具有一定强跟踪能力。该方法可用于改善系统模型不确定时鲁棒性差，滤波发散的问题，并解决了高维系统中的非局部采样问题，拓展了强跟踪滤波的应用范围。在MEMS/GPS组合导航系统中，该方法可提高其定位定姿性能。

主权项

1.一种基于变换采样点的强跟踪UKF的滤波方法，其特征是：步骤1：根据变换采样策略，确定采样点的个数、位置及相应权值，为k时刻的状态的一步预测估计值、P_k为估计方差、n维系统具有2n个采样点，采样点χ_i,k表示如下χi,k=x^k+Pkγ]]>γ=(γ₁,γ₂,…γ_2n)，其中γ_k=(γ_k,1,γ_k,2,…γ_k,n)^T,k=1,2,…2nγk,2r-1=2cos((2r-1)kπ/n)]]>γk,2r=2sin((2r-1)kπ/n)]]>r=1,2,…,[n/2]，[n/2]表示不大于n/2的最大整数，如果n是奇数，相应权值具体表示如下：W0=κ/(n+κ)Wi=1/2(n+κ)Wi+n=1/2(n+κ),i=1,···,n]]>W_i代表第i个点的相应权值，i=0,1,…,2n，且权值满足κ是一个比例因子，选择κ=3-n；步骤2：时间更新，按照步骤1所选择的采样点χ_i,k，通过非线性状态函数f传播为ξ_i,k+1|k，进而得到加权后的一步状态预测值ξi,k+1|k=f[χi,k,uk]+qk,i=0,1,···,2nx^k+1|k=Σi=02nWi(m)ξi,k+1|k]]>Pk+1|k(l)=Σi=02nWi(c)[ξi,k+1|k-x^k][ξi,k+1|k-x^k]T+Qk]]>表示没有引入渐消因子时的状态预测误差协方差矩阵；步骤3：量测更新，利用步骤2中得出的一步状态预测值和误差协方差矩阵采用步骤1中的采样策略计算Sigma点，通过GPS的速度、位置测量函数h(·)传播为即：ζi,k+1|k(l)=h(χi,k+1|k(l))+rk+1]]>r_k+1为k+1观测噪声计算未引入渐消因子的输出预测z^k+1|k=Σi=02nWimζi,k+1|k(l)]]>计算未引入渐消因子的自协方差Pz~k+1(l)=Σi=02nWi(c)(χi,k+1|k(l)-z^k+1|k)(χi,k+1|k(l)-z^k+1|k)T+Rk+1]]>R_k+1是观测噪声矩阵，计算未引入渐消因子的互协方差Px~k+1z~k+1(l)=Σi=02nWi(c)(χi,k+1|k(l)-x^k+1|k)(ζi,k+1|k(l)-z^k+1|k)T;]]>步骤4：计算渐消因子λ_k+1，设理论输出残差序列为λk+1=λ0,λ0≥11,λ0<1,λ0=tr[Nk+1]/tr[Mk+1]]]>其中N_k+1、M_k+1是由V_k+1计算得到的，V_k+1为实际输出残差序列的协方差阵，是未知的，Vk+1=ϵ1ϵ1T,k=1;ρVk+ϵk+1ϵk+1T1+ρ,k≥1]]>式中，ρ为遗忘因子，0<ρ≤1，取ρ=0.95，Nk+1=Vk+1-Rk+1-[Px~k+1z~k+1(l)]T[Pk+1|k(l)]-1Qk[Pk+1|k(l)]-1Px~k+1z~k+1(l)]]>Mk+1=Pz~k+1(l)-Vk+1+Nk+1;]]>步骤5：求得渐消因子λ_k+1后，；求得新的状态预测误差协方差矩阵P_k+1|k：Pk+1|k=λk+1Σi=02nWic[ξi,k+1|k-x^k][ξi,k+1|k-x^k]T+Qk]]>Q_k为系统噪声矩阵，采用步骤1中的采样策略计算Sigma点，通过非线性测量函数h(·)传播为ζ_i,k+1|k，重新对系统进行量测更新，χi,k+1|k=x^k+1|k+Pk+1|kγ]]>ζi,k+1|k=h(χi,k+1|k)+rk+1]]>计算输出预测z^k+1|k=Σi=02nWimζi,k+1|k]]>计算自协方差Pz~k+1=Σi=02nWic(χi,k+1|k-z^k+1|k)(χi,k+1|k-z^k+1|k)T+Rk+1]]>计算互协方差Px~k+1z~k+1=Σi=02nWic(χi,k+1|k-x^k+1|k)(ζi,k+1-z^k+1|k)T;]]>步骤6：滤波更新，计算k+1时刻的比例因子K_k+1，k+1时刻的估计值和估计误差协方差P_k+1：Kk+1=Px~k+1z~k+1Pz~k+1-1]]>x^k+1=x^k+1|k+Kk+1(zk+1-z^k+1|k)]]>Pk+1=Pk+1|k-Kk+1Pz~k+1Kk+1T]]>返回步骤1，进行下一时刻的估计。