几种卡尔曼滤波算法理论

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

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

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

4、了。 输出相关法自适应滤波的基本途径就是根据量测数据估计出输出函数序列 C k，再由C k推算出最佳增益矩阵K，使得增益矩阵K不断地与实际量测数 据Ck相适应。 . Sage-Husa自适应卡尔曼滤波是在利用量测数据进行递推滤波时，通过时变 噪声估计估值器，实时估计和修正系统噪声和量测噪声的统计特性，从而达到降 低系统模型误差、抑制滤波发散提高哦滤波精度的目的。 Xk =钏心心 Wk Zk = H kXk Vk T E(wQ=qk，E(WkWk)=Qk&kj E(vk) =k，E(vkvT)=巳、灯 Eg；) =0 Sage-Husa白适应卡尔曼滤波算法可描述为 ? - ?k,k 1

5、KkZk Xk,k=k,kXk, qk k =Zk -Hk?k,k-? Kk =Pk,jH：HkPk,k JI： Fk,k 4 - ：J k,k 4Pk i k,k 4 Ck 4 Pk =(I -KkHk)Pk,“ .其中，r?、RK、？k和冬由以下时变噪声统计估值器获得： 们=(1 -dk)? dk(Zk 1 -儿 1X5) Rk+ =(1dk)Rk +dk(k4rHzPz,kHT+) Gk 1 =(1-dk)qk dk(Xk 1 -气 LkXJ) . 1 (1 d k ) R * d k ( K k 4 4 A 1 K k -1 十 Pk1 - ：J k 1,k k 1,k) 式中：dk

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

7、不仅考虑新息的变 化，而且考虑新息协方差矩阵 Cvk的变化。它的量测噪声协方差矩阵F?和系统噪 声协方差矩阵Q为: Rk =艮-HkPk,kH： 。1 .k T . T Qk， X xi Fk - ,Jk,kJPk4Jk,kd N i 虫 _N 1 1 k T Cvk = N - ViVi 式中：Vk=Zk-4,J , N为平滑窗口的宽度。 扩展卡尔曼滤波 最初提出的卡尔曼滤波基本理论只适用丁状态方程和量测方程均为线性的 随机线性高斯系统。但是大部分系统是非线性的，其中还有许多事强非线性的。 非线性估计的核心就在丁近似，给出非线性估计方法的不同就在丁其近似处理的 思想和实现手段不同。近似的本质

8、就是对难以计算的非线性模型施加某种数学变 换，变换成线性模型，然后用Bayes估计原理进行估计。进一步说，非线性变换到线性变换主要有两种实现手段，一种是Taylor多项式展开，一种是插值多项式展 开。 Bucy和Y.Sunahara等人致力丁研究将经典卡尔曼滤波理论扩展到非线性随 机系统滤波估计中，提出了离散非线性随机系统扩展卡尔曼滤波 (Extended kalman filter, 以下简称EKF)。EKF是传统非线性估计中的代表，其基本思想是 将非线性状态函数和量测函数进行局部线性化， 即进行一阶Taylor多项式展开， 然后应用线性系统Kalman滤波公式。 非线性离散系统状态方程和观

9、测方程的一般形式如下所示 Xk 1 = f (Xk,Uk)】kWk / 、 1-1 Zk = g(Xk,Uk) Vk 式中：UkRr为输入向量；WkRp和VkRq均为高斯白噪声，且互不相关， 其统计特性为： ”E(Wk) =0,Cov(Wk,Wj)=。必 其中，E(Vk) =0,Cov(Vk,Vj) = Rkj Cov(wkM) =0 式中，Qk为过程激励噪声协方差矩阵，Rk为观测噪声协方差矩阵。 f(Xk,Uj)是一个非线性状态转换函数，g(Xj,Uj)是一个非线性量测函数。每 一个时刻点，根据一阶泰勒展开将f(Xk,Uk)，g(xj,Uk)线性化，即将非线性状 态函数f(，-)和非线性量测

10、函数g(，-)围绕滤波值展开泰勒级数，并略去二阶以上 项，得到 f (Xk,Uk)出 f(Xk,Uk) +* *；, ,* *) )Xk w(Xk - Xk) 1-2 CXk c , Eg(Xk,Uk) - g(Xk,Uk) &g(Xk,Uk) + - - Xk J?k (Xk Xk) 依k 和式(1-3)可以得到非线性系统线性化后只与状态变量有关的表达式，如下 Xk 1 ： ?kXk , f (Xk,Uk) -：?kXk ,】kWk .Zk 网 H?kXk +g(Xk,Uk)H?kXk +Vk 式1-4中，注意到f （兑,斗）-事由并非Xk的函数，g（又,uQ-并非Xk的 函数，1-

11、3 定义？k =f(Xk，Uk) k -Xk ：g(Xk,Uk) Xk Xk 彳，根据式(1-1 )、式(1-2) 1-4 根据1-4近似结果,应用上节的Kalman滤波器计算可以得到EKF迭代算法: 定义虱=哭，纠X , Hk =沏;X，U Uk）、,可得 滤波方程初始条件 X0 = E（xo）, R =var（xo） 状态先验估计值 Xk,k=f （Xk,uk） 误差协方差先验估计值 R,k=k,kPkk,k-k,k JQk J - k,k J 增益矩阵 Kk =Pk,kH：HkPk,kJHk Rk 状态后验估计值Xk =又,kn KkZk -g（?k,Uk） 误差协方差后验估计值 R =

12、 （I -KkHk）Pk,k = 无迹卡尔曼滤波（UKF EKF 是一种次优非线性高斯滤波器,它采用对非线性函数进行线性化 近似的方法，来计算状态分布经非线性函数传递之后的特性。尽管EKF得到了广 泛的应用，但它依然存在自身无法克服的理论局限性 ：要求非线性系统状态函 数和量测函数必须是连续可微的， 这限制了 EKF的应用范围；对非线性函数的 一阶线性化近似精度偏低，特别地，当系统具有强非线性时， EKF估计精度严重 下降，甚至发散；需要计算非线性函数的雅克比矩阵， 容易造成EKF数值稳定 性差和出现计算发散。 为了克服上述EKF的缺陷，能够以较高的精度和较快的计算速度处理 非线性高斯系统的滤

13、波问题， Julier等人根据确定性采样的基本思路，基丁 Unscented变换（UD 提出了 Unscented卡尔曼滤波（UKF。 与EKF类似，UKF仍继承了卡尔曼滤波器的基本结构，不同之处在丁 UKF用Unscented变换取代了 EKF中的局部线性化。UKF仍假设随机系统的状态 必须服从高斯分布，但取消了对系统模型的限制条件， 也就是说，不要求系统是 近似线性的，同时，UKE需要计算雅克比矩阵，因此不要求状态函数和量测函 数必须是连续可微的，它甚至可以应用丁不连续系统。 可以证明：不论系统非线 性程度如何，UT变换理论上至少能以三阶泰勒精度逼近任何非线性高斯系统状 态的后验均值和协方

14、差，因此 UKF的理论估计精度优丁 EKE UKIfe首先要构造Sigma散点集，设状态向量为n维，XkJL为时刻k-1的 状态向量估计值，Pk为该时刻状态向量的协方差矩阵,2n+1维的Sigma点集可 以表小为： Xk=Xk+(v(n+冏P,k)i , i=1,2，n X n,k=应 1 ( ., (n )p,k)i 对应丁乌的一阶二阶权系数为 ?/(n+ i=0 W = _ 、1/2(n + 3.) i。0 c 君/(n+兀)+1 + Ga2 i=0 W = 、1/2(nf i#0 其中， 九=o(2(n + Q -n 参数a决定第i个Sigma点在状态均值 筋工周围的扩展空间，是取值区间

15、为 0.0001, 1的常数；K为冗余量；&为与状态向量的先验分布相关的参 数，对高斯分布，臼=2为最优。 由时刻k-1的X. 和Pk 来计算Sigma点集x,k (i = 0,1., L),通过非线性函 由X,k/k可得状态向量预测值 又/“及误差协方差阵 R/k 4 L L Xk/kl = Wi Xi,k/k4 = i W i =0 L c T P/k4 - Wi (Xi ,k/k】一?k/kl)(Xi,k/k 品一 Xk/k 品 )Qk4 i =0 同理，利用XU和R 4按照前面的采样策略来计算Sigma点集 数fj() +qj传播为 Xi,k/k 4， Xi,k/k=fk 4(

16、xi,k 4) qk 0,1,., L Wi fk 4 (Xi,k 4) Qkl Xi心(i=0,1.,L)，通过非线性量测函数hk()+k传播为Ak/k，由、k/k直可得输 出预测值2k/k_i及自协方差阵PZk和互协方差阵P稠 i,k/k 1 = hk (xi,k/k Q rk L L ?k/k Wi i ,k/k d =，Wmhk(xi,k/k), rk i z0 i z0 L PX = W ( i,k/k-？k/k)(i,k/k J k/k J) R i z0 L Px =、W(Xi,k/k 一 &k A )(i ,k/k- Zk /k J ) i =0 在获得新的量测后Zk

17、,进行滤波量测更新 Xk =?k/k*Kk(Zk Zk/k) 1 Kk =喝 R =Pk/k-KkPzK 中心差分卡尔曼滤波器 (CDKF Ito 等人从数值积分的观点出发提出了一种次优高斯滤波器：中心差 分滤波器(Central Difference Filter,CDF )。CDF用多项式插值方法来计算 多维积分，其计算简单，易丁实现。 几乎同时，M.Norgaard等人也使用Stirling 多项式插值公式来近似 计算非线性函数的多维积分，得到了分开差分滤波器( Divided Difference Filter , DDF。武元新等人通过理论分析指出，DD拜日CDF是基丁函数拟合的 思想来实现的，即都是使用一个函数序列近似被积函数， 且函数序列中的每个函 数积分都有解析解，此时近似函数的积分就可以看作是对积分的近似。由丁 DDF 和CDFft本质上是一致的，有异曲同工之妙，因此R.V.Merwe等人统一将它们称 为中心差分卡尔曼滤波器(Central D