经典的卡尔曼滤波算法

自适应卡尔曼滤波卡尔曼滤波发散的原因如果卡尔曼滤波是稳定的，随着滤波的推进，卡尔曼滤波估计的精度应该越来越高，滤波误差方差阵也应趋于稳定值或有界值。但在实际应用中，随着量测值数目的增加，由于估计误差的均值和估计误差协方差可能越来越大，使滤波逐渐失去准确估计的作用，这种现象称为卡尔曼滤波发散。引起滤波器发散的主要原因有两点：（1）描述系统动力学特性的数学模型和噪声估计模型不准确，不能直接真实地反映物理过程，使得模型与获得的量测值不匹配而导致滤波发散。这种由于模型建立过于粗糙或失真所引起的发散称为滤波发散。（2）由于卡尔曼滤波是递推过程，随着滤波步数的增加，舍入误差将逐渐积累。如果计算机字长不够长，这种积累误差很有可能使估计误差方差阵失去非负定性甚至失去对称性，使滤波增益矩阵逐渐失去合适的加权作用而导致发散。这种由于计算舍入误差所引起的发散称为计算发散。针对上述卡尔曼滤波发散的原因，目前已经出现了几种有效抑制滤波发散的方法，常用的有衰减记忆滤波、限定记忆滤波、扩充状态滤波、有限下界滤波、平方根滤波、和自适应滤波等。这些方法本质上都是以牺牲滤波器的最优性为代价来抑制滤波发散，也就是说，多数都是次优滤波方法。自适应滤波在很多实际系统中，系统过程噪声方差矩阵Q和量测误差方差阵R事先是不知道的，有时甚至连状态转移矩阵或量测矩阵H也不能确切建立。如果所建立的模型与实际模型不符可能回引起滤波发散。自适应滤波就是这样一种具有抑制滤波发散作用的滤波方法。在滤波过程中，自适应滤波一方面利用量测值修正预测值，同时也对未知的或不确切的系统模型参数和噪声统计参数进行估计修正。自适应滤波的方法很多，包括贝叶斯法、极大似然法、相关法与协方差匹配法，其中最基本也是最重要的是相关法，而相关法可分为输出相关法和新息相关法。在这里只讨论系统模型参数已知，而噪声统计参数Q和R未知情况下的自适应滤波。由于Q和R等参数最终是通过增益矩阵K影响滤波值的，因此进行自适应滤波时，也可以不去估计Q和R等参数而直接根据量测数据调整K就可以了。输出相关法自适应滤波的基本途径就是根据量测数据估计出输出函数序列}{Ck，再由}{Ck推算出最佳增益矩阵K，使得增益矩阵K不断地与实际量测数据}{Ck相适应。.Sage-Husa自适应卡尔曼滤波是在利用量测数据进行递推滤波时，通过时变噪声估计估值器，实时估计和修正系统噪声和量测噪声的统计特性，从而达到降低系统模型误差、抑制滤波发散提高哦滤波精度的目的。kkkkkwxx11,kkkkvxHz0)()(,)()(,)(TjkkjkTkkkkkjkTkkkkvwERvvErvEQwwEqwESage-Husa自适应卡尔曼滤波算法可描述为kkkkkzKxx~ˆˆ1,111,1,ˆˆˆkkkkkkqxxkkkkkkrxHzzˆˆ~1,11,1,]ˆ[kTkkkkTkkkkRHPHHPK11,11,1,ˆkTkkkkkkkQPP1,)(kkkkkPHKIP.其中，krˆ、KRˆ、kqˆ和kQˆ由以下时变噪声统计估值器获得：)ˆ(ˆ)1(ˆ,1111kkkkkkkkxHzdrdr)~~(ˆ)1(ˆ1,11111TkkkkTkkkkkkHPHzzdRdR)ˆ(ˆ)1(ˆ,111kkkkkkkkxxdqdq.)~~(ˆ)1(ˆ,1,1111111TkkkkkkTkTkkkkkkkPPKzzKdQdQ式中：111kkbbd，10b为遗忘因子。如果系统状态变量的维数比较高，而Sage-Husa自适应滤波算法中又增加了对系统噪声统计特性的计算，计算量将大大增加，实时性也将难以得到保证。除此之外，对于阶次较高的系统，Sage-Husa自适应滤波算法中kR和kQ的在线估计有时会由于计算发散失去半正定性和正定性而出现滤波发散现象，此时Sage-Husa自适应滤波算法的稳定性和收敛性不能完全保证。基于极大似然准则的自适应卡尔曼滤波，通过系统状态方差阵和量测噪声方差阵实时估计系统噪声统计特性的变化，以保证滤波器更好地适应这种变化。极大似然估计从系统量测量出现概率最大的角度估计，其特点是不仅考虑新息的变化，而且考虑新息协方差矩阵vkC的变化。它的量测噪声协方差矩阵Rˆ和系统噪声协方差矩阵Qˆ为：TkkkkvkkHPHCR1,ˆˆ1,11,11ˆkkkTkkkTikNkiikPPxxNQkkkkkkvKxxx1,ˆˆkNkiTiivkvvNC11式中：1,ˆkkkkzzv，N为平滑窗口的宽度。扩展卡尔曼滤波最初提出的卡尔曼滤波基本理论只适用于状态方程和量测方程均为线性的随机线性高斯系统。但是大部分系统是非线性的，其中还有许多事强非线性的。非线性估计的核心就在于近似,给出非线性估计方法的不同就在于其近似处理的思想和实现手段不同。近似的本质就是对难以计算的非线性模型施加某种数学变换,变换成线性模型,然后用Bayes估计原理进行估计。进一步说,非线性变换到线性变换主要有两种实现手段,一种是Taylor多项式展开,一种是插值多项式展开。Bucy和Y.Sunahara等人致力于研究将经典卡尔曼滤波理论扩展到非线性随机系统滤波估计中，提出了离散非线性随机系统扩展卡尔曼滤波(Extendedkalmanfilter,以下简称EKF)。EKF是传统非线性估计中的代表，其基本思想是将非线性状态函数和量测函数进行局部线性化，即进行一阶Taylor多项式展开，然后应用线性系统Kalman滤波公式。非线性离散系统状态方程和观测方程的一般形式如下所示kkkkkkkkkvuxgzwuxfx),(),(11-1式中：rkRu为输入向量；pkRw和qkRv均为高斯白噪声，且互不相关，其统计特性为：其中，0),(),(,0)(),(,0)(jkkjkjkkkjkjkkvwCovRvvCovvEQwwCovwE式中，kQ为过程激励噪声协方差矩阵,kR为观测噪声协方差矩阵。),(11kkuxf是一个非线性状态转换函数,),(11kkuxg是一个非线性量测函数。每一个时刻点,根据一阶泰勒展开将),(11kkuxf,),(11kkuxg线性化，即将非线性状态函数)··(，f和非线性量测函数)··(，g围绕滤波值展开泰勒级数，并略去二阶以上项，得到)ˆ(),(),ˆ(),(ˆkkxxkkkkkkkxxxuxfuxfuxfkk1-2)ˆ(),(),ˆ(),(ˆkkxxkkkkkkkxxxuxguxguxgkk1-3定义kkxxkkkkxuxfˆ),(ˆ，kkxxkkkkxuxgHˆ),(ˆ，根据式（1-1）、式（1-2）和式（1-3）可以得到非线性系统线性化后只与状态变量有关的表达式，如下kkkkkkkkkkkkkkkkkvxHuxgxHzwxuxfxx]ˆ),ˆ([ˆ]ˆ),ˆ([ˆ11-4式1-4中,注意到kkkkxuxfˆ),ˆ(并非kx的函数,kkkkxHuxgˆ),ˆ(并非kx的函数,根据1-4近似结果,应用上节的Kalman滤波器计算可以得到EKF迭代算法:定义kkxxkkkkxuxfˆ),(ˆ，kkxxkkkkxuxgHˆ),(ˆ，可得滤波方程初始条件)var(),(ˆ0000xPxEx状态先验估计值),ˆ(ˆ111,kkkkuxfx误差协方差先验估计值TkkkkkTkkkkkkkQPP1,11,1,11,1,增益矩阵11,1,][kTkkkkTkkkkRHPHHPK状态后验估计值)],ˆ([ˆˆ11,kkkkkkkuxgzKxx误差协方差后验估计值1,)(kkkkkPHKIP无迹卡尔曼滤波（UKF）EKF是一种次优非线性高斯滤波器，它采用对非线性函数进行线性化近似的方法，来计算状态分布经非线性函数传递之后的特性。尽管EKF得到了广泛的应用，但它依然存在自身无法克服的理论局限性:①要求非线性系统状态函数和量测函数必须是连续可微的，这限制了EKF的应用范围；②对非线性函数的一阶线性化近似精度偏低，特别地，当系统具有强非线性时，EKF估计精度严重下降，甚至发散；③需要计算非线性函数的雅克比矩阵，容易造成EKF数值稳定性差和出现计算发散。为了克服上述EKF的缺陷，能够以较高的精度和较快的计算速度处理非线性高斯系统的滤波问题，Julier等人根据确定性采样的基本思路，基于Unscented变换（UT）提出了Unscented卡尔曼滤波（UKF）。与EKF类似，UKF仍继承了卡尔曼滤波器的基本结构，不同之处在于UKF用Unscented变换取代了EKF中的局部线性化。UKF仍假设随机系统的状态必须服从高斯分布，但取消了对系统模型的限制条件，也就是说，不要求系统是近似线性的，同时，UKF不需要计算雅克比矩阵，因此不要求状态函数和量测函数必须是连续可微的，它甚至可以应用于不连续系统。可以证明：不论系统非线性程度如何，UT变换理论上至少能以三阶泰勒精度逼近任何非线性高斯系统状态的后验均值和协方差，因此UKF的理论估计精度优于EKF。UKF法首先要构造Sigma散点集,设状态向量为n维,1ˆkx为时刻k-1的状态向量估计值,1kP为该时刻状态向量的协方差矩阵,2n+1维的Sigma点集可以表示为:ikikkniikikkikkPnxxPnxxxx))((ˆ))((ˆˆ1,11,1,11,11,0，i=1,2，...，n�对应于i的一阶二阶权系数为)(2/1)/(nnWmi00ii)(2/11)/(2nnWci00ii其中，nn）（2参数决定第i个Sigma点在状态均值1ˆkx周围的扩展空间,是取值区间为[0.0001,1]的常数；为冗余量；为与状态向量的先验分布相关的参数,对高斯分布,=2为最优。由时刻k-1的1ˆkx和1kP来计算Sigma点集1,kix）（Li...,1,0，通过非线性函数11)·(kkqf传播为1/,kkix，由1/,kkix可得状态向量预测值1/ˆkkx及误差协方差阵1/kkP11,11/,)(kkikkkiqxfxLi,...,1,0101,11/,01/)(ˆkLikikmikkiLimikkqxfWxWx101/1/,1/1/,1/)ˆ)(ˆ(kLiTkkkkikkkkicikkQxxxxWP同理，利用1ˆkx和1kP按照前面的采样策略来计算Sigma点集1,kix）（Li...,1,0，通过非线性量测函数kkrh)·(传播为1/,kki，由1/,kki可得输出预测值1/ˆkkz及自协方差阵kzP~和互协方差阵kkzxP~~kkkikkkirxh)(1/,1/,kkkiLiLikmikkimikkrxhWWz)(ˆ1/,001/,1/kLiTkkkkikkkkicizRzzWPk01/1/,1/1/,~)ˆ)(ˆ(LiTkkkkikkkkicizxzxxWPkk01/1/,1/1/,~~)ˆ)(ˆ(在获得新的量测后kz，进行滤波量测更新TkzkkkkzzxkkkkkkkkKPKPPPPKzzKxxkkkk~1/1~~~1/1/)ˆ(ˆˆ中心差分卡尔曼滤波器（CDKF）Ito等人从数值积分的观点出发提出了一种次优高斯滤波器：中心差分滤波器（CentralDifference