一种新的自适应非线性卡尔曼滤波算法_王秋平

第35卷第7期 光电工程V ol.35, No.7 2008年7月Opto-Electronic Engineering July, 2008文章编号：1003-501X(2008)07-0017-05

一种新的自适应非线性卡尔曼滤波算法

王秋平1,3，陈 娟1,2，王显利4，王习文1,3

( 1. 中国科学院长春光学精密机械与物理研究所，长春 130022；2. 长春工业大学电气与电子工程学院，长春130012；3. 中国科学院研究生院，北京 100039；4. 北华大学交通建筑工程学院，吉林吉林 132013) 摘要：为避免由于系统噪声统计特性不准确所导致的滤波性能下降问题，改进了一种基于新息的系统噪声方差调整方法，并将其与扩展卡尔曼滤波、Unscented 卡尔曼滤波和差分滤波相结合，形成自适应非线性卡尔曼滤波。

将此方法应用到非线性测量光电跟踪系统中，并与采用基本非线性卡尔曼滤波进行性能对比。仿真实验结果证明该方法可以实时调整系统噪声方差，有效地避免由于系统噪声统计特性不准确所带来的滤波性能下降的问题，而且其性能明显优于基本非线性卡尔曼滤波。

关键词：系统噪声方差估计；卡尔曼滤波；自适应非线性卡尔曼滤波；非线性测量

中图分类号：TP391 文献标志码：A

New Adaptive Nonlinear Kalman Filters Algorithm

WANG Qiu-ping1,3，CHEN Juan1,2，WANG Xian-li4，WANG Xi-wen1,3 ( 1．Changchun Institute of Optics, Fine Mechanics and Physics, Chinese Academy of Science, Changchun 130022，China;

2. School of Electrical & Electronic Engineering, Changchun University of Technology, Changchun 130012, China;

3. Graduate School of Chinese Academy of Sciences, Beijing 100039, China;

4. Traffic Construction Engineering College , Beihua University, Jilin 132013, Jilin Province, China )

Abstract: A new system noise covariance modification algorithm is proposed in order to avoid the problem of degraded performance of the filter due to the incorrect statistics of the system noise. Combined with the Extended Kalman Filter (EFK)、Unscented Kalman Filter (UKF) and Divided Difference Filter (DDF), adaptive nonlinear Kalman filters are developed. The algorithm is applied in nonlinear measurement electro-optical tracking system and the performances of the adaptive nonlinear Kalman filter is compared with the basic nonlinear Kalman filters. The Matlab simulation results show that the filter can modify system noise covariance in real time, efficiently avoid the above problem and the performance outperforms the basic nonlinear Kalman filters.

Key words: system noise covariance estimate; Kalman filter; adaptive nonlinear Kalman filter; nonlinear measurement

1 引 言

卡尔曼滤波[1]采用状态空间法描述系统，算法采用递推形式，为线性动态系统的最优估计。但是在实际应用中许多问题都存在非线性，如：物理系统模型本身的不精确、描述方程的非线性以及病态方差矩阵等因素导致的非线性问题[2]都需要非线性滤波技术。而最优非线性滤波技术需要得到其条件后验概率的完整描述，且这种精确描述又因需要无尽的参数而无法实际应用[3]。因此通常采用将精确的非线性方程近似处理成线性方程，忽略系统的非线性，得到次优状态估计的非线性滤波方法[4]。

由于卡尔曼滤波器在其更新规则中仅用到状态的前两阶信息(均值和协方差)，以及均值和协方差的传

收稿日期：2007-09-13；收到修改稿日期：2008-03-25

基金项目：长春光机所三级创新项目(20070102)

作者简介：王秋平(1973-)，女(汉族)，吉林东辽人，博士研究生，主要研究工作是非线性滤波方法的研究。E-mail: wabgqiuping@https://www.wendangku.net/doc/532791103.html,

光电工程 第35卷第7期

18播所具有的独特特性，目前人们在非线性滤波方法中仍然期望延用卡尔曼线性估计形式[5]。其中最简单、应用最广的非线性卡尔曼滤波为扩展卡尔曼滤波[6](Extended Kalman Filter ，EKF)，但是 EKF 在实际使用中存在线性化误差、雅可比矩阵难计算等明显的缺陷。针对这些问题，近年来出现了一类新的、高效的非线性滤波方法，这种方法包括Unscented 卡尔曼滤波(Unscented Kalman filter ，UKF)、中心差分滤波(Central difference filter ，CDF)和差分滤波(Divided difference filter ，DDF)，统称为Sigma 点卡尔曼滤波器(Sigma Point Kalman filter ，SPKF) [7]。SPKF 不需要对非线性方程进行泰勒近似处理，而是采用计算简便、精度更高的插值多项式近似方法或者直接采用非线性变换技术(Unscented Transformation ，UT)，使非线性系统的均值和协方差可以直接按非线性映射递推传播，从而得到易实现、高精度的非线性滤波方法。但是当系统噪声和测量噪声的统计特性不准确时，SPKF 也存在估计误差，致使估计精度降低甚至发散[8]。针对以上问题，Lee [8]在考虑测量噪声的统计特性相对系统噪声的统计特性容易获得基础上，提出了一种将Maybeck 系统噪声方差估计方法和Downhill Simplex 数值计算方法相结合，实时调整系统噪声方差的自适应非线性滤波方法。此方法需要先用整个过程的测量数据和非线性滤波得到的所有新息来确定系统噪声调整因子γ，然后再在非线性滤波过程中对系统噪声方差实时调整。从以上描述中可以看出，该方法由于用到所有的新息并不能真正的实时调整系统噪声方差。因此本文在基于Lee 的思想基础上，对其调整因子γ的计算进行了改进，采用在预测滤波过程中利用测量数据和新息计算γ，同时对γ和系统噪声方差进行调整，得到一种新的基于新息的系统噪声方差调整自适应次优非线性卡尔曼滤波方法。本文将该方法应用到非线性测量光电跟踪系统中，利用实测数据进行了仿真实验并与普通非线性卡尔曼滤波进行性能对比，仿真和实验结果表明，本文所提出的方法是有效的，且可以明显的改善滤波性能，提高滤波精度。

2 自适应非线性卡尔曼滤波

2.1 系统噪声方差估计方法

Lee 提出的系统噪声方差估计方法为

)?(1??1|1*1|1|?????+

=k k k k k k Q Q Q Q γ

(1) 1|2/11

2,22,21,?,])(1[

)(?=?=++=∑k k k k N i m i i i k ΖΖv v v v N

J "γ (2) 1||1|11||T *??,)]?([?????=??????=k k k k k k k k k k k k k ΧΧΧQ ΡΡΧΧQ (3) 其中：k k Q |?为k 时刻系统噪声估计方差，γ为调整因子，k v 为k 时刻测量新息，*Q 代表状态估计后和测量 更新前系统噪声方差变化，?X k 为k 时刻状态新息，m 为测量状态的维数，N 为整个过程测量数据的总数。

从式(1)~(3)中可看出，k

k Q

|?的调整关键在于γ值的确定，γ太小则更新过程影响会太大，γ太大则更新过 程影响又会太小；但是式(2)中计算γ 需用到整个过程测量数据所得到的新息值，这一点在实际应用中是无法达到实时性要求的，不适合实际工程应用。因此本文在基于Lee 的思想基础上，将式(2)用式(4)代替，其余方程不变，得到一种新的基于新息的系统噪声方差估计方法。

2/112,22,21,])(1[)(∑=++=k

i m i i i k v v v k

J "γ (4)

该方法具有如下特点：

1) 将式(2)中的N 用k 来代替，则只需用当前及以前时刻的新息值来计算γ，不需先用整个过程的新息值。而且可以实时调整γ 值和系统噪声方差，满足实时性的要求；

2) 在实际算法执行过程中，可以采用累加和的形式来保存新息的平方和，这样不需要保存所有时刻的测量值和新息值也不会增加运算负担，使得算法简单易行；

3) 这种基于新息的系统噪声方差估计方法可以和任何一种以卡尔曼滤波为框架的非线性滤波方法相结合，实现自适应非线性卡尔曼滤波算法，改善滤波性能。 2.2 自适应非线性卡尔曼滤波

2008年7月 王秋平 等：一种新的自适应非线性卡尔曼滤波算法

19

将目前最具有代表性以卡尔曼滤波为框架的非线性滤波方法扩展卡尔曼滤波(EKF)、Unscented 卡尔曼滤波(UKF)和差分滤波(DDF)和本文提出的基于新息的系统噪声方差估计方法相结合就可以得到自适应非线性卡尔曼滤波算法，分别记为AEKF 、AUKF 和ADDF 。算法结构具体描述如图1。

3 非线性测量的光电跟踪系统仿真实验及实验结果分析

3.1 非线性测量的光电跟踪系统仿真实验

光电跟踪系统的非线性主要产生于将极坐标下的光电传感器测量的距离、方位角和俯仰角转换为直角坐标下的目标的运动参数，因此运动参数的滤波预测属于非线性估计[9]。本文为实现光电跟踪系统目标运动参数的预测滤波仿真实验，做以下设计：

1) 目标运动模型选取考虑随机干扰的二阶常速(CV)、三阶常加速(CA)模型[10]，取目标的运动参数(位置、速度、加速度)作为系统的状态变量，且系统噪声假设为互不相关的高斯白噪声；

2) 光电跟踪目标测量模型选取极坐标系下测量模型，目标测量值(Z )由距离r ，方位角a ，俯仰角e 组成，极坐标形式的离散化光电跟踪测量模型如式(5)所示。

[]?

?????????+??????????+++=+=??

???

?????=e a r k k k k k k k k k k k k k k v v v y x z x y z y x V Χh e a r Ζ])/(arctan[)/arctan()(2/1222

/1222 (5) 并假定v r ，v a ，v e 为互不相关的高斯白噪声，方差分别为σ 2r ，σ 2a ，σ 2e ，测量噪声方差为R k =diag{σ 2r ，

σ 2a ，σ 2e }。

3) 测量数据取某靶场对实际空间随机运动目标进行跟踪的实测数据，并取测量误差为σr =5 m ，

σa =σe =3″，采样周期为T =0.006 25 s ；

4) 坐标系选择在混合坐标系下，实现文中提出的自适应非线性卡尔曼滤波。同时在仿真实验中还采用了EKF 、UKF 和DDF 预测滤波方法，并与文中提出的自适应非线性次优卡尔曼滤波进行性能对比。仿真实验框图如图2所示。

3.2 仿真实验结果分析

仿真实验结果见表1、2和图3~8。其中表1、2分别为CV 模型和CA 模型下各种滤波算法的均方根误差和均值误差统计数据，图3~8分别为CV 模型和CA 模型下各种滤波算法的预测误差曲线。

To workspace1

To workspace2

To workspace3

光电工程第35卷第7期20

从表格数据中可以看出：在两种目标运动模型下，虽然本文提出的自适应非线性卡尔曼滤波算法的俯仰角均方根误差和均值误差比基本非线性卡尔曼滤波算法的误差大，但数值相差并不大；而本文算法的距离和方位角的统计数据却明显低于基本非线性卡尔曼滤波算法。因此本文提出的自适应非线性卡尔曼滤波的性能要优于基本非线性卡尔曼滤波的性能。

E

r

r

o

r

-

r

/

m

E

r

r

o

r

-

e

/

d

e

g

-0.4

0.4

1 2 3 5 6 7 89

4

Time /s

E

r

r

o

r

-

a

/

d

e

g

-4

-2

2

-50

50

E

r

r

o

r

-

r

/

m

E

r

r

o

r

-

e

/

d

e

g

-0.5

0.5

E

r

r

o

r

-

a

/

d

e

g

-4

4

-100

100

0123 5 6 7 8 9

4

Time /s

图5DDF和ADDF算法CV模型预测误差曲线

Fig.5 Predicted error curves of DDF and ADDF in CV model

图6DDF和ADDF算法CA模型预测误差曲线

Fig.6 Predicted error curves of DDF and ADDF in CA model

E

r

r

o

r

-

e

/

d

e

g

-0.4

0.4

E

r

r

o

r

-

a

/

d

e

g

-3

1

E

r

r

o

r

-

e

/

d

e

g

-0.4

0.4

E

r

r

o

r

-

a

/

d

e

g

-4

2

-2

-1

-2

——:DDF -------:ADDF

0 1 2 3 5 6 7 89

4

Time /s

——:DDF-------:ADDF

0123 5 6 7 8 9

4

Time /s

2008年7月 王秋平 等：一种新的自适应非线性卡尔曼滤波算法

21

从误差曲线图形中可看出：对于两种目标运动模型，文中提出的自适应非线性卡尔曼滤波算法大大降低方位角误差，有效的改善距离误差；俯仰角误差与基本非线性卡尔曼滤波算法的俯仰角误差基本相当。

因此以上仿真实验结果证明了本文提出的系统噪声方差估计方法可以和任何一种以卡尔曼滤波为框架的非线性滤波方法相结合，实现自适应非线性卡尔曼滤波算法，且性能优于基本非线性卡尔曼滤波。

4 结 论

卡尔曼滤波为线性系统的最优状态估计方法，但由于在实际应用中许多问题都存在非线性，且最优非线性滤波的实现在理论上、工程上都存在极大的困难，因此多年来最优非线性滤波技术一直都是各国学者研究的热点。

在实际工程应用中，应用最早、最广泛的非线性滤波为扩展卡尔曼滤波(EKF)，但是EKF 在实际使用中存在线性化误差、雅可比矩阵难计算等明显的缺陷。针对这些问题，近年来出现了Sigma 点卡尔曼滤波器(SPKF)，可以有效的改善滤波效果。但是两种滤波算法在当系统噪声和测量噪声的统计特性不准确时，都会有估计误差，致使估计精度降低甚至发散。为解决此问题，本文在基于Lee 的思想基础上提出了一种新的基于新息的系统噪声方差调整方法，且将该方法与非线性卡尔曼滤波方法相结合，形成自适应非线性卡尔曼滤波算法。并在非线性测量光电跟踪系统进行了仿真试验，仿真实验结果证明文中提出的基于新息的系统噪声方差调整自适应非线性卡尔曼滤波，可以有效的解决由于系统噪声统计特性不准确所造成的估计精度低和滤波发散问题，而且不增加计算量和存储量。该方法也可以应用到其它需要采用非线性滤波技术的领域中。 参考文献：

[1] GregW ，BishopG . An introduction to the Kalmanfilter. Technica Report TR 95-041[R]. [S.l.]：Department of Computer

Science ，University Of North Carolina at Chapel Hill ，Updated ，2003：1-16.

[2] FRED DAUM. Nonlinear filters beyond the Kalman filter [J]. IEEE A&E SYSTEMS MAGAZINE ，2005，20(8)：57-69. [3] Kushner H J. Dynamical equations for optimum nonlinear filtering [J]. Journal of Differential Equations ，1967(3)：179-190. [4] Rong Li X ，Jilkov V esselin P. A Survey of Maneuvering Target Tracking ：Approximation techniques for Nonlinear Filtering [C]//

Signal and Data Processing of Small Targets . Orlando FL ，ETATS-UNIS ，2004，5428：537-550.

[5] SIMON J JULIER ，JEFFREY K ，UHLMANN. Unscented Filtering and Nonlinear estimation [J]. Proceedings of the IEEE ，

2004，92(3)：401-422.

[6] Sy-Miin Chow ，Kuala Lumpur ，Malaysia. Fitting Linear and Nonlinear Dynamic Models Using Different Kalman Filter

Approaches [D]. Virginia ：University of Virginia ，2004. (下转第27页)

图7 UKF 和AUKF 算法CV 模型预测误差曲线Fig.7

Predicted error curves of UKF and AUKF in CV model

图8 UKF 和AUKF 算法CA 模型预测误差曲线

Fig.8

Predicted error curves of UKF and AUKF in CA model

E r r o r -r /m

E r r o r -e /d e g

-0.4

0 0.4 E r r o r -a /d e g -3 0 1 -100 0 100 -1 -2 E r r o r -r /m

E r r o r -e /d e g -0.4

00.4E r r o r -a /d e g

-202-10001004——:UKF -------:AUKF

0 1 2 3 5 6 7 8

9

4 Time /s

——:UKF

-------:AUKF

1

2

3

5 6 7 8 94Time /s

2008年7月朱齐丹等：基于LMedS估计的快速稳像方法27 [6] Hansen M，Anadan P，Dana K，et al. Real-time Scene Stabilization and Mosaic Construction [C]// Proc. of IEEE CVPR. Los

Alamitos：IEEE，1994：54-62.

[7] Morimoto C，Chellappa R. Fast Electronic Digital Image Stabilization [C]// Proc. of 13th IEEE ICPR. Los Alamitos：IEEE

Comput. Soc. Press，1996：284-288.

[8] 杨雨东，徐光祐，朱志刚. 2.5维帧间运动估计方法 [J]. 清华大学学报：自然科学版，1997，37(9)：78-81.

YANG Yu-dong，XU Guang-yuo，ZHU Zhi-gang. Method of 2.5-D Inter-frame Motion Estimation [J]. Journal of Tsinghua University：Sci &Tech，1997，37(9)：78-81.

[9] Heuer J，Kaup A. Global Motion Estimation in Image Sequences Using Robust Motion Vector Field Segmentation [C]// Proc.

ACM Multimedia 99. New York：ACM，1999：261-264.

[10] Cheung H K，Siu W C. Fast Global Motion Estimation for Sprite Generation [C] //Proceedings of the IEEE Internationa

Symposium on Circuits and Systems (ISCAS). Piscataway：IEEE，2002：26-29.

[11] Hirooka M，Sumi K，Hashimoto M，et al. Hierarchical Distributed Template Matching [C]// Proc. SPIE Symposium on

Electronic Imaging & Science and Technology. USA：SPIE-Int. Soc. Opt. Eng，1997：176-183.

[12] 米长伟，刘晓利，徐明友. 基于SSDA的改进算法 [J]. 弹箭与制导学报，2004，24(1)：85-87.

MI Chang-wei，LIU Xiao-li，XU Ming-you. An Advanced Algorithm Based on SSDA[J]. Journal of Projectiles Rockets Missiles and Guidance，2004，24(1)：85-87.

[13] 李俊山，谭园园，张媛莉. SSDA的改进算法 [J]. 电光与控制，2007，14(2)：66-68.

LI Jun-shan，TAN Yuan-yuan，ZHANG Yuan-li. An Improved SSDA [J]. ELECTRONICS OPTICS & CONTRO，2007，14(2)：66-68.

[14] Shimizu M，Okutomi M. Precise Sub-Pixel Estimation on Area-Based Matching [C]// Proc. 8th IEEE International

Conference on Computer Vision (ICCV2001). Vancouver：Institute of Electrical and Electronics Engineers Inc，2001：90-97.

JJJJJJJJJJJJJJJJJJJJJJJJJJJJJJJJJJJJJJJJJJJJJ (上接第21页)

[7] Lee D J，Alfriend K T. Sigma Point Kalman Filters for Efficient Orbit estimation [C]// AAS/AIAA Astrodynamics Specialist

Conference. Big Sky：MT，2003.

[8] Lee Deok-Jin. Nonlinear Bayesian filtering with applications to estimation and navigation [D]. Texas：A&M University，2005.

[9] 杨秀华，陈涛，王延风，等. 光电跟踪目标的非线性滤波算法研究 [J]. 仪器仪表学报，2004，25(4)：810-812.

YANG Xiu-hua，CHEN Tao，WANG Yan-feng，et al. Research on nonlinear filtering to optoelectronic target [J]. Chinese Juranl of Scientific Instrument，2004，25(4)：810-812.

[10] 周宏仁，敬忠良，王培德. 机动目标跟踪 [M]. 北京：国防工业大学出版社，1991.

ZHOU Hong-ren，JING Zhong-liang，WANG Pei-de. Mobile target tracked [M]. Beijing：National Defence Industry Press，1991.