一種強(qiáng)跟蹤擴(kuò)展卡爾曼濾波器的改進(jìn)算法

1、 第21卷第1期V o l . 21N o. 1控制與決策Con trol and D ecision 2006年1月Jan . 2006文章編號:100120920(2006 0120073204一種強(qiáng)跟蹤擴(kuò)展卡爾曼濾波器的改進(jìn)算法范文兵1, 劉春風(fēng)1, 張素貞2(1. 鄭州大學(xué)信息工程學(xué)院, 鄭州450052; 2. 華東理工大學(xué)自動(dòng)化研究所, 上海200237摘要:針對模型不匹配卡爾曼的狀態(tài)估計(jì)發(fā)散和應(yīng)用范圍限于連續(xù)系統(tǒng)問題, 器(ST FD EKF . 在濾波計(jì)算中, ; 濾波器應(yīng)用有限差分方法計(jì)算濾波過程中非線性函數(shù)的偏導(dǎo)數(shù), . , . ST FD EKF 應(yīng)用于復(fù)雜非線性系統(tǒng)狀態(tài)

2、估計(jì)時(shí), 關(guān)鍵詞:有限差分; 強(qiáng)跟蹤濾波; ; 中圖分類號:O :AI m of Strong Track i ng Extended Kal man F ilterFA N W en 2bing , L IU Chun 2f eng , ZH A N G S u 2z hen112(1. Co llege of Info r m ati on Engineering , Zhengzhou U niversity , Zhengzhou 450052, Ch ina ; 2. R esearch Institute of A utom ati on , East Ch ina U niver

3、sity Science and T echno logy , Shanghai 200237, Ch ina . Co rrespondent :FAN W en 2bing , E 2m ail :iew bfan zzu . edu . cn Abstract :A strong track ing finite 2difference Kal m an filter (ST FD EKF is p resented to handle the divergence p roblem of state esti m ati on of a nonlinear m is m atched

4、model and li m ited app licati on scope . In filtering calculati on , strong track ing facto r is introduced to modify p ri o ri covariance m atrix to i m p rove the accuracy of the filter . T he filter uses finite 2difference m ethod to calculate partial derivatives of nonlinear functi ons to enlar

5、ge its app licati on scope . T he comparison of several Kal m an filters show s that the ST FD EKF filter has h igh num erical stability , strong track ing and larger app licati on scope and it can be app lied to state esti m ati on of comp lex nonlinear system s . Key words :F inite 2difference ; S

6、trong track ing filtering ; N onlinear system ; M odel m is m atch ; State esti m ati on1引言擴(kuò)展卡爾曼濾波器(EKF 是一種應(yīng)用最廣泛的非線性濾波方法之一, 人們在如何改善卡爾曼濾波的計(jì)算復(fù)雜性及數(shù)值穩(wěn)定性方面作了大量的研究, 提出各種基于平方根濾波、奇異值濾波與U 2D 分解濾波等方法. 但是, 濾波估計(jì)技術(shù)在實(shí)際應(yīng)用時(shí), 卡爾曼濾波器的收斂性、估計(jì)精度、實(shí)用性仍需不斷改進(jìn). 之所以如此, 主要由于實(shí)際非線性動(dòng)態(tài)模型較復(fù)雜, 存在不確定性或噪聲統(tǒng)計(jì)特性近似已知或未知, 造成了濾波器性能下降.文獻(xiàn)1提出帶

7、多重漸消因子的有色噪聲干擾的強(qiáng)跟蹤濾波器, 較好地解決了擴(kuò)展卡爾曼濾波器收稿日期:2004212207; 修回日期:2005204211.(EKF 關(guān)于不確定模型的魯棒性較差、狀態(tài)估計(jì)精度不高, 甚至發(fā)散等問題. 文獻(xiàn)2, 3提出采用內(nèi)插公式計(jì)算均值、方差和協(xié)方差矩陣的濾波算法, 使其應(yīng)用擴(kuò)展到非線性不連續(xù)情況, 擴(kuò)大了濾波器應(yīng)用范圍, 增強(qiáng)了濾波過程的收斂性.本文首先引入強(qiáng)跟蹤擴(kuò)展卡爾曼濾波概念, 然后采用非線性函數(shù)的差分運(yùn)算代替強(qiáng)跟蹤濾波器中的偏導(dǎo)數(shù)計(jì)算, 繼承文獻(xiàn)13中算法的優(yōu)點(diǎn), 進(jìn)而推導(dǎo)出基于有限差分的強(qiáng)跟蹤濾波算法, 并從濾波器數(shù)值穩(wěn)定性、強(qiáng)跟蹤性等方面比較幾種濾波器算法的狀態(tài)估計(jì)

8、過程.2強(qiáng)跟蹤非線性濾波器基金項(xiàng)目:國家863高科技計(jì)劃項(xiàng)目(2002AA 412120 ; 江蘇省科技攻關(guān)項(xiàng)目(BE 2005035 .作者簡介:范文兵(1969 , 男, 鄭州人, 博士, 副教授, 從事系統(tǒng)濾波、預(yù)測控制、智能控制等研究; 劉春風(fēng)(1983 ,男, 河南安陽人, 碩士生, 從事先進(jìn)控制算法的研究. 74控制與決策第21卷帶次優(yōu)漸消因子的擴(kuò)展卡爾曼濾波器(SFEKF 與常規(guī)擴(kuò)展卡爾曼濾波器相比, 具有以下優(yōu)點(diǎn)1:較強(qiáng)的魯棒性; 極強(qiáng)的跟蹤能力; 適中的計(jì)算復(fù)雜性. 由于漸消因子的作用, SFEKF 保持了不同時(shí)刻的殘差序列處處正交. 它的物理意義說明, 當(dāng)存在模型不確定性時(shí)

9、, 在線調(diào)整增益矩陣可使殘差始終保持高斯白噪聲的性質(zhì). 該濾波器充分利用了殘差序列中的有效信息, 對過程參數(shù)變動(dòng)具有較強(qiáng)的魯棒性, 其估值精度也較常規(guī)的EKF 大為提高. 考慮如下形式的一般非線性模型, 狀態(tài)方程和觀測方程如下:x k +1=f (x k , u k , v k , y k =g (x k , w k .當(dāng)系統(tǒng)滿足條件:F v (k Q k F v (k T 是半正定對是正定對稱陣, 漸消因稱陣, G w (k R k G w (k T 和P 0子(k +1 的計(jì)算采用文獻(xiàn)1的一步算法, 即(k +1 =式中trM (k +1 ,0=trN (k +1 N (k +1 =V 0

10、(k +1 -G x (k F v (k Q kT T TF v (k G x (k -G w (k R k G w ,0, 01; 1, . 0<1(13(14T TM (k 1 =F k x (k G x (k . (16(1 (15式中:x k 為狀態(tài)變量; y k ; ;v k 和w k . 其中 01k V 0(k +1 =j T j =kT G x (0 P 0G x (0 +G w (0 R 0G w (0 , k =0; T , k 1.1+j =1, , 其均值和方差為E v k =q k , cov v k , v j =Q k (k -j ,E w k =r k ,

11、 covw k , w j =R k (k -j .(3(17擴(kuò)展卡爾曼濾波器是在線性卡爾曼濾波器基礎(chǔ)上發(fā)展而來的, 采用非線性函數(shù)泰勒級數(shù)展開線性化方法, 近似表示原系統(tǒng)的狀態(tài)方程和觀測方程, 表達(dá)式如下:x k +1f (x k , u k , q k +F x (k (x k -F v (k (v k -q k , y k g (x k , r k +G x (k (x k -x k +G w (k (w k -r k .x k +式中0<1為殘差序列的遺忘因子, 一般取=0. 95.3基于有限差分的強(qiáng)跟蹤濾波器擴(kuò)展卡爾曼濾波器(EKF 是基于T aylo r 展開級數(shù)的線性卡爾曼

12、濾波, 其應(yīng)用場合是要求非線性狀態(tài)傳遞方程和觀測方程或函數(shù)具有可微性或連續(xù)性48. 然而, 非線性函數(shù)的一階級數(shù)線性化在許多方面不足以提供較精確的近似, 往往產(chǎn)生較大的偏差, 甚至引起濾波器的發(fā)散. 本節(jié)吸取了文獻(xiàn)2, 3中濾波器實(shí)現(xiàn)的一些思想, 提出應(yīng)用有限差分代替強(qiáng)跟蹤濾波算法中的非線性函數(shù)的偏導(dǎo)數(shù)計(jì)算, 進(jìn)而對函數(shù)的自協(xié)方差和互協(xié)方差的計(jì)算進(jìn)行改進(jìn), 得到基于有限差分的強(qiáng)跟蹤濾波器(ST FD EKF 算法.進(jìn)行Cho lesky 分解, 有k , P對Q k , R k , P kQ k =S v S v , R k =S w S w ,TT(4 (5式中:F x (k 和F v (k

13、 是f ( 對x , v 的偏導(dǎo)數(shù);G x (k 和G w (k 是f ( 對x , w 的偏導(dǎo)數(shù).SFEKF 濾波器的表達(dá)形式如下:驗(yàn)前狀態(tài)估計(jì)x k +1=f (x k , u k , q k ,y k =g (x k , r k ; 驗(yàn)前方差估計(jì)T P k +1=(k +1 F x (k P k F x (k +F v (k Q k F v (k .T(6 (7(8P k =S x S x , P k =S x S x . F x (k =f ij =T T(18式中(k +1 1為漸消因子, 它對過去的數(shù)據(jù)進(jìn)行漸消, 實(shí)時(shí)調(diào)整狀態(tài)的預(yù)報(bào)協(xié)方差陣. 驗(yàn)后增益矩陣、狀態(tài)估計(jì)和方差估計(jì)分別為

14、T TK k +1=P k +1G x (k G x (k P k +1G x (k +G w (k R k G w (k T -1, x k +1=x k +1+K k +1y k -y k ,P k +1=I -K k +1G x (k P k +1. 殘差序列為y k . k +1=y k -(9 (10 (11 (12非線性函數(shù)偏導(dǎo)數(shù)F x (k 的中心差分形式如下:(f i (x k , j +x k , j , u k , q k -f (x k , j -(19 x k , j , u k , q k 2x k , j ., h 為步長的調(diào)節(jié)系數(shù); s 表示S 的令x k , j

15、 =hs x , j x , j x第j 列, 則F (k S =S =xxx xf i (x k -(f i (x k +hs x , j , u k , q k -hs x , j , u k , q k 2h .(20 第1期同樣地F v (k S v =S x v =(f i (x k , u k , q k +hs v , j -f i (x , u k , q k -hs v , j 2h ,G x (k S x =S y x =范文兵等:一種強(qiáng)跟蹤擴(kuò)展卡爾曼濾波器的改進(jìn)算法狀態(tài)方程x 1k +1=x 1k +s co s (x 3k +t , x 2k +1=x 2k +s si

16、n (x 3k +t ,75(21 x 3k +1=x 3k +t ;k +hs x , j , r k -g i (g i (x x k -hs x , j , r k 2h ,(22輸出方程y 1k +1=x 1k +1+w y 2k +1=x 2k +1+w1k 2k, ,G w (k S w =S yw =k , r k +hs w , j -g i (x k , r k -hs w , j (g i (x 2h .(23y 3k +1=x 3k +1+w 3k .式中s =1. 09×10t 1. 0910-4a (u 1k +v 1k b (u 2k +v 2k ,b (

17、u 2k +v 2k c ,帶次優(yōu)漸消因子的擴(kuò)展卡爾曼濾波器(SFEKF 驗(yàn)前方差估計(jì)、驗(yàn)后增益矩陣和方差估計(jì)修改為P k +1=k (k T =(k +1 F xxv-4b .:k =0. 0, r k =0, 0, 0, Q k =diag (1 12, 1 12 , R k =diag (10-7TTT(k +1 F x (k +F v (k S v S T =x (k v F v (k T (k +1 S x x S T x x +S x v S x v ;, 10-7, 10-7 ,(24K k +1=T T T -1P k G x (k G x (k P k G x (k +G w

18、 (k R k G w (k =T -1T T T -1S x S x (S y =x S x S y x S y x +S yw S yw =0=PP 0x 0=1. 2, -0. 20, 2. 64, 0. 1, 0. 1, 0. 5T ,diag (10-4, 10-4, 10-4, 10-4, 10-4, 10-4 .S x S y ; x S y x S y x +S yw S yw P k +1=T T T -1(25k +1=I -K k +1G x (k PS x S x -T T TK k +1G x (k S x S x =TS x S x -S x S y x K k +

19、1-S x S x -S x S y x K k +1-TTT TT TK k +1S y x S x +S x S y x K k +1=K k +1S y x S x +TTT TT TK k +1S y x S y x K k +1+K k +1S yw S yw K k +1=x -K k +1S y S x K k +1S yw ×T x -K k +1S y S x K k +1S yw .(26從上述推導(dǎo)過程可以看出, 驗(yàn)前驗(yàn)后方差和驗(yàn)后增益矩陣的計(jì)算包含過程噪聲的影響和模型線性化誤差的影響, 而且非線性函數(shù)線性化的步長也隨上一次的驗(yàn)前驗(yàn)后方差、過程噪聲和觀測噪聲變化而

20、變化. 因此, 濾波器實(shí)現(xiàn)簡單, 不需要導(dǎo)數(shù)運(yùn)算, 僅需要函數(shù)的估值, 且具有較高的精度和更好的方差估計(jì), 增加了算法的數(shù)值穩(wěn)定性.4仿真為了闡明強(qiáng)跟蹤有限差分卡爾曼濾波器(ST FD EKF 算法的有效性, 下面以一個(gè)非線性隨機(jī)系統(tǒng)為例, 來仿真比較擴(kuò)展卡爾曼濾波器(EKF 、有限差分?jǐn)U展卡爾曼濾波器(FD EKF 及強(qiáng)跟蹤有限差分濾波器(ST FD EKF 的濾波性能. 假設(shè)非線性狀態(tài)方程和輸出方程表示如下:狀態(tài)估計(jì)過程比較圖1采用3種濾波器對系統(tǒng)x 1k 76控制與決策第21卷假設(shè)濾波器的數(shù)學(xué)模型含有模型失配參數(shù), 用擴(kuò)展?fàn)顟B(tài)方程表示如下(濾波器觀測方程為非線性系統(tǒng)的輸出方程 :x 1k

21、 +1=x 1k +s co s (4x 3k +t ,x 2k +1=x 2k +s sin (5x 3k +t , x 3k +1=x 3 k +t , a k +1=a k , b k +1=b k , c k +1=c k .模型存在誤差(即模型不匹配情況 時(shí)ST FD EKF 也能穩(wěn)定快速跟蹤實(shí)際系統(tǒng)狀態(tài)的變化. 圖2所示為模型待估參數(shù)a , b , c 隨新數(shù)據(jù)加入的變化過程. 圖3(a 表示漸消因子隨觀測數(shù)據(jù)加入的調(diào)整過程. 圖3(b 3(d 表示狀態(tài)估計(jì)均方差值的變化.5結(jié)論基于有限差分強(qiáng)跟蹤濾波器實(shí)際上是一種非線性自適應(yīng)濾波器, 它根據(jù)新的觀測數(shù)據(jù)自動(dòng)調(diào)整漸消因子, 使濾波過

22、程適應(yīng)新數(shù)據(jù)變化; 濾波過程利用有限差分運(yùn)算得到驗(yàn)前、, . 該, 可用于. (References 1Zhou D H , W ang Q L . Strong T rack ing F iltering ofN onlinear System s w ith Co lo red N o ise J . J of B eij ing Institu te of T echnology , 1997, 17(3 :3212326.圖1所示為采用EKF , FD EKF 和ST FD EKF 對系統(tǒng)狀態(tài)x 1k 進(jìn)行估計(jì)的結(jié)果, 其中:實(shí)線為真實(shí)值, 虛線為濾波器的估計(jì)值. 可以看出, EKF

23、濾波估計(jì)是發(fā)散的; 有限差分濾波器(FD EKF 斂的, ; EKF 和FD , 2Schei T S . A F inite 2difference M ethod fo r L inearizati onin N onlinear E sti m ati on A lgo rithm s J . A u to m atica , 1997, 33(11 :205122058. 3N o rgaard M ,Poulsen NK , R avn O .Easy andA ccurate State E sti m ati on fo r N onlinear System s A . 14t

24、h IFA C W orld Conf erence C . Beijing , 1999:3432348.4Park S , L ee J G .D esign of a P ractical T rack ingA lgo rithm w ith R adar M easurem ents J . IE E E T rans on A erosp ace and E lectronic S y ste m s , 1998, 34(4 :13372圖2應(yīng)用STF D EKF 濾波器模型參數(shù)a , b , c 估計(jì)1344.5T adayyon A , Rohani S . Extended