[发明专利]改进的强跟踪平方根容积卡尔曼滤波方法

权利要求书

1.一种改进的强跟踪平方根容积卡尔曼滤波方法，其特征在于包括下述步骤：

(1)设定初始参数设定，包括初始时刻系统状态值x₀、初始时刻系统状态协方差平方根S₀、系统噪声协方差Q、观测噪声协方差R和遗忘因子ρ；

(2)时间更新，包括以下内容：

首先定义S＝Tria(A^M×N)表示一种矩阵三角分解运算，A^T＝Q_AR_A，其中Q_A为正交阵，R_A为上三角矩阵，取R_A的前M×M阶矩阵的转置，即S＝(R^M×M)^T；

假设已知系统k时刻的估计状态和协方差阵平方根S_k，时间更新如下：

Xi,k=Skξi+x^k]]>

Xi,k+1/k*=f(Xi,k)]]>

x^k+1/k=1mΣi=1mXi,k+1/k*]]>

Sk+1/k=Tria([χk+1/k*SQ,k])]]>

χk+1/k*=1m[X1,k+1/k*-x^k+1/kX2,k+1/k*-x^k+1/k...Xn,k+1/k*-x^k+1/k]]]>

其中i＝1,2,…,m,m＝2n，n为状态向量维数；X_i,k为容积点集；记n维单位列向量e＝[1,0,…,0]^T，使用符号[1]表示对e的元素进行全排列和改变元素符号产生的点集，称为完整全对称点集，[1]_i表示点集[1]中的第i个点；为通过状态函数传递后的容积点集；f(·)为非线性状态函数；为k+1时刻状态预测值；S_k+1/k为k+1时刻预测误差协方差阵平方根；为k+1时刻的加权中心矩阵；S_Q,k为k时刻的系统噪声平方根，有

(3)量测更新，包括以下内容：

Xi,k+1/k=Sk+1/kξi+x^k+1/k]]>

y_i,k+1/k＝h(X_i,k+1/k)

y^k+1/k=1mΣi=1myi,k+1/k]]>

Yk+1/k=1m[y1,k+1/k-y^k+1/ky2,k+1/k-y^k+1/k...ym,k+1/k-y^k+1/k]]]>

Pxy,k+1/k=χk+1/kYk+1/kT]]>

χk+1/k=1m[X1,k+1/k-x^k+1/kX2,k+1/k-x^k+1/k...Xn,k+1/k-x^k+1/k]]]>

计算减消因子λ_k+1：

Pk+1/k=Sk+1/kSk+1/kTHk+1=[Pxy,k+1/k]T[(Pk+1/k)-1]TNk+1=Vk+1-Hk+1QkHk+1T-Rk+1Mk+1=Hk+1(Pk+1/k-Qk)Hk+1TCk+1=tr(Nk+1)tr(Mk+1)λk+1=max{1,Ck+1}]]>

其中V_k+1为实际残差序列的协方差矩阵，估算公式如下：

Vk+1=γ1γ1T,k=1ρVk+γk+1γk+1T1+ρ,k>1]]>

若λ_k+1＞1，表示残差信息没有被完全提取，要对增益矩阵K_k+1进行修正，相关计算如下：

χk+1/k=λk+1χk+1/ks]]>

Pxy,k+1/k=(χk+1/kχk+1/kT+(1-λk+1)Qk)Hk+1T]]>

P_yy,k+1/k＝H_k+1P_xy,k+1/k+R_k

K_k+1＝P_xy,k+1/k／P_yy,k+1/k

若λ_k+1≤1，表示在此时刻非线性系统是准确的，不用对增益矩阵K_k+1进行修正，则P_xy,k+1/k和Y_k+1/k已求得，增益矩阵K_k+1计算如下：

S_yy,k+1/k＝Tria([Y_k+1/kS_R,k])

Kk+1=(Pxy,k+1/k/Syy,k+1/kT)/Syy,k+1/k]]>

最后计算k+1时刻状态估计值和k+1时刻状态误差协方差阵平方根完成量测更新：

x^k+1=x^k+1/k+Kk+1(yk+1-y^k+1/k)]]>

Sk+1=Tria([χk+1/k-Kk+1Yk+1/kKk+1SR,k+1(1-λk+1)Qk])]]>

其中X_i,k+1/k为容积点集；y_i,k+1/k为通过量测函数传递后的容积点集；h(·)为非线性量测函数；为k+1时刻观测预测值；Y_k+1/k为k+1时刻y_i,k+1/k加权中心矩阵；P_xy,k+1/k为k+1时刻互相关协方差阵；χ_k+1/k为k+1时刻X_i,k+1/k的加权中心矩阵；λ_k+1为k+1时刻渐消因子；P_k+1/k为k+1时刻预测状态误差协方差阵；H_k+1为k+1时刻量测函数h(·)对x的偏导的雅可比矩阵；N_k+1,M_k+1,C_k+1为求解减消因子中使用的中间过程矩阵；tr(·)为矩阵求迹运算；max{·}为求最大值运算；残差y_k+1为k+1时刻量测值；ρ为遗忘因子，0＜ρ≤1，通常取ρ＝0.95；中上标s表示未引入减消因子时的变量；P_yy,k+1/k为k+1时刻量测误差协方差阵；K_k+1为k+1时刻增益矩阵；S_yy,k+1/k为k+1时刻量测误差协方差阵平方根；为k+1时刻状态估计值；S_k+1为k+1时刻状态误差协方差阵平方根；S_R,k+1为R_k+1的平方根，有