郭 彥 于艷婷 胡獻(xiàn)君

(1.海軍航空兵學(xué)院 葫蘆島 125000)(2.91329部隊(duì) 威海 264200) (3.海軍工程大學(xué) 武漢 430033)

?

非線性濾波方法及其性能比較*

郭 彥1于艷婷2胡獻(xiàn)君3

(1.海軍航空兵學(xué)院 葫蘆島 125000)(2.91329部隊(duì) 威海 264200) (3.海軍工程大學(xué) 武漢 430033)

針對(duì)非線性估計(jì)問(wèn)題，對(duì)擴(kuò)展卡爾曼濾波、無(wú)跡卡爾曼濾波以及容積卡爾曼濾波等常見(jiàn)非線性濾波方法的一般原理和步驟進(jìn)行了描述，以微小型飛行器姿態(tài)估計(jì)問(wèn)題為例，對(duì)非線性濾波方法的估計(jì)精度和運(yùn)行效率進(jìn)行了對(duì)比分析，為實(shí)際的非線性估計(jì)問(wèn)題的處理提供了參考。

非線性濾波； 擴(kuò)展卡爾曼濾波； 無(wú)跡卡爾曼濾波； 容積卡爾曼濾波； 性能比較

Class Number V448.22；TP301.6

1 引言

對(duì)于估計(jì)問(wèn)題，實(shí)際當(dāng)中的系統(tǒng)根本上都是非線性的，因此需要非線性濾波方法進(jìn)行處理。

擴(kuò)展卡爾曼濾波(Extend Kalman Filter, EKF)[1～2]以及無(wú)跡卡爾曼濾波(Unscented Kalman Filter, UKF)[3～4]是廣泛應(yīng)用的非線性濾波方法。通過(guò)對(duì)非線性系統(tǒng)的一階線性化近似，EKF能夠較好地處理一般的非線性系統(tǒng)，但是由于忽略了系統(tǒng)的部分非線性特性，當(dāng)系統(tǒng)的非線性特性較為突出、初始誤差較大時(shí)，EKF存在估計(jì)效果急劇下降和濾波收斂速度緩慢的問(wèn)題，不能達(dá)到可靠的估計(jì)效果[5]。UKF采用無(wú)跡變換進(jìn)行隨機(jī)變量傳播，以獲取更為準(zhǔn)確的非線性函數(shù)概率分布，能夠更好地應(yīng)對(duì)非線性特征突出的非線性系統(tǒng)估計(jì)問(wèn)題[4]。但是UKF需要對(duì)三個(gè)可調(diào)參數(shù)進(jìn)行適當(dāng)選擇才能達(dá)到良好的濾波相關(guān)，且需要2n+1個(gè)采樣點(diǎn)，收斂時(shí)間相比于EKF有一定的增加[6]，在工程實(shí)現(xiàn)上存在限制[7]。而文獻(xiàn)[8～9]針對(duì)非線性系統(tǒng)估計(jì)問(wèn)題提出的容積卡爾曼濾波(Cubature Kalman Filter，CKF)，使用一組等權(quán)值的容積點(diǎn)集來(lái)計(jì)算非線性變換后的隨機(jī)變量的均值和協(xié)方差，具有更優(yōu)的非線性逼近性能(三階矩)、數(shù)值精度以及濾波穩(wěn)定性，同時(shí)具有實(shí)現(xiàn)簡(jiǎn)單、運(yùn)算時(shí)間短的特點(diǎn)，目前廣泛應(yīng)用于數(shù)據(jù)融合[10]、姿態(tài)估計(jì)[11]以及移動(dòng)機(jī)器人位姿估計(jì)[12]問(wèn)題中。

本文對(duì)常見(jiàn)非線性濾波方法的一般原理和濾波步驟進(jìn)行了介紹，并結(jié)合微小型飛行器姿態(tài)問(wèn)題對(duì)其性能進(jìn)行了詳細(xì)對(duì)比，為實(shí)際的非線性估計(jì)問(wèn)題的處理提供了參考。

2 非線性濾波方法

2.1 擴(kuò)展卡爾曼濾波

假設(shè)一個(gè)非線性系統(tǒng)：

(1)

其中：xk∈Rn為k時(shí)刻系統(tǒng)的狀態(tài)向量，f為n維向量函數(shù)，zk∈Rm為k時(shí)刻系統(tǒng)的觀測(cè)向量，h為m維向量函數(shù)，wk為均值為零、協(xié)方差為Qk的n維隨機(jī)過(guò)程噪聲，vk為均值為零、協(xié)方差為Rk的m維隨機(jī)量測(cè)噪聲，且wk、vk互不相關(guān)。擴(kuò)展卡爾曼濾波步驟描述如下。

1) 初始化

(2)

2) 時(shí)間-狀態(tài)更新

(1)計(jì)算狀態(tài)方程的雅各比矩陣:

(3)

(2)時(shí)間更新:

(4)

(3)計(jì)算量測(cè)方程的雅各比矩陣:

(5)

(4)量測(cè)更新:

(6)

(5)狀態(tài)更新

(7)

2.2 無(wú)跡卡爾曼濾波

無(wú)跡變換(UnsentedTransformation,UT)最早由Julier等提出，其基本原理是作者認(rèn)為“對(duì)非線性系統(tǒng)狀態(tài)分布的近似遠(yuǎn)比對(duì)非線性函數(shù)的近似簡(jiǎn)單”[13]。該算法通過(guò)選取一組采樣點(diǎn)(Sigma點(diǎn))來(lái)逼近系統(tǒng)的后驗(yàn)均值和協(xié)方差，避免了對(duì)非線性系統(tǒng)的線性化處理，同時(shí)也避免了求解Jacobian矩陣或Hessian矩陣，且達(dá)到了至少二階以上的后驗(yàn)均值和協(xié)方差值的估計(jì)精度，成為了近年來(lái)非線性濾波領(lǐng)域的研究熱點(diǎn)之一。用UT變換替換EKF中線性化的統(tǒng)計(jì)特性(均值、協(xié)方差)傳播，可得無(wú)跡卡爾曼濾波，具體步驟如下。

1) 初始化

(8)

2) 時(shí)間更新

(1)Sigma點(diǎn)選取和權(quán)值計(jì)算：

(9)

(10)

(11)

(12)

3) 量測(cè)更新

(13)

對(duì)應(yīng)的均值以及協(xié)方差權(quán)值如式(10)和(11)。

(2)計(jì)算量測(cè)的一步預(yù)測(cè)、方差以及互協(xié)方差：

(14)

4) 狀態(tài)更新

狀態(tài)更新過(guò)程計(jì)算系統(tǒng)狀態(tài)增益矩陣、估計(jì)值以及狀態(tài)誤差方差：

(15)

2.3 容積卡爾曼濾波

CKF的核心思想是通過(guò)三階容積積分原理，計(jì)算函數(shù)的標(biāo)準(zhǔn)加權(quán)高斯積分[12]。標(biāo)準(zhǔn)的CKF步驟與UKF相似，區(qū)別在于采樣點(diǎn)的計(jì)算有所不同。

1) 時(shí)間更新

(1)設(shè)k-1時(shí)刻協(xié)方差矩陣Pk-1|k-1正定，對(duì)其進(jìn)行因式分解得到Sk-1|k-1，即

(16)

(2)容積點(diǎn)估計(jì)

(17)

(3)容積點(diǎn)傳播

(18)

(4)求解狀態(tài)一步預(yù)測(cè)值

(19)

(5)計(jì)算預(yù)測(cè)誤差協(xié)方差矩陣

(20)

其中：Qk-1表示系統(tǒng)噪聲協(xié)方差矩陣。

2) 量測(cè)更新

(1)預(yù)測(cè)誤差協(xié)方差矩陣分解

(21)

(2)容積點(diǎn)估計(jì)

(22)

(3)容積點(diǎn)傳播

(23)

(4)計(jì)算量測(cè)預(yù)測(cè)值

(24)

(25)

(26)

其中：Rk表示量測(cè)噪聲協(xié)方差矩陣。

3) 狀態(tài)更新

(1)求解Kalman增益

(27)

(28)

3 仿真分析

3.1 問(wèn)題描述

(29)

量測(cè)為根據(jù)加速度計(jì)與磁強(qiáng)計(jì)的輸出求解得到的姿態(tài)角，方程為

(30)

式中：wk為量測(cè)噪聲，其均值為零且方差為Rk。

3.2 仿真與分析

圖1 估計(jì)誤差比較

算法滾動(dòng)角俯仰角航向角EKF0.0190.0190.020UKF0.0160.0160.017CKF0.0150.0150.015

1) 運(yùn)算效率：EKF運(yùn)算效率最高，CKF次之，UKF最差，具體為令EKF的100次Monte-Carlo仿真耗時(shí)為1，則UKF為1.89，CKF為1.67；

2) 估計(jì)精度：CKF最優(yōu)，UKF次之，EKF最差，具體為相比于UKF、EKF，CKF在滾動(dòng)、俯仰和航向三個(gè)維度上分別提高了(22.51%、9.94%)、(24.48%、9.94%)、(23.35%、10.12%)。

表2 EKF/UKF/CKF100次Monte-Carlo仿真耗時(shí)(s)

4 結(jié)語(yǔ)

本文對(duì)EKF、UKF以及CKF等常見(jiàn)的非線性濾波器的一般步驟進(jìn)行了描述，并結(jié)合歐拉角描述的微小型飛行器姿態(tài)結(jié)算模型，對(duì)三個(gè)非線性濾波方法的估計(jì)精度、運(yùn)算效率進(jìn)行了對(duì)比，仿真結(jié)果表明，運(yùn)算效率方面EKF運(yùn)算效率最高，CKF次之，UKF最差，具體為令EKF的100次Monte-Carlo仿真耗時(shí)為1，則UKF為1.89，CKF為1.67； 估計(jì)精度方面，CKF最優(yōu)，UKF次之，EKF最差，具體為相比于UKF、EKF，CKF在滾動(dòng)、俯仰和航向三個(gè)維度上分別提高了(22.51%、9.94%)、(24.48%、9.94%)、(23.35%、10.12%)。

[1] A. Asadian, M. R. Kermani, and R. V. Patel. A novel force modeling scheme for needle insertion using multiple Kalman filters [J]. IEEE Trans. Instrum. Meas.,2012,61(2):429-438.

[2] M. Saha, R. Ghosh, and B. Goswami. Robustness and sensitivity metrics for tuning the extended Kalman filter [J]. IEEE Trans. Instrum. Meas.,2014,63(4):964-971.

[3] R. Kandepu, B. Foss, and L. Imsland. Applying the unscented Kalman filter for nonlinear state estimation [J]. J. Process Control,2008,18(7-8):753-768.

[4] D. H. Dini, D. P. Mandic, and S. J. Julier. A widely linear complex unscented Kalman filter [J]. IEEE Signal Process. Lett.,2011,18(11):623-626.

[5] 魏喜慶，宋申民.基于改進(jìn)容積卡爾曼濾波的奇異避免姿態(tài)估計(jì)[J].航空學(xué)報(bào)，2013,34(3):610-619. WEI Xiqing, SONG Shenmin. Improved Cubature Kalman Filter Based Attitude Estimation Avoiding Singularity [J]. Acta Aeronautica et Astronautica Sinica,2013,34(3):610-619.

[6] 賈瑞才.重力/地磁輔助的歐拉角無(wú)跡卡爾曼濾波姿態(tài)估計(jì)[J].光學(xué)精密工程，2014,22(12):3280-3286. JIA Ruicai. Attitude estimation base on gravity/magnetic assisted Euler angle UKF [J]. Optics and Precision Engineering,2014,22(12):3280-3286.

[7] 劉華，吳文，王世元.基于平方根CKF的多傳感器序貫式融合跟蹤算法[J].系統(tǒng)工程與電子技術(shù)，2015,37(7):1494-1498. LIU Hua, WU Wen, WANG Shiyuan. Multrsensor sequential fusion tracking algorithm based on square-root cubature Kalman filter [J]. Systems Engineering and Electronics,2015,37(7):1494-1498.

[8] Arasaratnam I，Haykin S. Cubature Kalman filters [J]. IEEE Trans. on Autoynatic Control,2009,54(6):1254-1269.

[9] Arasaratnam I，Haykin S. Cubature Kalman smoothers [J]. Automatrca,2011,47(10):2245-2250.

[10] 胡振濤，曹志偉，李松，等.基于容積卡爾曼濾波的異質(zhì)多傳感器融合算法[J].光電子激光，2014,25(4):697-703. HU Zhentao, CAO Zhiwei, LI Song, et al. Heterogeneous multrsensor fusion algorithm based on cubature Kalman filter [J]. Journal of Optoelectronics·Laser, 2014,25(4):697-703.

[11] 錢華明，黃蔚，葛磊.基于改進(jìn)的迭代容積卡爾曼濾波姿態(tài)估計(jì)[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2014,46(6):116-122. QIAN Huaming, HUANG Wei, GE Lei. Attitude estimation based on improved iterated cubature Kalman filter [J]. Journal of Harbin Institiitf of Technology,2014,46(6):116-122.

[12] 康軼非，宋永端，宋宇，等.平方根容積卡爾曼濾波在移動(dòng)機(jī)器人SLAM中的應(yīng)用[J].機(jī)器人，2013,35(2):186-193. KANG Yifei, SONG Yongduan, SONG Yu, et al. Square-Root Cubature Kalman Filter and Its Application to SLAM of an Mobile Robot [J]. ROBOT,2013,35(2):186-193.

Nonlinear Filtering Methods and Performances Comparison

GUO Yan1YU Yanting2HU Xianjun3

(1. Naval Aviation Academy, Huludao 125000)(2. No. 91329 Troops of PLA, Weihai 264200) (3. Naval University of Engineering, Wuhan 430033)

Aiming at the problem of nonlinear estimation, the procedures of extended Kalman filter, unscented Kalman filter and cubature Kalman filter have been described, taking the attitude estimation problem of the micro aerial vehicle as an example, the estimation accuracy and the running efficiency of the nonlinear filtering methods are compared and analyzed, which provides a reference for the actual processing of the nonlinear estimation problem.

nonlinear filter, extended Kalman filter, unscented Kalman filter, cubature Kalman filter, performance comparison

2016年5月9日,

2016年6月17日

郭彥,女,碩士研究生,助教,研究方向:航空電子、高等教育理論。于艷婷,女,助理工程師，研究方向:指揮自動(dòng)化。胡獻(xiàn)君,男,博士,助理研究員，研究方向:武器系統(tǒng)優(yōu)化。

V448.22；TP301.6

10.3969/j.issn.1672-9722.2016.11.003