基于矩阵分解的卡尔曼滤波技术分析及应用

基于矩阵分解的卡尔曼滤波技术分析及应用【摘要】本文简要介绍了卡尔曼滤波研究的发展历程,重点对卡尔曼滤波及其在改善数值稳定性，提高计算效率等数值方面的研究与发展进行了综述，对Q-R分解，U-D分解，奇异值分解（SVD）等在卡尔曼滤波的应用进行了介绍。最后给出了一种基于Q-R矩阵分解的自适应滤波方法，仿真验证了其有效性。1引言1960年，美籍科学家卡尔曼(R.E.Kalman)在系统状态空间模型的基础上提出了著名的线性卡尔曼滤波器，它在线性的前提假设下是一个线性无偏、最小方差估计器，从而可以为线性滤波问题提供精确解析解。自该技术被提出以来，它已成为控制、信号处理与通信等领域最基本最重要的计算方法和工具之一,并已成功地应用到航空、航天、电力系统及社会经济等不同领域。随着微型计算机的普及应用,对卡尔曼滤波的数值稳定性、计算效率、实用性和有效性的要求越来越高.为此,人们在如何改善卡尔曼滤波的计算复杂性和数值稳定性方面作了大量的探索工作,各种基于平方根滤波与平滑,U-D分解滤波与平滑,奇异值分解滤波与平滑,状态与偏差分离滤波以及并行与分散滤波等方法得到不断发展.本文给出了矩阵分解的一些基础知识，并着重从卡尔曼滤波数值计算方法入手,对现有的常规卡尔曼滤波、基于矩阵的因式分解滤波的数值计算方法进行了较系统的介绍和分析,并在第四章给出了一种基于Q-R矩阵分解的自适应滤波算法。2常规卡尔曼滤波2.1协方差卡尔曼滤波考虑如下线性离散系统kkkkkwxAx1(2.1.1)kkkkvxCz(2.1.2)式中nkRx是状态向量,mkRz是量测向量,pkRw是系统噪声向量,mkRv是量测噪声向量.假设系统噪声和量测噪声是互不相关的零均值高斯白噪声,方差阵分别为kQ，kR，则协方差卡尔曼滤波方程为：111|ˆˆkkkkxAx(2.1.3)TkkkkTkkkkQAPAP1111111|(2.1.4)]ˆ[ˆˆ1|1|kkkkkkkkxCzKxx(2.1.5)1|][kkkkkPCKIP(2.1.6)11|1||][kTkkkkTkkkkRCPCCPK(2.1.7)理论分析和实际应用均证明上述滤波公式是数值不稳定的,其原因是由于计算机有限字长的限制,计算中舍入误差和截断误差的累积、传递会使协方差阵kP失去对称正定性,因此,Joseph提出一种所谓“稳定化”卡尔曼滤波,其目的是减小滤波算法对计算舍入误差的灵敏性,保证kP的对称正定性,以提高滤波的数值稳定性,防止发散.其滤波阵公式，只是将(2.1.6)式改写为如下形式即可:TkkkTkkkkkkkKRKCKIPCKIP][][1|(2.1.8)但该算法由于所需计算量和存储量较大,而且并不一定很奏效,因而应用并不广泛.2.2信息滤波为了解决在某些没有有关初始状态信息和先验知识可供采用情况下的滤波,Fraser提出了信息滤波，即用协方差阵kP的逆1kP来代替kP的递推计算,这种算法对测量更新比较有效,但时间更新所需计算量较大.2.3推广卡尔曼滤波器推广卡尔曼滤波(EKF)是一种应用最广泛的非线性系统滤波方法。EKF与线性卡尔曼滤波公式完全类似,只是上述滤波公式中kA,k,和kC要在由非线性函数的偏导计算得到,不能象线性滤波那样可事先离线计算增益和协方差阵,但EKF与常规卡尔曼滤波一样,数值稳定性差,初值不易确定.为了改善上述常规滤波算法的数值稳定性,并提高计算效率,自七十年代以来,人们提出了平方根滤波、U一D分解滤波、奇异值分解滤波等一系列数值鲁棒的滤波算法.3基于矩阵因式分解的滤波方法3.1预备知识定理3.1.1设A是实正定对称矩阵，则存在唯一正线下三角矩阵S，使得TSSA（3.1.1）定理3.1.2Householder变换设nCu，且1uuH，则HnuuEuuEuH2)2;,()(（3.1.2）称为初等酉阵，或Householder变换。定理3.1.3Cholesky分解设nnnCA是正定Hermite矩阵，用L表示单位下三角矩阵，D是对角矩阵，则有HLDLDA))((2/12/1（3.1.3）定理3.1.4QR分解设nnnRA，则A可唯一地分解为QRA（3.1.4）定理3.1.5奇异值分解设nmrCA，r,,,21是A的r个奇异值，则存在m阶酉矩阵和n阶酉矩阵V，使得VOOODUA)(（3.1.5）其中),,,(21rdiagD，且),,2,1(||riii。3.2平方根协方差滤波（SRCF）首先提出平方根滤波思想的是Potter,他把kP按Cholesky方法分解为下三角阵kS,即令TkkkSSP,在滤波递推计算中用kS的传递计算代替kP的计算,由公式（3.1.1）可知，从而保证了kP的对称正定性.Potter的算法经美国阿波罗登月舱的实际应用,证明是很成功的.随后,Potter的算法被推广来解决存在着系统噪声和量测量为向量的情形。Schmidt给出了向量量测既可以同时处理,也可以序列处理的一种处理过程噪声的方法.为了提高平方根滤波的计算效率,Carlson注意到传递阵通常是块上三角阵的特点,给出了一种量测更新和时间更新均为上三角阵形式的快速平方根滤波,减少了计算量.上述平方根滤波均把时间更新和量测更新按常规分成两个分离的过程,其算法的关键是通过利用正交变换获得上三角阵的平方根矩阵.为了减小计算量,人们对如何构造正交变换的问题给予了很大的注意,常用的正交变换方法是Householder变换，即公式（3.1.2）、修正的Gram-Sehmidt正交化法及Givens变换等。1975年,Morf一Kailath在总结上述平方根滤波基础上,把时间更新和量测更新两个过程结合起来,给出了一种量测和时间更新的联合更新方程,从而仅需一个正交变换,即完成滤波计算,且无需计算滤波增益阵.3.2平方根信息滤波与平方根协方差滤波相对应,信息滤波的平方根滤波方法也得到人们的极大重视和研究。Dyer一MeReynolds基于Householder变换利用动态规划理论研究出一种平方根信息滤波(SRIF),与SRCF类似,SRIF把信息矩阵1kP定义为平方根阵形式,即定义TkkkSSP)1(11由1kS的递推计算来代替1kP的计算.Bierman利用“数据方程”法给出一种结构较简单的SRIF,并给出有色噪声情况的滤波公式,该算法需要计算状态转移矩阵的逆,即要求状态转移矩阵是非奇异的,针对这一问题,给出一种对状态转移阵奇异仍适用的SRIF,Bierma在此基础上,把SRIF应用于具有时间延迟系统的滤波,并把SRIF推广到大规模互联系统的情形,大大减少了计算量和存储量.正如SRCF那样,将量测更新和时间更新结合起来,可以容易的求得联合SRIF更新方程,Paige-Saunders基于把卡尔曼滤波转换为最小二乘估计的思想,提出一种联合量测更新和时间更新的SRIF方案。由于SRIF在某些情况下,如对于多量测量系统,比常规卡尔曼滤波有更高的计算效率、更好的数值稳定性和精度,因此,在轨道确定、飞行状态估计和多传感器跟踪与辨识等方面得到应用。3.3U-D分解滤波上述SRCF和SRIF,一般来讲由于存在矩阵的求逆运算和平方根计算,所需计算量较常规卡尔曼滤波要大,因而限制了在工程中的应用。Bierman在研究和应用SRIF及Carlson序列滤波的基础上,于1975~1977年间,提出了一套计算效率高、数值稳定的称之为“U-D分解”滤波的算法。该算法把协方差阵分解为单位上三角阵U和对角阵D,即有TkkkkUDUP，2/1UD相当于协方差平方根阵S，即公式（3.1.3）。U-D分解滤波既具有平方根滤波的优点,即始终能保证协方差阵的正定性,同时避免了Carlson等平方根滤波算法中平方根的计算,因而具有与常规卡尔曼滤波相当的计算量,是上述滤波算法中效率最高的一种算法,并且在实际应用问题中,结合实际问题的特点,U-D分解算法计算效率还更高,因而,近年来在轨道确定、目标跟踪和飞行状态估计及神经网络学习算法等方面得到广泛应用和发展,但该算法由于量测更新采用序列处理,对于量测量较多的系统,计算效率受到一定影响,对有色噪声的处理不如SRIF,SRCF方便。3.4基于奇异值分解（SVD）的滤波方法奇异值分解由于具有很强的数值鲁棒性和可靠性,广泛应用于最小二乘问题、病态方程组求解及广义逆计算等场合,并在控制、通讯与信号处理等领域越来越受到人们的极大重视。在滤波问题中,也已得到应用。Oshman基于协方差阵的频谱分解,以SVD为计算工具,提出称之为v-Lambda滤波的方法,他把协方差阵分解成TVVP形式,其中V是矩阵P的特征向量矩阵,为对角元是P阵奇异值的对角矩阵。首先给出量测更新为信息滤波模式,时间更新为协方差滤波模式的v一Lambda滤波方法。随后,Oshman又给出量测更新和时间更新方程均为信息滤波模式的滤波公式,该算法不需计算滤波增益,由于利用SVD,使得状态估计算法鲁棒性较之平方根滤波、U-D分解滤波更好,但该算法由于进行一步滤波迭代计算,需一次正交变换,两次奇异值分解,所以,其缺点是计算量较大,但其优异的数值鲁棒性以及随着奇异值分解并行处理的实现而随之带来计算时间的减少,使得此算法将成为一种极富吸引力的滤波方法.另外利用协方差阵的对称正定性,给出一种类似U-D分解形式、计算量较小的基于SVD的滤波算法,本文又给出一种基于SVD的推广卡尔曼滤波算法,并应用于飞行状态估计问题,随后又提出一种基于SVD的递推最小二乘辨识新方法,与递推最小二乘、基于U-D分解的递推最小二乘法相比,不仅收敛速度快、数值稳定性和辨识精度高,而且能得到系统参数的无偏估计。4一种基于Q-R矩阵分解的自适应滤波算法4.1机动目标加速度模型及自适应卡尔曼算法令状态向量X为:TxxxX]3,2,1[,x1,3,2,1xxx分别表示目标的位置、速度和加速度。假设目标当前加速度服从非零均值的一阶马尔可夫过程,即:)(3tx（4.1.1）)()()(twtt（4.1.2）目标的时域状态方程为：)(tLwBAXX（4.1.3）式中：00100010A、00B、100L、)(tw为白噪声序列。根据上述时域方程,经过离散化处理后,得到系统的离散状态方程和观测方程为:)()()(),1()1(kWkBkXkkkX（4.1.4）)()()(kVkHXkY（4.1.5）式中：TTTeeeTTkk00)1(110)1(11),1(2TTTeeTeTTkB11)12(1)(2dweeeTkkWTkTkTkTkkT)(/}1{/)}))1((1{)())1(())1((2))1(()1(]001[H)(V)(kkW、均为白噪声序列。基于状态方程（4.1.4）和观测方程（4.1.5），依据标准的卡尔曼滤波方程，得到当前机动加速度统计模型的自适应卡尔曼滤波算法。1.时间更新)(ˆˆ)1/1()1,()1/(kBXXkkkkkk(4.1.6))1/(3ˆkkx(4.1.7)/))/(3ˆ)(4(2max2kkx(4.1.8)wTQkkkkPkkkkp)1,()1/1()1,()1/((4.1.9)1))1/(()1/()(RHkkHPHkkPkKTT(4.1.10)其中，max表示最大可能的加速度。2.测量更新)]1/(ˆ)()[(ˆˆ)1/()/(kkXHkYkKXXkkkk(4.1.11))1/(])([)1/()()