改进的强跟踪平方根容积卡尔曼滤波方法技术

本发明专利技术提供了一种改进的强跟踪平方根容积卡尔曼滤波方法，通过分析减消因子提高强跟踪算法鲁棒性的机理和SCKF算法流程特点，ISTCKF重新选择减消因子引入位置，减少由于减消因子引入带来额外计算量。

【技术实现步骤摘要】

本专利技术涉及一种非线性滤波方法。
技术介绍
容积卡尔曼滤波（cubatureKalmanfilter,CKF)是由加拿大学者Ienkaran Arasaratnam和SimonHaykin在2009年提出的一种新的非线性近似滤波算法。由于CKF 求解容积点时，要对协方差阵开方，随着滤波迭代次数的增加，舍入误差的积累，可能会导 致协方差阵失去非负定性甚至失去对称性。IenkaranArasaratnam和SimonHaykin在 CKF的基础上，加入平方根算法，提出平方根容积卡尔曼滤波算法（square-rootcubature Kalmanfilter，SCKF)。该算法有效地解决了CKF的数值稳定性问题，并减少计算量，提供 更佳的滤波性能。 在实际系统中，由于（1)数学模型不准确；(2)系统本身在缓慢动态变化，而建立 的数学模型难以根据这些变化动态改变模型，导致模型匹配性逐渐变差；（3)受系统外部 变化影响等原因，系统模型并不能完全准确，导致SCKF滤波效果不理想，严重时甚至导致 滤波发散。周华东等人基于正交原理建立强跟踪算法（strongtrackingfilter,STF)，大 大增强了非线性滤波算法的鲁棒性。结合强跟踪的思想，将减消因子引入SCKF中，可建立 强跟踪SCKF算法（strongtrackingSCKF，STSCKF)，克服SCKF在系统模型不确定时滤波 精度下降的缺点。STSCKF已经被应用在机动跟踪、组合导航等领域，有效提高系统的鲁棒性 和精度。 但STSCKF算法引入减消因子过程中，首先要获取k+Ι时刻一步预测互相关协方差 阵，再计算减消因子，最后进行量测更新。根据SCKF实现步骤，这种减消因子引入方法等价 于将量测更新中"计算容积点"到"计算互相关协方差阵"部分重复执行了两次，大幅增加 了算法的时间复杂度。
技术实现思路
为了克服现有技术的不足，本专利技术提供一种改进的强跟踪SCKF算法（improved strongtrackingSCKF)，通过分析减消因子提高强跟踪算法鲁棒性的机理和SCKF算法流 程特点，ISTCKF重新选择减消因子引入位置，减少由于减消因子引入带来额外计算量。 本专利技术解决其技术问题所采用的技术方案包括以下步骤：(1)设定初始参数设定，包括初始时刻系统状态值X。、初始时刻系统状态协方差平 方根S。、系统噪声协方差Q、观测噪声协方差R和遗忘因子P; ⑵时间更新，包括以下内容：首先定义S=Tria(AMXN)表示一种矩阵三角分解运算，AT=QARA，其中QA为正交 阵，Ra为上三角矩阵，取Ra的前MXM阶矩阵的转置，即S=(Rmxm)t; 假设已知系统k时刻的估计状态％和协方差阵平方根Sk，时间更新如下： 其中i= 1，2, 一,111,111= 2n，n为状态向量维数；Xlik为容积点集； 维单位列向量e= τ，使用符号表示对e的元素进行全排列和改变元素符号 产生的点集，称为完整全对称点集，1表示点集中的第i个点；为通过状态函 数传递后的容积点集；f( ·)为非线性状态函数；4+1/?为k+Ι时刻状态预测值；Sk+1/k为k+1 时刻预测误差协方差阵平方根；;^+1?为k+1时刻XL+m的加权中心矩阵；为k时刻的系 统噪声平方根，有 ⑶量测更新，包括以下内容： 其中Vk+1为实际残差序列的协方差矩阵，估算公式如下： 若Ak+1> 1，表示残差信息没有被完全提取，要对增益矩阵Kk+1进行修正，相关计 算如下： Pyy,k+l/k=Hk+ 1Pxyjk+1/k+Rk Kk+1=Pxyik+l/k/Pyy,k+l/k 若Ak+1彡1，表示在此时刻非线性系统是准确的，不用对增益矩阵Kk+1进行修正， 贝ljPjiy,k+1/k和Yk+1/k已求得，增益矩阵Kk+1计算如下： Syy_k+1/k=Tria([Yk+1/kSRjJ) 最后计算k+1时刻状态估计值和k+1时刻状态误差协方差阵平方根完成量测更 新： 其中Xlik+1/k为容积点集；ylik+1/kS通过量测函数传递后的容积点集；h( ·)为非线 性量测函数；Ιω?为k+Ι时刻观测预测值；Yk+1/kSk+Ι时刻ylik+1/k加权中心矩阵；Pxyik+1/ k+1时刻互相关协方差阵；xk+1/kSk+1时刻Xlik+1/k的加权中心矩阵；λk+1为k+1时刻 渐消因子；Pk+1/k为k+1时刻预测状态误差协方差阵；Hk+1为k+1时刻量测函数h( ·)对X的 偏导的雅可比矩阵；Nk+1，Mk+1，Ck+1为求解减消因子中使用的中间过程矩阵；tr( ·)为矩阵求 迹运算；max{ ·}为求最大值运算；残差；JVtl，yk+1为k+1时刻量测值；P为遗忘 因子，0 <P彡1，通常取P= 0. 95 ;%+ιλ中上标s表示未引入减消因子时的变量；Pyy,k+1/ k+1时刻量测误差协方差阵；Kk+1为k+1时刻增益矩阵；S">1/14为k+1时刻量测误差协 方差阵平方根；％为k+Ι时刻状态估计值；Sk+1为k+Ι时刻状态误差协方差阵平方根；SRik+1 为Rk+1的平方根，有L=Wiw。 本专利技术的有益效果是： (1)减消因子引入位置调整到xk+1/k#，仅有X k+l/k、Pxy，k+1/k、Pyy，k+1/k和Kk+1等变量 的计算受到减消因子的影响，不必对容积点以及传递后的容积点集等变量重复求解，减少 了重复执行步骤，降低了时间复杂度，提高了算法效率。由于计算传递后的容积点集步骤的 时间复杂度与状态向量维数和量测函数h( ·)复杂程度密切相关，对于系统状态维数越高， h( ·)越复杂的系统，时间复杂度的优化效果越明显； (2)算法精度与改进前相当，并没有因为降低算法时间复杂度而影响算法精度。【附图说明】 图1是SCKF状态估计与估计误差不意图； 图2是STSCKF状态估计与估计误差示意图； 图3是ISTSCKF状态估计与估计误差示意图。【具体实施方式】 下面结合附图和某轮船推进系统的实施例对本专利技术进一步说明，本专利技术包括但不 仅限于下述实施例。 本专利技术的实现步骤如下： 考虑如下离散时间非线性动态系统： xk+1=fk(xk)+wk yk+i=hk+1 (xk+1)+vk+1 其中xkeRn为系统状态向量，yk+le为量测向量；_/；(□和/W(□分别为非线性 系统的状态函数和量测函数;wkeRn为系统噪声，Vk+1eRm为量测噪声，二者均为高斯白噪 声，互不相关，且协方差分别为Q和R。 基于以上非线性系统的ISTSCKF算法具体流程如下： 1)设定初始参数 设定初始时刻系统状态值X。，初始时刻系统状态协方差平方根S。，系统噪声协方 差Q，观测噪声协方差R，遗忘因子p。 2)时间更新 假设已知系统k时刻的估计状态％和协方差阵平方根Sk。①选取容积点Xlik(i= 1，2, · · ·，m) 其中m=2η，η为状态向量维数。 ②计算传递后容积点μ ); ④计算k+1时刻预测误差协方差阵平方根Sk+1/k为k时刻的 系统噪声平方根，有α= 。 3)量测更新①计算容积点 = G=U当前第1页1 2 本文档来自技高网...

【技术保护点】
一种改进的强跟踪平方根容积卡尔曼滤波方法，其特征在于包括下述步骤：(1)设定初始参数设定，包括初始时刻系统状态值x0、初始时刻系统状态协方差平方根S0、系统噪声协方差Q、观测噪声协方差R和遗忘因子ρ；(2)时间更新，包括以下内容：首先定义S＝Tria(AM×N)表示一种矩阵三角分解运算，AT＝QARA，其中QA为正交阵，RA为上三角矩阵，取RA的前M×M阶矩阵的转置，即S＝(RM×M)T；假设已知系统k时刻的估计状态和协方差阵平方根Sk，时间更新如下：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为状态向量维数；Xi,k为容积点集；记n维单位列向量e＝[1,0,…,0]T，使用符号[1]表示对e的元素进行全排列和改变元素符号产生的点集，称为完整全对称点集，[1]i表示点集[1]中的第i个点；为通过状态函数传递后的容积点集；f(·)为非线性状态函数；为k+1时刻状态预测值；Sk+1/k为k+1时刻预测误差协方差阵平方根；为k+1时刻的加权中心矩阵；SQ,k为k时刻的系统噪声平方根，有(3)量测更新，包括以下内容：Xi,k+1/k=Sk+1/kξi+x^k+1/k]]>yi,k+1/k＝h(Xi,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}]]>其中Vk+1为实际残差序列的协方差矩阵，估算公式如下：Vk+1=γ1γ1T,k=1ρVk+γk+1γk+1T1+ρ,k>1]]>若λk+1＞1，表示残差信息没有被完全提取，要对增益矩阵Kk+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]]>Pyy,k+1/k＝Hk+1Pxy,k+1/k+RkKk+1＝Pxy,k+1/k／Pyy,k+1/k若λk+1≤1，表示在此时刻非线性系统是准确的，不用对增益矩阵Kk+1进行修正，则Pxy,k+1/k和Yk+1/k已求得，增益矩阵Kk+1计算如下：Syy,k+1/k＝Tria([Yk+1/k SR,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])]]>其中Xi,k+1/k为容积点集；yi,k+1/k为通过量测函数传递后的容积点集；h(·)为非线性量测函数；为k+1时刻观测预测值；Yk+1/k为k+1时刻yi,k+1/k加权中心矩阵；Pxy,k+1/k为k+1时刻互相关协方差阵；χk+1/k为k+1时刻Xi,k+1/k的加权中心矩阵；λk+1为k+1时刻渐消因子；Pk+1/k为k+1时刻预测状态误差协方差阵；Hk+1为k+1时刻量测函数h(·)对x的偏导的雅可比矩阵；Nk+1,Mk+1,Ck+1为求解减消因子中使用的中间过程矩阵；tr(·)为矩阵求迹运算；max{·}为求最大值运算；残差yk+1为k+1时刻量测值；ρ为遗忘因子，0＜ρ≤1，通常取ρ＝0.95；中上标s表示未引入减消因...

【技术特征摘要】

【专利技术属性】
技术研发人员：张安，鲍水达，任卫，
申请(专利权)人：西北工业大学，
类型：发明
国别省市：陕西;61