雙層無跡卡爾曼濾波

楊峰 鄭麗濤 王家琦 潘泉

狀態(tài)估計(jì)在信號處理、計(jì)算機(jī)視覺、自動(dòng)控制、目標(biāo)跟蹤、導(dǎo)航、金融、通信等領(lǐng)域[1?6]有著廣泛應(yīng)用.在高斯噪聲環(huán)境下,卡爾曼濾波(Kalman fllter,KF)[7]及其次優(yōu)濾波算法可以很好解決該問題.在非高斯噪聲環(huán)境下,KF算法及其次優(yōu)濾波算法不再適用,因此存在著粒子濾波(Particlefilter,PF)[8]及其衍生濾波算法來解決狀態(tài)估計(jì)問題.

基于無跡變換(Unscented transform,UT)的無跡卡爾曼濾波(Unscented Kalman fllter,UKF)[9?11]是一種計(jì)算非線性變換均值和協(xié)方差的次優(yōu)卡爾曼濾波算法.相比于擴(kuò)展卡爾曼濾波(Extended Kalman fllter,EKF),UKF不需要計(jì)算雅可比矩陣,且其可以達(dá)到非線性函數(shù)二階泰勒展開式的精度[9].因此其在導(dǎo)航制導(dǎo)、目標(biāo)跟蹤、信號處理和圖像跟蹤等方面有著很廣泛應(yīng)用.但UKF算法也存在著在某些情況下估計(jì)效果差等問題.

目前,針對UKF算法估計(jì)值不準(zhǔn)確的問題,有眾多改進(jìn)方法.為了解決UKF在工程應(yīng)用中因舍入誤差導(dǎo)致數(shù)值不穩(wěn)定的問題,提出了求根UKF(Square-root unscented Kalman fllter,SRUKF)[12]算法.在加性噪聲條件下,為了降低UKF算法的計(jì)算復(fù)雜度,提出了簡化UKF(Simplified unscented Kalman fllter,SUKF)[13]算法.在先驗(yàn)信息不確定性大而量測精度高的情況下,只用一次量測值的UKF算法的估計(jì)效果較差.因此,提出了多次利用量測值的迭代UKF(Iterated unscented Kalman fllter,IUKF)[14],遞歸更新濾波器(遞歸更新擴(kuò)展卡爾曼濾波(Recursive update extended Kalman fllter,RUEKF)[15]、遞歸更新容積卡爾曼濾波(Recursive update cubature Kalmanfllter,RUCKF)[16])等算法.基于二階UT變換的UKF算法濾波估計(jì)精度只能達(dá)到二階,為了提高濾波精度,提出了基于高階UT變換和高階容積變換(Cubature transform,CT)的高階UKF[17?18]和高階容積卡爾曼濾波(Cubature Kalman fllter,CKF)[19?21]等算法.

UKF及其改進(jìn)算法雖然可以較好處理UKF算法的估計(jì)不準(zhǔn)確的問題,但其仍然存在在非線性程度高的環(huán)境下估計(jì)效果差等問題,文獻(xiàn)[22?23]中提出將UKF算法作為PF算法建議分布,將UKF算法估計(jì)值作為重要性密度函數(shù),這就是無跡粒子濾波(Unscented particle fllter,UPF)[22?23]算法.從理論上講,隨著隨機(jī)采樣粒子數(shù)量提高,UPF算法的精度可以逐漸提高.但UPF算法也存在一些問題,如其運(yùn)算時(shí)間很長,時(shí)效性較差.且UPF算法效果不總是好于UKF算法,在量測噪聲較大時(shí),UPF算法估計(jì)精度會(huì)不如UKF算法.

為了在低計(jì)算負(fù)載的情況下獲得高的濾波估計(jì)精度,本文提出了雙層無跡卡爾曼濾波器(Double layer unscented Kalman filter,DLUKF)算法.其核心思想是用帶有權(quán)值的采樣點(diǎn)表示前一個(gè)時(shí)刻的后驗(yàn)密度函數(shù);而后用內(nèi)層的UKF算法對每個(gè)帶權(quán)值的采樣點(diǎn)進(jìn)行更新,并用最新的量測值對采樣點(diǎn)的權(quán)值進(jìn)行更新;然后將各個(gè)采樣點(diǎn)進(jìn)行加權(quán)融合,得到了初始的估計(jì)值;最后用外層UKF算法的更新機(jī)制對初始估計(jì)值進(jìn)行更新得到最終的估計(jì)值.

1 無跡卡爾曼濾波(UKF)算法

1.1 UT變換

假設(shè)非線性函數(shù)為y=f(x),UT變換是通過近似非線性函數(shù)的概率密度分布來近似非線性函數(shù).其在得到先驗(yàn)均值和協(xié)方差Pxx的基礎(chǔ)上,用采樣策略選取一組確定性采樣點(diǎn)集.而后得到這些采樣點(diǎn)集經(jīng)非線性變換后的采樣點(diǎn)集,進(jìn)而求得經(jīng)非線性變換后的均值和協(xié)方差Pyy.

UT變換算法可以歸納為以下三步:

1.2 UKF算法

考慮典型的非線性系統(tǒng),其狀態(tài)方程和量測方程分別為:

xk為k時(shí)刻n維的狀態(tài)向量,zk+1為k+1時(shí)刻的量測向量.wk為m維的過程噪聲,其服從均值為0方差為Q的高斯分布.vk+1為q維的量測噪聲,其服從均值為0方差為R的高斯分布.濾波算法的目的就是從帶有噪聲的量測值zk+1中估計(jì)出真實(shí)值xk+1.

在實(shí)際應(yīng)用中,受初始誤差的影響,UKF算法存在著收斂速度慢,精度不高等問題.基于此,文獻(xiàn)[14]提出了IUKF算法,文獻(xiàn)[15]提出了RUEKF算法,文獻(xiàn)[16]出了RUCKF算法.這三種算法的核心思想都是多次利用量測值對估計(jì)值進(jìn)行更形,以獲得更好的濾波估計(jì)效果.

2 無跡粒子濾波(UPF)算法

UPF[22?23]算法是在PF算法的基礎(chǔ)上,用UKF算法的濾波估計(jì)值作為PF算法的建議密度函數(shù).這雖然可以解決UKF算法不適用于非高斯環(huán)境等問題,但其由于要選取大量的隨機(jī)性采樣點(diǎn)來逼近密度函數(shù),所以UPF算法會(huì)臨著計(jì)算量大的問題.UPF算法具體步驟如下:

4)當(dāng)粒子退化嚴(yán)重時(shí),對粒子進(jìn)行重采樣.

5)計(jì)算每個(gè)粒子點(diǎn)x(i)對應(yīng)的協(xié)方差.

6)重復(fù)步驟2)～5).

3 雙層無跡卡爾曼濾波(DLUKF)算法

UPF需要用大量的粒子點(diǎn)去逼近狀態(tài)的后驗(yàn)密度函數(shù),因此其有著運(yùn)算量大的問題.本文所提的DLUKF算法用帶權(quán)值的采樣點(diǎn)去表征狀態(tài)的后驗(yàn)密度函數(shù),其核心思想為用內(nèi)層的UKF對每個(gè)帶權(quán)值的采樣點(diǎn)進(jìn)行更新,而后用最新的量測值對每個(gè)采樣點(diǎn)的權(quán)值進(jìn)行更新,并對更新后的采樣點(diǎn)進(jìn)行加權(quán)求和得到下一時(shí)刻初始估計(jì)值,然后將該初始估計(jì)值作為預(yù)測值運(yùn)行外層UKF算法,從而得到最終估計(jì)值.

3.1 DLUKF算法框架

DLUKF算法由外層UKF算法和內(nèi)層UKF算法組成,其算法流程如下:

內(nèi)層UKF算法:

量測更新:

在采樣點(diǎn)用內(nèi)層UKF算法更新后,類似于UPF算法,表示一階矩的權(quán)值和表示二階矩的權(quán)值的更新可以表示為:

在得到權(quán)值更新的基礎(chǔ)上,對權(quán)值進(jìn)行歸一化處理,有

k+1時(shí)刻的初始估計(jì)值及其協(xié)方差可以表示為

外層UKF算法:

不斷重復(fù)方程(5)～(25),即可求得DLUKF算法在每個(gè)時(shí)刻的估計(jì)值.

DLUKF算法的流程圖如圖1所示.

3.2 基于對稱采樣策略的DLUKF算法

根據(jù)選取粒子點(diǎn)的采樣策略不同,又可以得到多種DLUKF算法.在UT變換中,目前的采樣策略方法包括對稱采樣、單形采樣、3階矩偏度采樣和高斯分布4階矩對稱采樣[8]等.還有為了保證經(jīng)過非線性變換后協(xié)方差Pyy的正定性而提出的對基本采樣策略進(jìn)行比例修正的算法框架.

下面主要詳細(xì)介紹對稱采樣策略.

圖1 DLUKF算法流程圖Fig.1 The flow-chart of DLUKF

4 仿真分析

將本文所提的基于對稱采樣策略DLUKF算法與UKF算法、IUKF算法、RUEKF算法、RUCKF算法、高階UKF算法、高階UKF算法和UPF算法分別在一維和二維仿真場景下進(jìn)行仿真對比分析,用濾波算法估計(jì)值與真實(shí)值間的均方根誤差(Root mean square error,RMSE)來表示濾波算法估計(jì)效果.

4.1 單維仿真

假設(shè)有下述狀態(tài)空間模型,其狀態(tài)方程和量測方程分別可以表示為:

式(28)中wk表示過程噪聲,其服從Ga(3,2)的伽馬分布.式(29)中的vk+1表示量測噪聲,其服從均值為0,方差為R=10?5的高斯分布.初始位置為x0=3,IUKF算法,RUEKF算法和RUCKF算法的迭代次數(shù)都為10次.UPF算法粒子數(shù)量為100個(gè),DLUKF算法產(chǎn)生粒子的方法是對稱采樣策略.仿真時(shí)間為30s,蒙特卡洛仿真次數(shù)為100次.其仿真結(jié)果如圖2所示.

通過圖2可以看出,IUKF算法、RUEKF算法、RUCKF算法、高階UKF算法、高階UKF算法和UPF算法濾波估計(jì)效果都略好于UKF算法.這是因?yàn)镮UKF算法、RUEKF算法、RUCKF算法、高階UKF算法、高階UKF算法和UPF算法都對UKF算法進(jìn)行了改進(jìn),所以其效果是好于UKF算法的.本文所提的DLUKF算法在每個(gè)時(shí)刻的估計(jì)效果都好于其他的濾波算法.這說明,DLUKF算法對于UKF算法的改進(jìn)效果比其他經(jīng)典算法更加顯著.且因?yàn)镈LUKF算法用兩層UKF算法對狀態(tài)進(jìn)行估計(jì),所以可以有著很好的濾波估計(jì)效果.

圖2 300次蒙特卡洛仿真的RMSEFig.2 The calculation time and RMSE of each algorithm

將UPF算法的粒子數(shù)由100逐漸增加到500,其與UKF算法、IUKF算法、RUEKF算法、RUCKF算法、高階UKF算法、高階UKF算法和DLUKF算法的單次運(yùn)行時(shí)間以及平均RMSE如表1所示.

表1 各算法計(jì)算時(shí)間及RMSE對比分析表Table 1 The calculation time and RMSE of each algorithm

通過表1可以看出,UKF算法、RUEKF算法、高階UKF算法和高階CKF算法的用時(shí)都很少.IUKF算法、RUCKF和DLUKF算法的用時(shí)略長,這是由于這三種算法都進(jìn)行了多次濾波計(jì)算.UPF算法用時(shí)最長.在UPF算法中,隨著粒子數(shù)目的增多,用時(shí)也是逐漸增加.在RMSE方面,DLUKF算法比另外7種方法小很多.在UPF算法中,隨著粒子數(shù)增多,RMSE也是逐漸變小的.但當(dāng)500個(gè)粒子點(diǎn)時(shí),UPF算法RMSE依然是DLUKF算法的兩倍.這說明了基于帶權(quán)值的采樣點(diǎn)表征后驗(yàn)分布的方法是優(yōu)于隨機(jī)點(diǎn)表征后驗(yàn)分布的.

4.2 多維仿真

考慮一個(gè)二維勻速直線運(yùn)動(dòng)的例子,其狀態(tài)方程和量測方程分別為:

式(31)中,Zk+1=[rk+1,θk+1]T為觀測變量,分別表示對目標(biāo)的徑向距和方位角.vk+1為量測噪聲,其為閃爍噪聲,可以表示為:

量測方程h(·)可以表示為:

式(34)中,R1和R2分別可以表示為

仿真中,仿真時(shí)間為100s,蒙特卡洛仿真次數(shù)為300次.目標(biāo)初始位置為(20000m,40000m),初始速度為(?160m/s,?150m/s).IUKF 算法、RUEKF 算法和RUCKF算法的迭代次數(shù)都為10次.UPF算法粒子數(shù)量為300個(gè),DLUKF算法產(chǎn)生粒子的方法是對稱采樣策略.

其他的參數(shù)設(shè)置為:

表2 仿真參數(shù)設(shè)置Table 2 The Simulation parameters

圖3是各個(gè)算法在位置方面的RMSE,可以看出,RUEKF算法、RUCKF算法、高階UKF算法、高階CKF算法和UKF算法的估計(jì)效果基本相同,而IUKF算法和UPF算法的估計(jì)效果優(yōu)于UKF算法.而本文算法性能是最好的,這是因?yàn)楸疚乃惴ㄓ脦?quán)值的采樣點(diǎn)表征后驗(yàn)分布,這比隨機(jī)的粒子點(diǎn)表征后驗(yàn)分布更有優(yōu)勢,故DLUKF算法的RMSE是好于其他濾波算法的.這也說明所提的DLIKF算法對勻速直線運(yùn)動(dòng)可以有著很好的濾波估計(jì)效果.

圖3 位置的RMSEFig.3 The RMSE of position

在勻速直線運(yùn)動(dòng)中,將UPF算法的粒子數(shù)由300逐漸增加到1000,其與其他算法的單次運(yùn)行時(shí)間以及位置和速度的平均RMSE如表3所示.

表3 各個(gè)算法的性能Table 3 The performance of each algorithm

從表3可以看出,本文算法的運(yùn)算時(shí)間雖然略長于UKF算法、IUKF算法、REUKF算法、RUCKF算法,高階UKF算法和高階CKF算法,卻遠(yuǎn)遠(yuǎn)小于UPF算法.且由于DLUKF算法的外層UKF算法選取了9個(gè)確定性采樣點(diǎn),所以其運(yùn)算時(shí)間是大約是UKF算法的9倍.在UPF算法中,隨著粒子數(shù)目的增多,運(yùn)算時(shí)間也是逐漸增加的.在各個(gè)方面的RMSE、DLUKF算法都是最好的.在UPF算法中,隨著粒子數(shù)目的增多,RMSE也是逐漸減小的,但比起DLUKF算法、UPF算法的RMSE依然是很大的.這說明基于雙層采樣的的DLUKF算法在多維目標(biāo)跟蹤中有著很好的濾波估計(jì)效果.

5 結(jié)論

本文所提的DLUKF算法是在雙層UKF算法的基礎(chǔ)上,用采樣策略選取帶權(quán)值的采樣點(diǎn),而后用內(nèi)層UKF算法對每個(gè)采樣點(diǎn)進(jìn)行更新,同時(shí)用最新的量測對采樣點(diǎn)的權(quán)值進(jìn)行更新,最后通過外層UKF算法的更新機(jī)制得到每個(gè)時(shí)刻的濾波估計(jì)值.仿真結(jié)果表明,在一維和二維的仿真場景中,相比于存在的經(jīng)典算法,本文所提的DLUKF算法可以在較短的時(shí)間內(nèi)獲得很好的濾波估計(jì)效果.