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