几种卡尔曼滤波算法理论

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

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

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

4、本途径就是根据量测数据估计出输出函数序列Ck,再由Ck推算出最佳增益矩阵 K,使得增益矩阵 K 不断地与实际量测数据CJ相适应。.Sage-Husa 自适应卡尔曼滤波是在利用量测数据进行递推滤波时，通过时变噪声估计估值器，实时估计和修正系统噪声和量测噪声的统计特性，从而达到降低系统模型误差、抑制滤波发散提高哦滤波精度的目的。Xk=k,jXjWkZk=HkXkVkTE(Wk)=qk，E(WkWk)=Q*kjE(Vk)”E(VkVT)=Rk、*jE(wkVT)=0Sage-Husa 自适应卡尔曼滤波算法可描述为?=?k,k1KkZkXk,k=.k,k尺，很k=Zk-Hk?k,k4-?Kk=Pk,k

5、#THkPk“HT昆广R,k=：k,k4Pk,k,kQk4R=(I-KkHJPk”.其中，r?、是、？卜和由以下时变噪声统计估值器获得：傕1=(1-dk)?dk(Zk1-2Xkik)Rk由=(1dk)R+dk/ZHk+PkkHM)bki=(1-dk)&dk(Xkiki,k尺)1-b式中：dk=k4，0bk核心就在于近似，给出非线性估计方法的不同就在于其近似处理的思想和实现手段不同。近似的本质就是对难以计算的非线性模型施加某种数学变换，变换成线性模型，然后用 Bayes 估计原理进行估计。进一步说，非线性变换到线性变换主要有两种实现手段,一种是 Taylor 多项式展开,一种是插值多项式

6、展开。Bucy 和 Y.Sunahara 等人致力于研究将经典卡尔曼滤波理论扩展到非线性随机系统滤波估计中，提出了离散非线性随机系统扩展卡尔曼滤波(Extendedkalmanfilter,以下简称 EKF)。EKF 是传统非线性估计中的代表，其基本思想是将非线性状态函数和量测函数进行局部线性化，即进行一阶 Taylor 多项式展开，然后应用线性系统 Kalman 滤波公式。非线性离散系统状态方程和观测方程的一般形式如下所示Xk1=f(Xk,Uk)-kWk/、1-1Zk=g(Xk,Uk)Vk式中：UkWRr为输入向量；WkwRp和 VkwRq均为高斯白噪声，且互不相关，其统计特性为：,E(Wk

7、)=0,Cov(Wk,Wj)=Qgkj其中，,E(vJ=0,Cov(Vk,Vj)=RECov(wk,Vj)=0式中，Qk为过程激励噪声协方差矩阵，R 为观测噪声协方差矩阵。f(XkUk)是一个非线性状态转换函数，g(Xy,Uk)是一个非线性量测函数。每一个时刻点，根据一阶泰勒展开将 f(Xy,Uk)，g(Xk,Uk)线性化，即将非线性状态函数f(;)和非线性量测函数g(;)围绕滤波值展开泰勒级数，并略去二阶以上项，得到f(Xk,Uk)定f(Xk,Uk)名(Xk-Xk)1-2CXk八，勾(xk,uk)-/cg(Xk,Uk)为g(XLUk)+xkJ?k(XkXk)1-3%定义.=叼叼华,品=四2|

8、4 国，根据式(1-1)、式(1-2)OXkCXk和式(1-3)可以得到非线性系统线性化后只与状态变量有关的表达式，如下Xk书*&kXk+f(Xk,Uk)dkXk+kWk.)1-4jk之Hxk+g(K,Uk)Rxk+vk式 1-4 中，注意到 f（浦山）一本kXk并非 Xk的函数，g（兄,u/用人并非 Xk的函数,根据 1-4 近似结果,应用上节的 Kalman 滤波器计算可以得到 EKF 迭代算法:定义格k=旦 8_!0,用=期人必）期人必）xk总，可得 cXkcXk滤波方程初始条件 X0=E（X0）,P0=var（X0）状态先验估计值 Xk,k=f（Xk,uk）误差协方差先验估计值

9、Pk,k1=k,kPkk,kk,kJQkJ-k,kJ增益矩阵 Kk=Pk,k,HTHkPk,kHTRk状态后验估计值 XkuQ,kKkZk-g（?k,Uk）误差协方差后验估计值 R=（I-KkHk）Pk,k无迹卡尔曼滤波（UKFEKF 是一种次优非线性高斯滤波器，它采用对非线性函数进行线性化近似的方法，来计算状态分布经非线性函数传递之后的特性。尽管 EKF 得到了广泛的应用，但它依然存在自身无法克服的理论局限性： 要求非线性系统状态函数和量测函数必须是连续可微的，这限制了 EKF 的应用范围；对非线性函数的一阶线性化近似精度偏低，特别地，当系统具有强非线性时，EKF 估计精度严重下降，甚至发散

10、；需要计算非线性函数的雅克比矩阵，容易造成 EKF 数值稳定性差和出现计算发散。为了克服上述 EKF 的缺陷，能够以较高的精度和较快的计算速度处理非线性高斯系统的滤波问题，Julier 等人根据确定性采样的基本思路，基于 Unscented 变换（UT）提出了 Unscented 卡尔曼滤波（UKF。与 EKF 类似，UKF 仍继承了卡尔曼滤波器的基本结构，不同之处在于 UKF用 Unscented 变换取代了 EKF 中的局部线性化。UKF 仍假设随机系统的状态必须服从高斯分布，但取消了对系统模型的限制条件，也就是说，不要求系统是近似线性的，同时，UKF需要计算雅克比矩阵，因此不要求状态函数

11、和量测函数必须是连续可微的，它甚至可以应用于不连续系统。可以证明：不论系统非线性程度如何，UT 变换理论上至少能以三阶泰勒精度逼近任何非线性高斯系统状态的后验均值和协方差，因此 UKF的理论估计精度优于 EKEUK 祛首先要构造 Sigma 散点集,设状态向量为 n 维，念为时亥 Uk-1 的状态向量估计值，Pj 为该时刻状态向量的协方差矩阵,2n+1 维的 Sigma 点集可以表示为：x0,kxkAdx,k=/+(v(n+QP,k)i,i=1,2,.,nxin,k=xk(.(n)p,k)i对应于与的一阶二阶权系数为“仅/(n+笛i=0W、1/2(n+K)i#0c九/(n+入)+1+B口2i=

12、0W=J/2(n+.)i#0其中，九=o(2(n+K)-n参数 a 决定第 i 个 Sigma 点在状态均值蹂,周围的扩展空间，是取值区间为0.0001,1的常数；K为冗余量；P 为与状态向量的先验分布相关的参数，对高斯分布，0=2 为最优。由时亥 ijk-1 的功和 Pk来计算 Sigma 点集由 Xi,k/y可得状态向量预测值其/k及误差协方差阵Pk/k4xi,k/k1=fk4(xi,k4)qkJLLXk/kl=WiXi,k/k4=i0i=0LcTR/ki=Wi(Xi,k/k1一？k/kj)(Xi,k/k.一尺/k.)-Qk4i=0同理，利用 XU 和 PU 按照前面的采样策略来计算 Si

13、gma 点集(i=0,1.,L),通过非线性函数 fy()+qk二传播为Xi,k/k4?i=0,1,.,LWifk4(xi,k4)qk-1Xi,k_!(i=0,1.,L),通过非线性量测函数 hk()十 rk传播为？i,k/k，由,k/k可得输出预测值Z/k及自协方差阵吟和互协方差阵 P 混i,k/kJ.=hk(xi,k/k1)rkLL?k/kJ.=Wii,k/kd=u，Wimhk(xi,k/k1)rki0iz0LP4=Wi(i,k/k一？k/k)(i,k/k一zk/k)Rkiz0LPx之=、Wj(Xi,k/k二-xk/k)(i,k/k-Zk/kJ)i=0在获得新白量测后 Zk,进行滤波量测更

14、新泣=?k/k+Kk(Zk-Zk/k)1Kk=皿叱R=Pk/kKkPzkKT中心差分卡尔曼滤波器(CDKFIto 等人从数值积分的观点出发提出了一种次优高斯滤波器：中心差分滤波器(CentralDifferenceFilter,CDF)。 CDF用多项式插值方法来计算多维积分， 其计算简单，易于实现。几乎同时，M.Norgaard 等人也使用 Stirling 多项式插值公式来近似计算非线性函数的多维积分，得到了分开差分滤波器(DividedDifferenceFilter,DDF。武元新等人通过理论分析指出，DD 林口 CDF是基于函数拟合的思想来实现的，即都是使用一个函数序列近似被积函数，且函数序列中的每个函数积分都有解析解，此时近似函数的积分就可以看作是对积分的近似。由于 DDF 和CDFfc 本质上是一致的，有异曲同工之妙，因此 R.V.Merwe 等人统一将它们称为中心差分卡尔曼滤波器(CentralDifferenceKalmanFilter,CDKF),并给出了 CDKF 勺滤波递推公式。平方根SPKF算法由于 CDK 所采用的多项式