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

摘要

本发明提供了一种改进的强跟踪平方根容积卡尔曼滤波方法，通过分析减消因子提高强跟踪算法鲁棒性的机理和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，时间更新如下：$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_kK_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/k S_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的平方根，有