带有指数渐消因子的限定记忆卡尔曼滤波方法#

1、中国科技论文在线带有指数渐消因子的限定记忆卡尔曼滤波方法基金项目：中国矿业大学青年科技基金(2592009043)；高等学校博士点基金资助项目(20100095120006)；江苏省自然科学基金项目(BK2011226) 郭萌，刘万里作者简介：郭萌（1987-），男，硕士生。研究领域为多传感器数据融合、机器人定位通信联系人：刘万里（1979-），男，博士，副教授。研究领域为激光精密测量技术和机器人技术. E-mail: liuwanli2181.51.51.51.51.51.51.51.51.51.51.51.51.51.51.51.51.51.51.51.51.51.51.51.51.51.

2、51.5School of Mechanical and Electrical Engineering，China University of Mining and Technology，Xuzhou 221116;School of Mechanical and Electrical Engineering，China University of Mining and Technology，Xuzhou 221116中国矿业大学机电学院，江苏 徐州 221116;中国矿业大学机电学院，江苏 徐州 221116221116;22111613407538609;15190677196151906

3、77196苏省徐州市泉山区中国矿业大学南湖校区杏3B5112 ;江苏省徐州市泉山区中国矿业大学南湖校区机电学院A413guomengmaster;liuwanli218郭萌（1987-），男，硕士生。研究领域为多传感器数据融合、机器人定位。;刘万里（1979-），男，博士，副教授。研究领域为激光精密测量技术和机器人技术。郭萌;刘万里GUO Meng;LIU Wanli刘万里中国矿业大学青年科技基金(2592009043)；高等学校博士点基金资助项目(20100095120006)；江苏省自然科学基金项目(BK2011226)1.51.51.51.51.51.51.51

4、.51.51.51.51.51*|*专著*|*秦永元，张洪钺，汪叔华.卡尔曼滤波与组合导航原理M.西安:西北工业大学出版社，2010.<CR>2*|*专著*|*付梦印，邓志红，闫莉萍.Kalman滤波理论及其在导航系统中的应用M.北京科学出版社，2010.<CR>3*|*学位论文*|*朱萍. 软件GPS接收机定位算法研究D.上海： 上海交通大学, 2009.<CR>4*|*期刊*|*韩华，刘仁溥. 带有状态噪声的限定记忆卡尔曼滤波方法J.火力与指挥控制，2001，26(2): 55-58.<CR>5*|*期刊*|*孙章国，钱峰. 一种基于指数渐消

5、因子的自适应卡尔曼滤波算法J. 电子测量技术，2010，33(1): 40-42.<CR>6*|*期刊*|*安鸿志，严加安. 限定记忆滤波方法J. 数学认识与实践，1973(3): 28-38.<CR>7*|*期刊*|*Levent Ozbek, Aliev Fazil A. Coment on adaptive fading kalman filter with an applicationJ. Aautomatica, 1998, 34 (12) : 1663-1664.<CR>8*|*译著*|*库索夫可夫 HT. 控制系统的最优滤波和辨识方法M. 章燕

6、申. 北京: 国防工业出版社，1984.<CR>9*|*期刊*|*徐景硕，秦永元，彭蓉. 自适应卡尔曼滤波器渐消因子选取方法研究J.系统工程与电子技术，2004，26(11): 1552-1554.<CR>10*|*期刊*|*牛华伟，张厚超. 关于矩阵迹的性质与应用J. 宁波职业技术学院学报, 2009，13(2): 41-43.<CR>11*|*期刊*|*Xia Qijun, Rao Ming, Ying Yiqun, et al. Adaptive fading kalman filter with an applicationJ . Automatic

7、a, 1994, 30 (12): 1333-1338.<CR>12*|*其他文献*|*Jouni Hartikainen, Simo Särkkä. Optimal filtering with Kalman filters and smoothers-a Manual for Matlab toolbox EKF/UKFZ. February 25, 2008.|1|郭萌|GUO Meng|中国矿业大学机电学院，江苏 徐州 221116|School of Mechanical and Electrical Engineering，China Univers

8、ity of Mining and Technology，Xuzhou 221116|郭萌（1987-），男，硕士生。研究领域为多传感器数据融合、机器人定位。|江苏省徐州市泉山区中国矿业大学南湖校区杏3B5112 |221116|guomengmaster13407538609<CR>*|2|刘万里|LIU Wanli|中国矿业大学机电学院，江苏 徐州 221116|School of Mechanical and Electrical Engineering，China University of Mining and Technology，Xuzhou

9、 221116|刘万里（1979-），男，博士，副教授。研究领域为激光精密测量技术和机器人技术。|江苏省徐州市泉山区中国矿业大学南湖校区机电学院A413|221116|liuwanli21815190677196带有指数渐消因子的限定记忆卡尔曼滤波方法|Limited Memory Kalman Filtering Algorithm with Exponential Fading Factor|中国矿业大学青年科技基金(2592009043)；高等学校博士点基金资助项目(20100095120006)；江苏省自然科学基金项目(BK2011226)- 13 -（中国矿

10、业大学机电学院，江苏 徐州 221116）摘要：针对传统限定记忆卡尔曼滤波在记忆长度结点处精度下降甚至发散而传统衰减记忆卡尔曼滤波无法根除陈旧量测值对估计值的影响的问题，提出了一种带有指数渐消因子的限定记忆卡尔曼滤波方法。首先根据经典卡尔曼滤波的数学模型，在传统的限定记忆卡尔曼滤波当中加入过程噪声；接着在此基础之上，引入指数渐消因子，推导出了带有限定记忆和衰减记忆功能的卡尔曼滤波递归方程。最后使用Matlab对该方法进行性能分析。该次优滤波方法克服了上述两种滤波方法的缺点并能有效抑制因系统方程不准确造成的滤波发散。仿真结果同样表明：在现实的过程噪声和参数设定下，该滤波方法依然收敛，跟踪运载体位

11、置的精度可以达到0.5m以内，表明了在系统方程不准确的条件下，带有指数渐消因子的限定记忆卡尔曼滤波是一种较高精度且易于实现的估计和预测方法。关键词：信号处理；限定记忆滤波；衰减记忆滤波；指数渐消因子中图分类号：TP273Limited Memory Kalman Filtering Algorithm with Exponential Fading FactorGUO Meng, LIU Wanli(School of Mechanical and Electrical Engineering，China University of Mining and Technology，Xuzhou 2

12、21116)Abstract: To solve the problems that conventional limited memory Kalman filtering declines in accuracy or even diverges at its memory length joint and conventional attenuated memory Kalman filtering can not eliminate the influence brought by obsolete measurements, a new limited memory filterin

13、g algorithm with exponential fading factor is brought forward. In the first place, process noise is added into conventional limited memory Kalman filtering on basis of the mathematical model used in classic Kalman filter. Then, an exponential fading factor is introduced to derive limited memory Kalm

14、an filtering recursive equations which function as limited memory and at the same time attenuated memory Kalman filtering. Finally, performance analysis on Matlab is done. This suboptimal method overcomes the demerits of the former two filtering methods and can effectively restrain Kalman filtering

15、from the divergence due to inaccuracy of systematic equations and noise model. Simulation results equally demonstrate that under realistic process noise and parameter settings, the proposed filtering algorithm still converges, and improves positioning accuracy to within 0.5 meter in tracking targets

16、, proving that limited memory Kalman filtering with exponential fading factor is an autonomous estimation and prediction method with relatively higher precision and easier implementation.Key words: signal processing; limited memory filtering; attenuated memory filtering; exponential fading factor0 引

17、言经典卡尔曼滤波在实际应用中，常常会发生量测次数不断增大、状态估计的均方误差趋于稳定，但是状态估计值相对于真实值的偏差却趋于增大的现象，即滤波器的发散现象12。造成滤波器发散的主要原因有两点：（1）系统数学模型和噪声模型过于粗糙或失真，无法反映系统真实的物理过程，由此引起的滤波器发散称为滤波发散3。（2）卡尔曼滤波的递推特性使滤波计算中的舍入误差(Roundoff Errors)逐渐累积，导致一步预测均方误差和估计均方误差矩阵逐渐失去非负定性，尤其当计算机字长不足时将导致增益矩阵失真、丧失加权作用而使卡尔曼滤波器发散，这种发散称之为计算发散。后者可以采用平方根滤波加以克服。对于前者，可以通过加

18、大当前量测值的权重，而相应降低较早量测值的权重实现对于滤波发散的抑制，常用的方法有衰减记忆法和限定记忆法。衰减记忆法的缺点是只能削减但无法根除陈旧量测值对估计值的修正作用；而限定记忆法的缺点是时刻数在不大于记忆长度数时只能采用经典卡尔曼滤波方法，在第时刻起需要重新设定初值以转入限定记忆滤波计算，因此在前个时刻里会累积较大误差，且记忆长度数越大，记忆长度结点之前时刻的误差越大。文献1是在无过程噪声的假定下推导的；文献4中的数学模型虽然带有过程噪声，但不具有衰减记忆功能，无法抑制到达记忆长度结点之前估计值较大的误差。带有指数渐消因子的限定记忆卡尔曼滤波方法就是在此基础上推导出的卡尔曼滤波改进方法。

19、1 带有指数渐消因子的限定记忆卡尔曼滤波方程的推导由于时刻的状态估计值，即根据个量测值对作线性最小方差估计，所以经典卡尔曼滤波无限保存有之前的量测数据信息，基于里程计的航迹推算采用经典卡尔曼滤波时累积误差也会一直保留。而带有指数渐消因子的限定记忆滤波器仅保留距时刻最近的个时刻的量测值，摈弃了时刻之前的量测值，同时引入指数渐消因子进行运算，进一步提高靠近当前时刻的量测值的权重，从而将衰减记忆滤波和限定记忆滤波相结合，减小了累积误差。推导思路如下：对及其过去的离散值进行了次量测，量测值为。假设至共有个测量值45，记， (1) (2) (3) (4)同时建立起和之间的线性关系式以及和之间的线性关系式

20、，再根据这两式确定出与之间的线性关系式12，同时对一步预测均方误差方程中的估计均方误差引入标量指数渐消因子，最后获得的带有指数渐消因子的限定记忆滤波方程。假设系统方程和量测方程为 (5) (6)式中：为时刻至时刻的一步转移矩阵；为系统噪声驱动阵；为系统激励噪声序列；为量测阵；为量测噪声序列。同时，和满足式中， 为函数；为系统噪声序列的方差阵，假设为非负定阵；为零均值的量测白噪声序列的方差阵，假设为正定阵。1)与间的线性关系。根据线性最小方差估计的线性性质，由式(5)得 (7)即 (8)由于比多了在时刻的量测值，所以根据卡尔曼滤波基本方程 (9)式中表示根据对所做的修正。即 (10)式中 (11

21、) (12) (13)2)与间的线性关系。由于比多了一个在时刻的量测值，所以根据卡尔曼滤波基本方程 (14)式中表示根据对所做的修正。而 (15)又因为 (16)即 (17)所以 (18)则时刻量测噪声的方差阵修正为 (19)将视为量测阵，视为的量测值，所以式(14)可写成 (20)式中 (21) (22) (23)3)的带有指数渐消因子的限定记忆滤波方程。式(10)与式(20)等式两边对应相减得 (24)由于 (25)则式(24)可写成 (26)将式(26)右边最后一项移至左边，两边同时左乘，得 (27)记 (28) (29)由式(11)，可得 (30)也可以写成 (31)由式(13)与式(

22、23)，可得 (32)用右乘两边并应用式(30)和式(31)，可得 (33)所以 (34)则由式(28)、式(31)以及式(34)，可得限定记忆滤波的增益矩阵 (35)由式(29)并应用式(21)得， (36)对上式求逆，得增益矩阵 (37)比较式(13)和式(23)，得限定记忆滤波的均方误差阵 (38)将式(8)代入式(27)，则限定记忆滤波的状态方程为： (39)式(35)、(37)、(38)和(39)为传统限定记忆卡尔曼滤波的递推方程，用代替，整理后的式(40)至式(43)就是带有指数渐消因子的限定记忆卡尔曼滤波递推方程： (40) (41) (42) (43)式(39)(43)中为状态

23、估计；为一步预测均方误差；和为滤波增益矩阵；为估计均方误差；称为衰减记忆滤波的渐消因子，定义为指数函数；为限定记忆长度；星号*代表加入了指数渐消因子。当量测时刻小于等于记忆长度时，不能进行带有指数渐消因子的限定记忆卡尔曼滤波，而只能进行传统的衰减记忆卡尔曼滤波，由初值和计算到和。但由起，若直接取，作为初值进行限定记忆滤波，则随后的滤波值一直受和的影响，不符合限定记忆滤波要求6。为了消除初值对于限定记忆滤波的影响，可按下式选取初值7: (44) (45)系统噪声序列的方差阵和量测噪声序列的方差阵对滤波的影响是通过增益矩阵间接影响估计值的，所以在系统方程不准确或未知的条件下可以不用估计和，直接对滤

24、波增益和进行估计，此时对滤波有重要影响。2 指数渐消因子的选取方法由上节可知，要抑制滤波器发散，关键在于选取合适的。卡尔曼滤波器用一步预测代替真实状态，这种方法引起的对量测的估计误差称为残差（也叫新息）： (46)若系统方程建模正确，并且系统噪声和量测噪声均为高斯白噪声序列，那么残差序列也应该为高斯白噪声序列。残差的协方差矩阵为 (47)根据控制系统最优滤波的相关文献8910，滤波器发散的定义是实际估计误差超过理论预计值的过程，因此下式可以作为滤波是否发散的判据： (48)不等式中：左端的是通过实际量测得到的残差序列的平方和；右端的为大于1 的贮备系数，根据工程实践经验值选取； tr为矩阵迹的

25、代表符号trace。由式（47）、式（48），用矩阵的理论值代替，即 (49)若根据式（49）采用最严格的收敛判据形式 (50)则由矩阵迹的线性性质11，可以解出： (51)而若考虑工程实践中的贮备系数，则判断滤波发散与否依赖于的选取，如果选取的贮备系数与理论值相比误差较大则极有可能使滤波效果不理想甚至导致滤波发散。当系统模型不准确或噪声统计特性未知的时候，根据参考文献5，可以将定义为指数渐消因子、通过实际量测的残差与理论残差协方差矩阵迹的比值来确定指数的值。假设实际量测的残差与理论残差协方差矩阵迹的比值为： (52) 若贮备系数已知，则： (53)当贮备系数未知或者经验值存在错误时，直接按式

26、(52)求取。定义指数渐消因子为： (54)图1给出了带有指数渐消因子的限定记忆卡尔曼滤波的程序图，当等于1时，得出的滤波结果是最理想的；当大于或者小于1时，通过以为底数的指数函数确定渐消因子，从而间接影响增益矩阵和，改善系统建模不准确和噪声特性未知对滤波的影响，增加了系统的灵活性和鲁棒性。图1 带有指数渐消因子的限定记忆卡尔曼滤波程序流程图Fig. 1 Program flow chart of limited memory kalman filtering with exponential fading factor3 Matlab仿真与性能分析本文的仿真实验使用的是美国MathWorks

27、公司出品的商业数学软件matlab 6.5，以下仿真数据都是在英特尔酷睿i7-2630QM 2.0Ghz处理器、4G内存的笔记本主机上获得。仿真1：采用-滤波12方式，设运载体沿某一直线运动，运载体初始位置坐标为s0=5m，初始速度为v0=1m/s，初始加速度为a0=0.1m/s2，初始状态为，每隔1s对位移作一次测量作为卡尔曼滤波器的观测值，设卡尔曼滤波器误以为运载体无初始速度（即认为v0=0m/s），以a0=0.1m/s2的初始加速度在运动。故被估计信息的方程为其中加加速度jk对于观测者来讲是随机量，用白噪声描述。取，状态方程为方程中，一步转移矩阵为，系统噪声驱动阵为。由于，故量测矩阵。图

28、2是采用不同参数组合得到的滤波误差曲线。系统方程不准确，采用经典卡尔曼滤波器时，状态噪声和量测噪声都会导致滤波器的发散。为了方便对比限定记忆滤波和带有指数渐消因子的限定记忆滤波，图2-1和图2-2仿真时间相同而设定的记忆长度不同；图2-2和图2-3仿真时间不同而设定的记忆长度相同。图2-1 仿真时间200s，记忆长度N=30时的误差曲线Fig.2-1 Error curves when simulation time is 200s, memory length N=30图2-2 仿真时间200s，记忆长度N=50时的误差曲线Fig.2-2 Error curves when simulati

29、on time is 200s, memory length N=50图2-3 仿真时间600s，记忆长度N=50时的误差曲线Fig.2-3 Error curves when simulation time is 600s, memory length N=50由图2-1至图2-3可以看出，系统方程不准确，导致经典卡尔曼滤波一直呈发散状态，失去了滤波功能；而限定记忆滤波和带有指数渐消因子的限定记忆滤波均抑制了滤波发散。由于限定记忆滤波在量测时刻到达记忆长度之前必须采用经典卡尔曼滤波，此时误差比较大，而带有指数渐消因子的限定记忆滤波加入了基于验后信息的指数渐消因子，具有衰减记忆特性，在全区间总

30、以误差0为中心的-0.5m,0.5m区间内波动，降低了系统模型不准确带来的误差。记忆长度结点N=30、N=50之前的误差也得以削减，但记忆长度较小时，白噪声带来的随机误差没有得到有效抑制。图2-3表明了带有指数渐消因子的限定记忆滤波器的长期稳定性，仿真时间增加到600s时经典卡尔曼滤波器的正误差发散至150m左右，而带有指数渐消因子的限定记忆滤波器仍基本在-0.5m,0.5m区间内，保证了估计值的无偏性。同时由三幅误差曲线图注意到，增大记忆长度可以减小白噪声的影响，但相应地会较多地受到系统方程不准的影响；减小记忆长度可以减弱系统方程不准带来的影响，但会较多地受到白噪声的干扰。应合理权衡保证滤波

31、精度。仿真2：假定一个两坐标雷达对一个平面上运动的目标进行观测，目标在t=0400s沿y轴作匀速直线运动，运动速度为15m/s，目标的起始坐标为(2000,10000)。t=400s时目标高速机动，x轴方向具有初始速度10m/s，加速度为24.975m/s2。在t=430s时，x轴方向的加速度变为0.975m/s2，直到t=530s。之后加速度降为零，保持匀速运动到观测截止时间。观测周期T=1s，x和y轴独立地进行观测，观测噪声的标准差均为100m。图3-1 带有指数渐消因子的限定记忆滤波方法跟踪二维平面运动Fig.3-1 Limited memory kalman filter with e

32、xponential fading factor tracking two-demensional planar motion图3-2 跟踪二维平面运动的误差和标准差曲线Fig.3-2 Error and standard deviation of the curves when tracking two-demensional planar motion图3中所示的是利用带有指数渐消因子的限定记忆卡尔曼滤波方法跟踪运载体二维平面运动的综合仿真图，通过编辑matlab的M文件可以方便地对观测周期以及观测噪声方差进行设定。图3-1是真实轨迹、量测轨迹和滤波估计值的对比图，图3-2分别代表X轴和Y轴方向上的跟踪误差和标准差。由以上仿真图，可以看出带有指数渐消因子的限定记忆卡尔曼滤波方法对于高速机动动态目标的跟踪有着较好的效果，而且可以有效地抑止环境中的噪声影响，使滤波曲线更贴近真实运动轨迹。4 结论综上所述，系统方程不准的条件下，经典卡尔曼滤波方法鲁棒性不够理想，主要是因为增益矩阵完全由先验数据决定，而带有指数渐消因子的限定记忆卡尔曼滤波方法，通过记忆长度截断状态误差的累积，并根据实测误差信息与理论预计误差的信息比较，判断滤波是否发散，由此引入基于验后信息的指数渐消因子，在每一步递推中，都保证对增益矩阵和进行必要的修正：