CN105356860A - 改进的强跟踪平方根容积卡尔曼滤波方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明涉及一种非线性滤波方法。

背景技术

容积卡尔曼滤波(cubatureKalmanfilter,CKF)是由加拿大学者IenkaranArasaratnam和SimonHaykin在2009年提出的一种新的非线性近似滤波算法。由于CKF求解容积点时，要对协方差阵开方，随着滤波迭代次数的增加，舍入误差的积累，可能会导致协方差阵失去非负定性甚至失去对称性。IenkaranArasaratnam和SimonHaykin在CKF的基础上，加入平方根算法，提出平方根容积卡尔曼滤波算法(square-rootcubatureKalmanfilter,SCKF)。该算法有效地解决了CKF的数值稳定性问题，并减少计算量，提供更佳的滤波性能。

在实际系统中，由于(1)数学模型不准确；(2)系统本身在缓慢动态变化，而建立的数学模型难以根据这些变化动态改变模型，导致模型匹配性逐渐变差；(3)受系统外部变化影响等原因，系统模型并不能完全准确，导致SCKF滤波效果不理想，严重时甚至导致滤波发散。周华东等人基于正交原理建立强跟踪算法(strongtrackingfilter,STF)，大大增强了非线性滤波算法的鲁棒性。结合强跟踪的思想，将减消因子引入SCKF中，可建立强跟踪SCKF算法(strongtrackingSCKF，STSCKF)，克服SCKF在系统模型不确定时滤波精度下降的缺点。STSCKF已经被应用在机动跟踪、组合导航等领域，有效提高系统的鲁棒性和精度。

但STSCKF算法引入减消因子过程中，首先要获取k+1时刻一步预测互相关协方差阵，再计算减消因子，最后进行量测更新。根据SCKF实现步骤，这种减消因子引入方法等价于将量测更新中“计算容积点”到“计算互相关协方差阵”部分重复执行了两次，大幅增加了算法的时间复杂度。

发明内容

为了克服现有技术的不足，本发明提供一种改进的强跟踪SCKF算法(improvedstrongtrackingSCKF)，通过分析减消因子提高强跟踪算法鲁棒性的机理和SCKF算法流程特点，ISTCKF重新选择减消因子引入位置，减少由于减消因子引入带来额外计算量。

本发明解决其技术问题所采用的技术方案包括以下步骤：

(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，时间更新如下：

其中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)量测更新，包括以下内容：

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

计算减消因子λ_k+1：

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

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

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])

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

其中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的平方根，有

本发明的有益效果是：

(1)减消因子引入位置调整到χ_k+1/k处，仅有χ_k+1/k、P_xy,k+1/k、P_yy,k+1/k和K_k+1等变量的计算受到减消因子的影响，不必对容积点以及传递后的容积点集等变量重复求解，减少了重复执行步骤，降低了时间复杂度，提高了算法效率。由于计算传递后的容积点集步骤的时间复杂度与状态向量维数和量测函数h(·)复杂程度密切相关，对于系统状态维数越高，h(·)越复杂的系统，时间复杂度的优化效果越明显；

(2)算法精度与改进前相当，并没有因为降低算法时间复杂度而影响算法精度。

附图说明

图1是SCKF状态估计与估计误差示意图；

图2是STSCKF状态估计与估计误差示意图；

图3是ISTSCKF状态估计与估计误差示意图。

具体实施方式

下面结合附图和某轮船推进系统的实施例对本发明进一步说明，本发明包括但不仅限于下述实施例。

本发明的实现步骤如下：

考虑如下离散时间非线性动态系统：

x_k+1＝f_k(x_k)+w_k

y_k+1＝h_k+1(x_k+1)+v_k+1

其中x_k∈Rⁿ为系统状态向量，y_k+1∈R^m为量测向量；和分别为非线性系统的状态函数和量测函数；w_k∈Rⁿ为系统噪声，v_k+1∈R^m为量测噪声，二者均为高斯白噪声，互不相关，且协方差分别为Q和R。

基于以上非线性系统的ISTSCKF算法具体流程如下：

1)设定初始参数

设定初始时刻系统状态值x₀，初始时刻系统状态协方差平方根S₀，系统噪声协方差Q，观测噪声协方差R，遗忘因子ρ。

2)时间更新

假设已知系统k时刻的估计状态和协方差阵平方根S_k。

①选取容积点X_i,k(i＝1,2,...,m)

其中m＝2n，n为状态向量维数。

②计算传递后容积点

③计算k+1时刻状态预测值

④计算k+1时刻预测误差协方差阵平方根S_k+1/k

其中S_Q,k为k时刻的系统噪声平方根，有

3)量测更新

①计算容积点

②计算传递后容积点y_i,k+1/k＝h(X_i,k+1/k)；

③计算k+1时刻观测预测值

④计算k+1时刻加权中心矩阵Y_k+1/k

⑤计算互相关协方差阵

其中

⑥计算渐消因子λ_k+1

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

ρ为遗忘因子，0＜ρ≤1，通常取ρ＝0.95。

若λ_k+1＞1，则执行步骤⑦，否则执行步骤⑧。

⑦引入减消因子

当λ_k+1＞1时，由于将减消因子引入位置选择在χ_k+1/k处。计算引入减消因子后χ_k+1/k、P_xy,k+1/k、P_yy,k+1/k和K_k+1，推导如下：

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已由步骤④和步骤⑤求得，可按照SCKF中公式计算S_yy,k+1/k和K_k+1，计算公式如下：

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

其中S_yy,k+1/k表示k+1时刻量测误差协方差阵平方根。

⑨计算k+1时刻状态估计值

⑩计算k+1时刻状态误差协方差阵平方根S_k+1

减消因子引入位置调整到χ_k+1/k后，χ_k+1/k中有关Q_k的信息被放大，导致S_k+1偏大，现推导修正式如下：

根据正交原理，w_k与Y_k+1/k、v_k是相互正交的，χ_k+1/k与Y_k+1/k乘积结果并没有被影响，只需对导致的误差进行修正。

其中(λ_k+1-1)Q_k即为误差项，则有

其中P′_k+1表示修正后的k+1时刻状态误差协方差阵。因此修正后的S_k+1求解公式如下：

其中S_R,k+1表示R_k+1的平方根，有

与标准SCKF流程相比，ISTSCKF对“量测误差协方差阵平方根”的计算位置进行了调整，从量测更新中的步骤④调整到步骤⑧，在步骤④中仅对残差序列进行了计算，避免当λ_k+1＞1时，P_yy,k+1/k重复计算。

引入减消因子后P_xy,k+1/k、P_yy,k+1/k和K_k+1计算公式与SCKF中存在明显差异，因此在量测更新的步骤⑥中存在一个跳转：若λ_k+1＞1，则执行步骤⑦，否则执行步骤⑧。

某轮船推进系统模型如下：

其中，a为船壳所受阻力，b为发动机效率，标称值为a₀＝-0.58，b₀＝0.2；u为控制信号。仿真初始参数设置如下：

Q＝0，R＝0.001

x0＝0，Skk＝10

设置过程参数b＝0.75b₀，算法中参数仍取a₀和b₀。

图1、图2和图3分别给出了SCKF、STSCKF和ISTSCKF的算法滤波结果和状态估计误差，估计误差的定义为图中u(t)表示控制信号波形。图中显示，三种算法都能完成滤波，没有发散，但是各算法滤波效果有所不同。SCKF鲁棒性较差，对系统参数变化比较敏感，故它的滤波效果最差，估计误差最大。STSCKF和ISTSCKF引入强跟踪滤波器思想，基于正交原理，调整增益，迫使残差正交，动态增大了估计值中量测值所占的比重，降低一步预测值在估计值中的重要性，故滤波结果均优于SCKF。ISTSCKF和STSCKF滤波性能相当，ISTSCKF并没有因为修改减消因子引入位置而影响滤波精度。

接下来比较两种算法时间复杂度。仿真软件采用matlab2013a，仿真计算机处理器为intel酷睿i3，主频3.20GHz。利用matlab提供的Profiler对算法时间复杂度进行分析。

设置滤波次数为2000次，运行算法的M文件，由Profile工具得两种算法运行时间如表1所示。ISTSCKF比STSCKF少消耗0.312s，它的时间复杂度约为STSCKF的73.31％，极大地降低了算法时间复杂度。

表1算法运行时间

Claims (1)

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，时间更新如下：

$X_{i,k}=S_k{\mathrm{\ξ}}_i+{\hat{x}}_k$

$X_{i,k+1/k}^{*}=f\left(X_{i,k}\right)$

${\hat{x}}_{k+1/k}=\frac{1}{m}\underset{i=1}{\overset{m}{\Σ}}{X}_{i,k+1/k}^{*}$

$S_{k+1/k}=Tria(\[\begin{array}{cc}{\mathrm{\χ}}_{k+1/k}^{*}& S_{Q,k}\end{array}\])$

${\mathrm{\χ}}_{k+1/k}^{*}=\frac{1}{\sqrt{m}}\[\begin{array}{cc}{X}_{1,k+1/k}^{*}-{\hat{x}}_{k+1/k}& X_{2,k+1/k}^{*}-{\hat{x}}_{k+1/k}...X_{n,k+1/k}^{*}-{\hat{x}}_{k+1/k}\end{array}\]$

其中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)量测更新，包括以下内容：

$X_{i,k+1/k}=S_{k+1/k}{\mathrm{\ξ}}_i+{\hat{x}}_{k+1/k}$

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

${\hat{y}}_{k+1/k}=\frac{1}{m}\underset{i=1}{\overset{m}{\Σ}}{y}_{i,k+1/k}$

$Y_{k+1/k}=\frac{1}{\sqrt{m}}\[\begin{array}{cc}{y}_{1,k+1/k}-{\hat{y}}_{k+1/k}& y_{2,k+1/k}-{\hat{y}}_{k+1/k}...y_{m,k+1/k}-{\hat{y}}_{k+1/k}\end{array}\]$

$P_{xy,k+1/k}={\mathrm{\χ}}_{k+1/k}{Y}_{k+1/k}^T$

${\mathrm{\χ}}_{k+1/k}=\frac{1}{\sqrt{m}}\[\begin{array}{cc}{X}_{1,k+1/k}-{\hat{x}}_{k+1/k}& X_{2,k+1/k}-{\hat{x}}_{k+1/k}...X_{n,k+1/k}-{\hat{x}}_{k+1/k}\end{array}\]$

计算减消因子λ_k+1：

$\left\{\begin{array}{c}{P}_{k+1/k}=S_{k+1/k}{S}_{k+1/k}^T\\ H_{k+1}={\[P_{xy,k+1/k}\]}^T{\[{\left(P_{k+1/k}\right)}^{-1}\]}^T\\ N_{k+1}=V_{k+1}-H_{k+1}{Q}_k{H}_{k+1}^T-R_{k+1}\\ M_{k+1}=H_{k+1}(P_{k+1/k}-Q_k)H_{k+1}^T\\ C_{k+1}=\frac{tr\left(N_{k+1}\right)}{tr\left(M_{k+1}\right)}\\ {\mathrm{\λ}}_{k+1}=max\{1,C_{k+1}\}\end{array}\right.$

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

$V_{k+1}=\left\{\begin{array}{c}{\mathrm{\γ}}_1{\mathrm{\γ}}_1^T,k=1\\ \frac{{\mathrm{\ρV}}_k+{\mathrm{\γ}}_{k+1}{\mathrm{\γ}}_{k+1}^T}{1+\mathrm{\ρ}},k>1\end{array}\right.$

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

${\mathrm{\χ}}_{k+1/k}=\sqrt{{\mathrm{\λ}}_{k+1}}{\mathrm{\χ}}_{k+1/k}^s$

$P_{xy,k+1/k}=({\mathrm{\χ}}_{k+1/k}{\mathrm{\χ}}_{k+1/k}^T+(1-{\mathrm{\λ}}_{k+1}\left)Q_k\right)H_{k+1}^T$

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])

$K_{k+1}=(P_{xy,k+1/k}/S_{yy,k+1/k}^T)/S_{yy,k+1/k}$

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

${\hat{x}}_{k+1}={\hat{x}}_{k+1/k}+K_{k+1}(y_{k+1}-{\hat{y}}_{k+1/k})$

$S_{k+1}=Tria(\[\begin{array}{ccc}{\mathrm{\χ}}_{k+1/k}-K_{k+1}{Y}_{k+1/k}& K_{k+1}{S}_{R,k+1}& \sqrt{(1-{\mathrm{\λ}}_{k+1})Q_k}\end{array}\])$

其中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的平方根，有