几种卡尔曼滤波算法理论

1、自适应卡尔曼滤波卡尔曼滤波发散的原因如果卡尔曼滤波是稳定的， 随着滤波的推进， 卡尔曼滤波估计的精度应该越 来越高，滤波误差方差阵也应趋于稳定值或有界值。 但在实际应用中， 随着量测 值数目的增加， 由于估计误差的均值和估计误差协方差可能越来越大， 使滤波逐 渐失去准确估计的作用，这种现象称为卡尔曼滤波发散。引起滤波器发散的主要原因有两点：（1）描述系统动力学特性的数学模型和噪声估计模型不准确，不能直接真 实地反映物理过程， 使得模型与获得的量测值不匹配而导致滤波发散。 这种由于 模型建立过于粗糙或失真所引起的发散称为滤波发散。（2）由于卡尔曼滤波是递推过程，随着滤波步数的增加，舍入误差将逐渐

2、 积累。如果计算机字长不够长， 这种积累误差很有可能使估计误差方差阵失去非 负定性甚至失去对称性，使滤波增益矩阵逐渐失去合适的加权作用而导致发散。 这种由于计算舍入误差所引起的发散称为计算发散。针对上述卡尔曼滤波发散的原因，目前已经出现了几种有效抑制滤波发散 的方法，常用的有衰减记忆滤波、 限定记忆滤波、 扩充状态滤波、 有限下界滤波、 平方根滤波、 和自适应滤波等。 这些方法本质上都是以牺牲滤波器的最优性为代 价来抑制滤波发散，也就是说，多数都是次优滤波方法。自适应滤波在很多实际系统中，系统过程噪声方差矩阵 Q 和量测误差方差阵 R 事先是 不知道的，有时甚至连状态转移矩阵 或量测矩阵 H

3、也不能确切建立。如果所 建立的模型与实际模型不符可能回引起滤波发散。 自适应滤波就是这样一种具有 抑制滤波发散作用的滤波方法。 在滤波过程中， 自适应滤波一方面利用量测值修 正预测值，同时也对未知的或不确切的系统模型参数和噪声统计参数进行估计修 正。自适应滤波的方法很多，包括贝叶斯法、极大似然法、相关法与协方差匹配 法，其中最基本也是最重要的是相关法， 而相关法可分为输出相关法和新息相关 法。在这里只讨论系统模型参数已知，而噪声统计参数 Q 和 R 未知情况下的自 适应滤波。由于Q和R等参数最终是通过增益矩阵 K影响滤波值的，因此进行 自适应滤波时，也可以不去估计 Q 和 R 等参数而直接根据

4、量测数据调整 K 就可 以了。输出相关法自适应滤波的基本途径就是根据量测数据估计出输出函数序列Ck，再由C k推算出最佳增益矩阵K,使得增益矩阵K不断地与实际量测数据Ck相适应。Sage-Husa自适应卡尔曼滤波是在利用量测数据进行递推滤波时，通过时变 噪声估计估值器， 实时估计和修正系统噪声和量测噪声的统计特性， 从而达到降 低系统模型误差、抑制滤波发散提高哦滤波精度的目的。xkk,k 1 xk 1wkzkHkxk vkE(wk ) qk , E(wkwkT) Qk kj E(vk) rk,E(vkvkT ) Rk kjE(wk vTj )0Sage-Husa自适应卡尔曼滤波算法可描述为x?

5、kx?k,k 1 K kzkx?k,k1k,k 1x?k 1q?k 1zkzkH kx?k,k 1r?kq?k 1 (1 dk)q?k dk(xk 1k1,kx?k)KkPk,k1HkTHkPk,k1HkT R?k 1Pk,kk ,k 1Pk 1 k,k 1Q?k 1Pk(IKkHk)Pk,k 1.其中，/、 I 5r?k 1(1r?、Rk、（?k和Qk由以下时变噪声统计估值器获得:dk)r?k dk(zk 1 Hk 1x?k 1,k)R?k 1 (1 dk )R?k dk (zk 1zk 1 H k 1Pk 1,kHk 1).3k 1(1 dk)(?kdk(Kk 1k 1【1K k 1Fk

6、1 k 1,k Fkk 1,k)式中：dk 丄，0 b 1为遗忘因子。1 b如果系统状态变量的维数比较高，而Sage-Husa自适应滤波算法中又增加了对系统噪声统计特性的计算，计算量将大大增加，实时性也将难以得到保证。除此之外，对于阶次较高的系统，Sage-Husa自适应滤波算法中Rk和Qk的在线估 计有时会由于计算发散失去半正定性和正定性而出现滤波发散现象，此时 Sage-Husa自适应滤波算法的稳定性和收敛性不能完全保证。基于极大似然准则的自适应卡尔曼滤波，通过系统状态方差阵和量测噪声方 差阵实时估计系统噪声统计特性的变化，以保证滤波器更好地适应这种变化。极 大似然估计从系统量测量出现概率

7、最大的角度估计， 其特点是不仅考虑新息的变化，而且考虑新息协方差矩阵Cvk的变化。它的量测噪声协方差矩阵F?和系统噪声协方差矩阵(3为:RkCvkHkPk,kiHT3kkXik N 1xT耳k,k lR 1 k,k 1xkxkCvk式中:VkZk Zk,k 1，N为平滑窗口的宽度。kT ViViN 1扩展卡尔曼滤波最初提出的卡尔曼滤波基本理论只适用于状态方程和量测方程均为线性的 随机线性高斯系统。但是大部分系统是非线性的，其中还有许多事强非线性的。 非线性估计的核心就在于近似，给出非线性估计方法的不同就在于其近似处理的 思想和实现手段不同。近似的本质就是对难以计算的非线性模型施加某种数学变换，

8、变换成线性模型，然后用Bayes估计原理进行估计。进一步说，非线性变换到线 性变换主要有两种实现手段，一种是Taylor多项式展开，一种是插值多项式展开。BUcy和等人致力于研究将经典卡尔曼滤波理论扩展到非线性随机系统滤波估计中，提出了离散非线性随机系统扩展卡尔曼滤波(Exte nded kalman filter,以下简称EKF。EKF是传统非线性估计中的代表，其基本思想是将非线性状态函数和 量测函数进行局部线性化，即进行一阶Taylor多项式展开，然后应用线性系统Kalman滤波公式。非线性离散系统状态方程和观测方程的一般形式如下所示Xk 1f (Xk,Uk)zk g(Xk,Uk)kWk式

9、中：Uk Rr为输入向量;UkWkVk1-1rp和Vk Rq均为高斯白噪声，且互不相关,其统计特性为：Qk kjE(Wk) 0,Cov(Wk,Wj) 其中，E(Vk) 0,Cov(Vk,Vj) Rk kjCov(wk ,vj)0式中，Q为过程激励噪声协方差矩阵，Rk为观测噪声协方差矩阵。f(Xk 1,Uk 1)是一个非线性状态转换函数，g(Xk 1,Uk 1)是一个非线性量测函数。每一个时刻点，根据一阶泰勒展开将f(Xki，Uki),g(Xki,Uki)线性化，即将非线性状态函数f(;)和非线性量测函数g(;)围绕滤波值展开泰勒级数，并略去二阶以上项,得到f(Xk,Uk)f (Xk,Uk)f(

10、Xk,Uk)XkxkXk ( Xk Xk )1-2g(Xk,Uk)g(Xk,Uk)g(Xk,Uk)XkXk& (Xk Xk)1-3定义?kf (Xk,Uk)XkXkXk，Vkg(Xk, Uk)XkXk Xk，根据式(1-1)、式(1-2)和式(1-3)可以得到非线性系统线性化后只与状态变量有关的表达式，如下Xk 1Zk?kXk f (?k,Uk)?kXkH?kXk g(Xk,Uk) H?kXkkWkVk1-4式1-4中,注意到f（Xk,Uk） ?kXk并非Xk的函数，g（Xk,Uk）RkXk并非Xk的函数，根据1-4近似结果,应用上节的Kalman滤波器计算可以得到EKF迭代算法:定义？k f

11、 (Xk,Uk)XkXk Xk4 g(Xk,Uk) ，Hk XkXk Xk，可得滤波方程初始条件X0E( X0 ), P0var( X0 )状态先验估计值Xk,k 1 f ( ?k 1，Uk 1 )误差协方差先验估计值Rk 1 k,k R 1Tk,k 1k,k 1QkT1 k,k 1增益矩阵KkPk,k 1HTHkFk,k1HTRk 1状态后验估计值Xk ?k,k 1KkZkg(Xk1,Uk)误差协方差后验估计值Fk(IKkHk)Pk,k 1无迹卡尔曼滤波（UKF)EKF是一种次优非线性高斯滤波器，它采用对非线性函数进行线性化 近似的方法，来计算状态分布经非线性函数传递之后的特性。尽管EKF得

12、到了广 泛的应用，但它依然存在自身无法克服的理论局限性：要求非线性系统状态函数和量测函数必须是连续可微的， 这限制了 EKF的应用范围；对非线性函数的 一阶线性化近似精度偏低，特别地，当系统具有强非线性时，EKF估计精度严重下降，甚至发散；需要计算非线性函数的雅克比矩阵，容易造成EKF数值稳定性差和出现计算发散。为了克服上述EKF的缺陷，能够以较高的精度和较快的计算速度处理 非线性高斯系统的滤波问题，Julier等人根据确定性采样的基本思路，基于Unscented变换（UT）提出了 Unscented卡尔曼滤波（UKF）。与EKF类似，UKF仍继承了卡尔曼滤波器的基本结构，不同之处在于 UKF

13、用Un see nted变换取代了 EKF中的局部线性化。UKF仍假设随机系统的状态必须服从高斯分布，但取消了对系统模型的限制条件， 也就是说，不要求系统是 近似线性的，同时，UKF不需要计算雅克比矩阵，因此不要求状态函数和量测函 数必须是连续可微的，它甚至可以应用于不连续系统。 可以证明：不论系统非线 性程度如何，UT变换理论上至少能以三阶泰勒精度逼近任何非线性高斯系统状 态的后验均值和协方差，因此 UKF的理论估计精度优于EKFUKF法首先要构造Sigma散点集,设状态向量为n维,Xk i为时刻k-1的状态向量估计值,R 1为该时刻状态向量的协方差矩阵,2n+1维的Sigma点集可以 表示

14、为:XOk 1Xi,k 1Xi n,k 1Xk 1 (J(n)P,k 1)i ，i=1,2，.，nXk 1 (押)R,k1)i对应于i的一阶二阶权系数为Wm/(n1/2(nW：/(n1/2(n其中,2(n)参数决定第i个Sigma点在状态均值Xk 1周围的扩展空间,是取值区间为,1的常数；为冗余量；为与状态向量的先验分布相关的参数，对高斯 分布,=2为最优。由时刻k-1的Xk 1和Pk 1来计算Sigma点集和1( i 0,1丄)，通过非线性函数fk 1() qk 1传播为Xi,k/k 1，由Xi,k/k 1可得状态向量预测值Xk/k 1及误差协方差阵R/k 1Xi,k/k 1fk 1 (xi

15、,k 1 ) qk 1i 0,1,，LXk/k 1LWi Xi ,k/k 1LWmfk1(Xi,k1)qk 1Pk/ k 1iL eTWi (xi,k /k 1x?k/k 1 )(xi,k/k 1x?k/k 1)Qk 10同理，利 用 x?k 1 和 Pk 1 按 照 前 面 的 采 样 策 略 来 计 算 Sigma 点 集xi,k 1 ( i 0,1., L)，通过非线性量测函数hk()厲传播为i,k/k 1，由i,k/k 1可得输出预测值 z?k/k 1及自协方差阵 Pzk 和互协方差阵 Pxk zki,k /k 1hk (xi,k/k 1)rkz?k/k 1LWimi0i ,k/k 1

16、LWimhk(xi,k/k 1) rk i0PzkWie(i,k/k 10z?k/k 1)( i,k/k 1z?k/k 1)RkPxkzkLWi (xi,k/k 1i0x?k/k 1)( i,k/k 1z?k/k 1)在获得新的量测后 zk ，进行滤波量测更新x?kKkx?k/k 1Kk(zkz?k/k 1)P P 1 xkzk zkPkPk/k 1K kPzk KkT中心差分卡尔曼滤波器CDKF)Ito 等人从数值积分的观点出发提出了一种次优高斯滤波器： 中心差分 滤波器(Central Differenee Filter,CDF。CDF使用多项式插值方法来计算多维积分， 其计算简单，易于实现。几乎同时， 等人也使用 stirling 多项式插值公式来近似计算非线性函数 的多维积分，得到了分开差分滤波器(Divided Differenee Filter, DDF)。武元新等 人通过理论分析指出，DDF和CDF都是基于函数拟合的思想来实现的， 即都是使 用一个函数序列近似被积函数， 且函数序列中的每个函数积分都有解析解， 此时 近似函数的积分就可以看作是对积分的近似。由于 DDF和CDF在本质上是一致 的，有异曲同工之妙，因此等人统一将它们称为中心差分卡尔曼滤波器( CentralDi