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

说明书

技术领域

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

背景技术

容积卡尔曼滤波(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进行修正，相关计算如下：