改进的强跟踪SVD-UKF算法在组合导航中的应用

改进的强跟踪SVD-UKF算法在组合导航中的应用孙磊;黄国勇;李越【摘要】针对无迹卡尔曼滤波(Unscented Kalman Filter,UKF)在系统强非线性或状态模型不精确的情况下,存在滤波精度降低甚至发散的问题,提出一种改进的强跟踪SVD-UKF算法.该算法采用奇异值分解(Singular Value Decom-position,SVD)的方法改进UKF中状态协方差矩阵的迭代,保证协方差矩阵的非负定性及迭代的稳定性;算法基于强跟踪滤波(Strong Tracking filter,STF)理论框架,对改进的SVD-UKF引入多重渐消因子自适应调整状态协方差矩阵,在系统状态发生突变的情况下,实现系统真实状态的强跟踪.将该算法在BDS/INS组合导航中仿真验证,结果表明了该算法的有效性.%The performance of the Unscented Kalman filter would be degraded in accuracy or divergences when the sys-tem states are uncertain and strong nonlinear, an improved strong tracking SVD-UKF algorithm is proposed. The iteration of covariance matrix in UKF is improved by Singular Value Decomposition(SVD)of covariance matrix, ensured the sta-bility of the iteration of covariance matrix and restrained the negative definiteness of system state covariance matrix. Mul-tiple fading factors matrices are introduced in improved SVD-UKF, in order to automatic improve system state covariance matrix based on Strong Tracking Filter(STF)theory framework, and realize the strong tracking of the real state while sys-tem status are mutating. The proposed strong tracking SVD-UKF is applied to the BDS/INS integrated system for simula-tion, simulation results show the effectiveness of the presented algorithm.【期刊名称】《计算机工程与应用》【年(卷),期】2017(000)010【总页数】6页(P225-229,240)【关键词】无迹卡尔曼滤波(UKF);奇异值分解(SVD);强跟踪;渐消因子;组合导航【作者】孙磊;黄国勇;李越【作者单位】昆明理工大学信息工程与自动化学院,昆明 650500;云南省矿物管道输送工程技术研究中心,昆明 650500;昆明理工大学信息工程与自动化学院,昆明650500;云南省矿物管道输送工程技术研究中心,昆明 650500;昆明理工大学信息工程与自动化学院,昆明 650500;云南省矿物管道输送工程技术研究中心,昆明650500【正文语种】中文【中图分类】V249.32+8非线性系统状态估计在组合导航中应用十分广泛，非线性滤波方法成为组合导航的热门研究之一。

扩展卡尔曼滤波（Extended Kalman filter，EKF）是典型的代表之一，EKF是以标准卡尔曼滤波为基本框架，将非线性系统模型线性化，即对系统状态估值进行一阶Taylor展开，然后进行卡尔曼滤波计算[1]。

然而，当系统模型非线性较强时，对系统模型的近似线性会引起较大的截断误差，滤波精度将大大降低甚至发散[2]。

为此，一些学者提出了无迹卡尔曼滤波（Unscented Kalman filter，UKF）算法，UKF在非线性系统状态估计的思路上与EKF不同，以确定性采样估计的方法，采用UT变换对非线性系统后验概率密度函数进行近似[3]。

UKF 相对于EKF有其二阶以上近似精度的后验均值与方差[4-5]。

然而UKF计算时由于计算机的字长限制会出现协方差矩阵的负定，导致系统发散；UKF在强非线性系统状态突变的情况下，对实际状态并没有很好的跟踪能力[6]。

许多学者在UKF的计算机字长限制导致数值计算稳定性问题上进行了研究。

其中，Erica.Wan等人提出了平方根无迹卡尔曼滤波器（Square Root Unscented Kalman Filter，SRUKF），在协方差矩阵迭代时，使用协方差矩阵的平方根进行迭代，从而保证协方差矩阵的（半）正定性[7]。

而在UKF中，采用奇异值分解代替平方根分解能够更好保证协方差矩阵迭代计算的稳定性，文献[8]中提出采用奇异值分解的方法代替UKF中cholesky分解的方法，能够提高数值计算稳定性，但未将奇异值分解引入协方差矩阵迭代，本质上并没有真正提高协方差矩阵计算的稳定性。

而对于UKF的系统强非线性时的突变跟踪能力，文献[9]中将STF框架引入UKF中，通过渐消因子在线调整状态预测误差协方差阵，强迫新息序列保持正交性，从而保持对系统状态变化的实时跟踪。

而将STF应用在UKF中需要计算雅可比矩阵，提高了计算复杂度，并且对于计算数值稳定性不能保证。

针对以上UKF存在矩阵计算数值稳定性和状态突变无法实时跟踪的问题，本文利用奇异值分解协方差矩阵进行迭代，并应用STF框架结合UKF，提出了一种改进的强跟踪SVD-UKF算法。

将简化的UKF算法的协方差矩阵部分因子进行奇异值分解，将分解后的协方差矩阵进行迭代，保证了算法中矩阵计算的稳定性；同时将STF中的渐消因子引入UKF中，在系统状态突变、系统模型不精确的情况下，提高了滤波系统的自适应能力。

2.1 简化的UKF算法在组合导航中，量测方程为线性，而在标准UKF算法中，非线性量测方程运算复杂度较高，在运算能力较低的硬件设备上运行性能较低。

当UKF算法中量测方程为线性时，将线性量测方程带入UKF算法中并简化，降低算法复杂度。

故针对组合导航，设如下带加性噪声的非线性系统状态方程和线性量测方程:式中，Xk∈Rn为n×1维的系统状态向量，Zk∈Rm为m×1维的量测向量，f(⋅)为非线性函数，Hk为线性量测阵。

wk和vk分别为状态系统高斯白噪声和测量系统高斯白噪声，它们的均值为0，且相互独立。

它们的协方差分别为Qk和Rk。

基于非线性系统式（1），UKF算法中量测方程变为线性时，则UKF中系统量测更新方程如下:由上可知，将线性量测方程带入UKF算法中，对Pxˆkzˆk和Pzˆk/zˆk进行迭代计算时，将线性系数分配提出，则系统状态协方差Pk/k和增益矩阵K表达式都退化为标准卡尔曼滤波的表达式，得到简化的UKF算法。

简化的UKF算法针对组合导航量测方程为线性的特性进行简化，在滤波性能上并没有降低，满足组合导航的要求。

2.2 改进的SVD-UKF算法为了保证UKF中误差协方差矩阵的正定性，SRUKF中引入了误差协方差矩阵的平方根代替误差协方差矩阵进行迭代，保证了协方差矩阵的（半）正定性。

但是当Sigma点的零权值系数为负或者数值计算误差太大时，有可能造成Cholesy分解失败，引起算法无法迭代，滤波器发散[10]。

而SVD在矩阵发生病态之后依然能够分解矩阵，表现了比Cholesy分解更好的稳定性。

文献[11]中将SVD引入EKF 中，得到了很好的协方差矩阵迭代稳定性，而将SVD引入UKF中，UKF的协方差矩阵迭代过程相比于EKF更加复杂。

本文针对量测方程为线性的组合导航系统，通过对简化的UKF算法引入SVD方法分解协方差矩阵部分因子，利用矩阵的性质对分解后的协方差矩阵进行迭代，优化了迭代过程，不仅保证了系统数值稳定性，还提高了算法迭代的稳定性。

2.2.1 奇异值分解假设A为m×n阶矩阵，根据奇异值分解[12]理论，存在正交矩阵U与V使得:称式（7）和式（8）为A的奇异值分解。

其中σi为A的奇异值，数值依次从大变小。

当矩阵A为对称正定矩阵时，式（7）则变成如下:式中，A的奇异值σi均大于0。

2.2.2 改进的SVD-UKF算法将简化的UKF算法中的误差协方差矩阵部分因子进行奇异值分解，利用矩阵的性质对分解后的矩阵进行迭代，即为改进的SVD-UKF算法。

由式（9）可知:则对UKF中的Pk/k进行奇异值分解，可以得到如下:式中Pk/k奇异值分解形式由上一个状态迭代产生的。

这一状态的Sigma点计算更新如下:下面求解状态误差预测协方差矩阵Pk/k-1的奇异值分解矩阵。

对Pk/k-1直接进行奇异值分解可以得到如式（13）:UKF算法通过预测协方差矩阵Pk/k-1求解增益矩阵K和输出协方差矩阵Pk/k，而在SVD-UKF中这样求解不利于分解后的协方差矩阵迭代，故采用式（14）和式（15）求解增益矩阵K和输出协方差矩阵Pk/k。

由式（14）和式（15）可以求得输出协方差矩阵逆P-1k/k如下:式（16）中，对R-1进行cholesky分解，得到如下:则式（16）中间部分可以等价为如下:对式（18）中的因子进行奇异值分解，可以得到如下:将奇异值分解后的式（19）带入式（16）中，则式（16）可以表示成如下:由式（20）可以发现，在一个迭代周期内，对协方差矩阵的求解，只需要由式中因子奇异值分解后计算得出，P-1k/k中的参数可以表示如下:通过以上推导可知，Pk-/1k的求解并不需要多次求逆和多步分解计算，计算复杂度较低。

由式（15）可知，增益矩阵K的公式如下:由此可以得到改进的SVD-UKF算法，算法中通过误差协方差矩阵部分因子奇异值分解后，再对协方差矩阵进行迭代，优化了SVD-UKF的迭代过程，即使协方差矩阵发生病态，SVD依然能够对协方差矩阵进行分解，保证UKF算法的正常迭代和协方差矩阵的半正定性。

改进的SVD-UKF相比于SRUKF和UKF具有更强的矩阵分解能力和迭代稳定性，但是当系统本身所建状态模型不精确，或者系统状态发生突变的时候，改进的SVD-UKF并没有较好的自适应能力，可能会因突变状态引起滤波器发散。

为此，本文在改进的SVD-UKF中引入多重渐消因子，构成改进的强跟踪SVD-UKF算法，该算法既保证了在滤波过程中误差协方差的（半）正定性，又能在突变情况下抑制滤波器发散，实现了对真实状态的强跟踪。