雙層無跡卡爾曼濾波

楊峰 鄭麗濤 王家琦 潘泉

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

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

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

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

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

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

1.1 UT變換

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

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

1.2 UKF算法

考慮典型的非線性系統,其狀態方程和量測方程分別為:

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

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

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

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

4)當粒子退化嚴重時,對粒子進行重采樣.

5)計算每個粒子點x(i)對應的協方差.

6)重復步驟2)～5).

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

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

3.1 DLUKF算法框架

DLUKF算法由外層UKF算法和內層UKF算法組成,其算法流程如下:

內層UKF算法:

量測更新:

在采樣點用內層UKF算法更新后,類似于UPF算法,表示一階矩的權值和表示二階矩的權值的更新可以表示為:

在得到權值更新的基礎上,對權值進行歸一化處理,有

k+1時刻的初始估計值及其協方差可以表示為

外層UKF算法:

不斷重復方程(5)～(25),即可求得DLUKF算法在每個時刻的估計值.

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

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

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

下面主要詳細介紹對稱采樣策略.

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

4 仿真分析

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

4.1 單維仿真

假設有下述狀態空間模型,其狀態方程和量測方程分別可以表示為:

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

通過圖2可以看出,IUKF算法、RUEKF算法、RUCKF算法、高階UKF算法、高階UKF算法和UPF算法濾波估計效果都略好于UKF算法.這是因為IUKF算法、RUEKF算法、RUCKF算法、高階UKF算法、高階UKF算法和UPF算法都對UKF算法進行了改進,所以其效果是好于UKF算法的.本文所提的DLUKF算法在每個時刻的估計效果都好于其他的濾波算法.這說明,DLUKF算法對于UKF算法的改進效果比其他經典算法更加顯著.且因為DLUKF算法用兩層UKF算法對狀態進行估計,所以可以有著很好的濾波估計效果.

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

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

表1 各算法計算時間及RMSE對比分析表Table 1 The calculation time and RMSE of each algorithm

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

4.2 多維仿真

考慮一個二維勻速直線運動的例子,其狀態方程和量測方程分別為:

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

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

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

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

其他的參數設置為:

表2 仿真參數設置Table 2 The Simulation parameters

圖3是各個算法在位置方面的RMSE,可以看出,RUEKF算法、RUCKF算法、高階UKF算法、高階CKF算法和UKF算法的估計效果基本相同,而IUKF算法和UPF算法的估計效果優于UKF算法.而本文算法性能是最好的,這是因為本文算法用帶權值的采樣點表征后驗分布,這比隨機的粒子點表征后驗分布更有優勢,故DLUKF算法的RMSE是好于其他濾波算法的.這也說明所提的DLIKF算法對勻速直線運動可以有著很好的濾波估計效果.

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

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

表3 各個算法的性能Table 3 The performance of each algorithm

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

5 結論

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