，，，"观):；
[0067] ②计算传递后容积点yiik+1/k=h(Xiik+1/k);
[0069] ④计算k+1时刻加权中心矩阵Yk+1/k
[0070]
[0071]⑤计算互相关协方差阵戽= 册if册；
[0073] ⑥计算渐消因子Ak+1
[0074]
[0075] 其中Vk+1为实际残差输出序列的协方差矩阵，估算公式如下：
[0076]
[0077] P为遗忘因子，0 <P彡1，通常取P= 0·95。
[0078] 若λk+1> 1，则执行步骤⑦，否则执行步骤⑧。
[0079] ⑦引入减消因子
[0080] 当Ak+1> 1时，由于/将减消因子引入位置选择在xk+1/1^。计 算引入减消因子后Xk+l/k、Pxy，k+l/k、Pyy，k+l/k和Kk+1，推导如下：
[0084] Kk+1=Pxyik+l/k/Pyy,k+l/k
[0085] 其中X#表示未引入减消因子时的变量。
[0086] 执行完成后转步骤⑨。
[0087] ⑧不引入减消因子
[0088] 当Ak+1彡1成立时，表示在此时刻非线性系统是准确的，不用对增益矩阵Kk+1进行 修正，则Pxy,k+1/jPYk+1/k已由步骤④和步骤⑤求得，可按照SCKF中公式计算Syy,k+1/dPKk+1， 计算公式如下：
[0089] Syy_k+1/k=Tria([Yk+1/kSRjJ)
[0090]
[0091] 其中Syy，k+1/k表示k+1时刻量测误差协方差阵平方根。
[0092]⑨计算k+Ι时刻状态估计值+ /^(八+1-U;
[0093] ⑩计算k+Ι时刻状态误差协方差阵平方根Sk+1
[0094] 减消因子引入位置调整到xk+1/k后，Xk+1/k中有关Qk的信息被放大，导致Sk+1偏大， 现推导修正式如下：
[0095]
[0096] 根据正交原理，wk与Yk+1/k、vk是相互正交的，Xk+1/1^Yk+1/k乘积结果并没有被影 响，只需对导致的误差进行修正。
[0097]
[0098] 其中Uk+1-l)Qk即为误差项，则有
[0099]
[0100] 其中P'k+1表示修正后的k+1时刻状态误差协方差阵。因此修正后的Sk+1求解公 式如下：
[0101]
[0102] 其中SR,k+1表示Rk+1的平方根，有 "i+l. =&，A-+1.4./-+1。
[0103] 与标准SCKF流程相比，ISTSCKF对"量测误差协方差阵平方根"的计算位置进行了 调整，从量测更新中的步骤④调整到步骤⑧，在步骤④中仅对残差序列进行了计算，避免当 入k+1> 1时，Pyy,k+1/k重复计算。
[0104] 引入减消因子后Pxy，k+1/k、Pyy，k+1/k和Kk+1计算公式与SCKF中存在明显差异，因此在 量测更新的步骤⑥中存在一个跳转：若λk+1> 1，则执行步骤⑦，否则执行步骤⑧。
[0105] 某轮船推进系统模型如下：
[0106]
[0107] 其中，a为船壳所受阻力，b为发动机效率，标称值为aQ= -0· 58，bQ= 0· 2 ;u为控 制信号。仿真初始参数设置如下：
[0108] Q= 0，R= 0.001
[0109] x0 = 0,Skk= 10
[0110] 设置过程参数b= 0. 75b。，算法中参数仍取a。和b。。
[0111] 图1、图2和图3分别给出了SCKF、STSCKF和ISTSCKF的算法滤波结果和状态估 计误差，估计误差的定义为Δτ,=元-巧，图中u(t)表示控制信号波形。图中显示，三种 算法都能完成滤波，没有发散，但是各算法滤波效果有所不同。SCKF鲁棒性较差，对系统参 数变化比较敏感，故它的滤波效果最差，估计误差最大。STSCKF和ISTSCKF引入强跟踪滤波 器思想，基于正交原理，调整增益，迫使残差正交，动态增大了估计值中量测值所占的比重， 降低一步预测值在估计值中的重要性，故滤波结果均优于SCKF。ISTSCKF和STSCKF滤波性 能相当，ISTSCKF并没有因为修改减消因子引入位置而影响滤波精度。
[0112] 接下来比较两种算法时间复杂度。仿真软件采用matlab2013a，仿真计算机处理器 为intel酷睿i3,主频3. 20GHz。利用matlab提供的Profiler对算法时间复杂度进行分 析。
[0113] 设置滤波次数为2000次，运行算法的Μ文件，由Profile工具得两种算法运行时 间如表1所示。ISTSCKF比STSCKF少消耗0. 312s，它的时间复杂度约为STSCKF的73. 31 %， 极大地降低了算法时间复杂度。
[0114] 表1算法运行时间
[0115]
【主权项】
1. 一种改进的强跟踪平方根容积卡尔曼滤波方法，其特征在于包括下述步骤： (1) 设定初始参数设定，包括初始时刻系统状态值Xc、初始时刻系统状态协方差平方根 S。、系统噪声协方差Q、观测噪声协方差R和遗忘因子P ; (2) 时间更新，包括以下内容： 首先定义S = Tria (Amxn)表示一种矩阵三角分解运算，At= QaRa，其中Qa为正交阵，R a 为上三角矩阵，取Ra的前MXM阶矩阵的转置，即S = (Rmxm) τ; 假设已知系统k时刻的估计状态毛和协方差阵平方根Sk，时间更新如下：其中i = 1，2, = 2η，η为状态向量维数；Xiik为容积点集；记η维 单位列向量e = [1，0,…，0]τ，使用符号[1]表示对e的元素进行全排列和改变元素符号产 生的点集，称为完整全对称点集，[1]1表示点集[1]中的第i个点；为通过状态函数 传递后的容积点集；f( ·)为非线性状态函数；为k+Ι时刻状态预测值；Sk+1/k为k+Ι时 刻预测误差协方差阵平方根；为k+Ι时刻.的加权中心矩阵；Stu为k时刻的系统 噪声平方根，有(3) 量测更新，包括以下内容：计算减消因子Ak+1:其中Vk+1为实际残差序列的协方差矩阵，估算公式如下：若Ak+1> 1，表示残差信息没有被完全提取，要对增益矩阵Kk+1进行修正，相关计算如 下：若Ak+1< 1，表示在此时刻非线性系统是准确的，不用对增益矩阵Kk+1进行修正，则 Pxy,k+1/$ γ k+1/k已求得，增益矩阵K k+1计算如下：最后计算k+Ι时刻状态估计值和k+Ι时刻状态误差协方差阵平方根完成量测更新：其中\k+1/k为容积点集；y llk+1/k为通过量测函数传递后的容积点集；h( ·)为非线性量 测函数；^为k+Ι时刻观测预测值；Yk+1/k为k+Ι时刻y lik+1/1jjp权中心矩阵；P xy,k+1/A k+1 时刻互相关协方差阵；xk+1/kSk+i时刻Xlik+1/k的加权中心矩阵；λ k+1为k+Ι时刻渐消因子； Pk+1/k为k+Ι时刻预测状态误差协方差阵；H k+1为k+Ι时刻量测函数h ( ·)对X的偏导的雅 可比矩阵；Nk+1，Mk+1，Ck+1为求解减消因子中使用的中间过程矩阵；tr( ·)为矩阵求迹运算； max{ · }为求最大值运算；残差为k+Ι时刻量测值；P为遗忘因子，O < P彡1，通常取P = 0. 95 ; 中上标s表示未引入减消因子时的变量；Pyy,k+1/kS k+1 时刻量测误差协方差阵；Kk+1为k+1时刻增益矩阵；S yyik+1/k为k+1时刻量测误差协方差阵平 方根；元+1为k+Ι时刻状态估计值；S k+1为k+Ι时刻状态误差协方差阵平方根；SRik+1为Rk+1的 平方根，有
【专利摘要】本发明提供了一种改进的强跟踪平方根容积卡尔曼滤波方法，通过分析减消因子提高强跟踪算法鲁棒性的机理和SCKF算法流程特点，ISTCKF重新选择减消因子引入位置，减少由于减消因子引入带来额外计算量。
【IPC分类】H03H17/02
【公开号】CN105356860
【申请号】CN201510377553
【发明人】张安, 鲍水达, 任卫
【申请人】西北工业大学
【公开日】2016年2月24日
【申请日】2015年7月1日