基于修正的自適應平方根容積卡爾曼濾波算法

李春輝, 馬 健, 楊永建,2,*, 肖冰松, 鄧有為, 盛 濤

(1. 空軍工程大學航空工程學院, 陜西 西安 710038; 2. 西北工業大學電子信息學院, 陜西 西安 710072)

0 引 言

近年來,非線性系統確定采樣型濾波算法受到國內外學者廣泛關注并在目標跟蹤領域有著重要的應用[1]。典型的包括無跡卡爾曼濾波(unscented Kalman filter,UKF)、中心差分卡爾曼濾波(central difference Kalman filter,CDKF)、容積卡爾曼濾波(cubature Kalman filter,CKF)等[2-5]。其中,Arasaratnam和Haykin等人提出的CKF算法利用球面徑向容積準則逼近最優估計的狀態后驗分布,CKF不僅能克服UKF在高維和強非線性狀態估計中的缺點同時具有更高的濾波精度。為了進一步提高CKF算法的穩定性和濾波精度,學者將正交三角分解引入CKF,通過直接計算協方差矩陣的平方根,提出了平方根CKF(square-root CKF,SRCKF)。

盡管SRCKF算法性能優良,但是卻無法克服系統模型的不確定性,當目標狀態突變或者出現不良量測時,SRCKF算法跟蹤性能就會下降。主要可以從兩方面解決這一問題,一是對目標機動模型進行再建模,提高模型的自適應性,如文獻[6-7]中分別將自適應當前統計(current statistical, CS)模型以及改進的交互式多模型(interacting multiple model, IMM)與SRCKF結合,有效提高了對機動目標的跟蹤精度。另外一種方法是對濾波算法進行改進以提高濾波器性能,目前改進SRCKF算法主要選擇的是構造強跟蹤濾波器(strong tracking filter, STF),通過引入漸消因子實時調整增益矩陣以彌補較大的預測誤差[8-14]。但這種方法不僅理論推導復雜,求解計算量較大[15-16],而且也存在漸消因子引入位置隨意[17-19]、收斂精度不高等問題。

為了避免上述構造STF時的局限和不足,本文在SRCKF算法的基礎上,通過平衡先驗的預測值和后驗反饋的量測值在濾波中所占的比重,提出基于修正的自適應SRCKF算法。該算法通過設置判定準則和修正準則直接修正狀態預測值或濾波增益,能夠有效減小狀態估計誤差。仿真結果表明,該算法具有在目標狀態突變和量測非線性時的良好濾波性能和數值穩定性,同時相比STF在計算量和收斂速度上具有優勢。

1 目標建模與SRCKF

1.1 目標建模

非線性加性噪聲離散系統可以通過下式表示:

(1)

式中:xk+1∈Rn和zk+1∈Rm分別是k+1時刻的系統狀態向量和量測向量;fk(·)和hk(·)分別是非線性狀態轉移函數和非線性量測函數;wk是系統過程噪聲,vk是量測噪聲,兩者相互獨立且wk～N(0,Qk),vk～N(0,Rk);系統初始狀態x0～N(0,P0)且與wk和vk互不相關。

1.2 SRCKF

為了保持CKF遞推過程中誤差協方差矩陣的正定性和對稱性,SRCKF在濾波時引入QR分解,直接計算協方差矩陣的平方根并進行迭代更新,大大提高了濾波穩定性和數值精度。

通常將QR分解運算定義為S=Tria(A)=RT,其中R是矩陣AT通過QR分解得到的上三角矩陣,則此時S為下三角矩陣。根據文獻[4],SRCKF算法流程可以歸納為如下過程。

1.2.1 時間更新

(1) 分解狀態估計誤差協方差矩陣:

Pk|k=Sk|k(Sk|k)T

(2)

(2) 構造容積點:

(3)

(3) 容積點經非線性傳播并計算狀態量預測值:

(4)

(5)

(6)

(4) 計算預測誤差協方差矩陣的平方根:

(7)

(8)

1.2.2 量測更新

(1) 構造容積點:

(9)

(2) 容積點經非線性傳播并計算量測預測值:

Zi,k+1|k=hk+1(Xi,k+1|k),i=1,2,…,m

(10)

(11)

(3) 計算新息和新息協方差矩陣的平方根:

(12)

Szz,k+1|k=Tria([Zk+1|k,SR])

(13)

式中:SR表示Rk+1的平方根系數,可以通過對Rk+1進行Cho-lesky分解獲得,即SR=Chol(Rk+1);Zk+1|k是加權中心矩陣,表示為

(14)

(4) 計算互協方差矩陣:

(15)

加權中心矩陣Xk+1|k定義為

(16)

(5) 計算濾波增益:

(17)

(6) 狀態估計:

(18)

(7) 計算估計誤差協方差矩陣的平方根:

Sk+1|k+1=Tria([Xk+1|k-Kk+1Zk+1|k,Kk+1SR])

(19)

2 基于修正的SRCKF算法

2.1 誤差分析與修正

在進行非線性濾波時,目標運動建模的不準確是導致濾波精度下降、跟蹤效果惡化甚至跟蹤發散的關鍵原因。目標機動、實際量測的非線性等因素都會使得目標模型與實際不匹配。

主要有兩種方法來克服模型誤差,一是通過改進濾波算法以提高濾波器精度;二是提高模型的準確性和自適應性,如單模型(single model,SM)算法中的CS模型、Singer模型[20]等,以及多模型(multiple model,MM)算法中的交互式多模型[21]、可能模型集(likely model set, LMS)[22]等。

針對系統建模不準確或者目標狀態突變造成的誤差,改進SRCKF算法主要通過構造強跟蹤濾波器引入漸消因子,通過實時調整增益矩陣以彌補較大的預測誤差。但這種改進方法理論推導復雜,求解計算量較大,而且也存在漸消因子引入位置隨意和收斂精度不高等問題。

從式(18)可以看出,狀態估計值由先驗的預測值和量測新息的反饋兩部分組成。

一方面當k時刻目標狀態發生突變或者目標運動模型與實際不匹配時,k+1時刻的狀態預測值和量測預測值會出現較大偏差,進而狀態估計的精度也隨之下降,這體現在k+1時刻較大的新息里。強跟蹤濾波器通過引入漸消因子調整增益矩陣從而使保持濾波殘差序列正交,以此達到對目標實際狀態的跟蹤,其本質上是對式(18)第二項進行修正。實際上可以對第一項即狀態預測值直接進行修正,這樣即使目標狀態發生突變,通過對預測值進行符合實際變化的修正補償,濾波器也能夠實時地“追上”目標狀態,并最終達到提高估計精度的目的。這種修正方法不僅直觀形象而且相比之下計算量減小很多。

另一方面由于量測的非線性以及實際干擾和量測噪聲的存在,量測值不準確的情況也時有發生。當不良量測出現時,新息也會增大,仍然對預測值修正顯然會導致預測誤差進一步增大甚至導致濾波發散。此時可以通過直接修正增益項來實時減小量測值在濾波中的比重。

通過上面的分析可以得出新息的突變決定了是否修正;同時應當設置門限來判定誤差的主要來源,以此決定修正的方式。

2.2 判定準則與修正準則

量測誤差協方差Rk反映了量測值的偏差程度。建模時噪聲為高斯白噪聲,則若以k+1時刻的量測值為圓心,以Rk的均方值為半徑畫一個圓,當沒有出現不良量測等情況,目標k+1時刻的真實狀態值以及量測預測值就在這個量測圓里。同理,可以將修正門限等效為修正圓的半徑,當目標狀態突變或者出現不良量測時,新息的絕對值會增大即量測預測值超出了修正圓,此時需要修正。

圖1是修正判定準則的示意圖,圖中虛線圓是量測圓,實線圓是修正圓,只畫出了四分之一。Rk+1(i,i)為Rk+1對角線上第i個分量,也是對應維度變量的量測誤差。

圖1 修正判定準則Fig.1 Judgement rules of amending

r2是最大的修正圓半徑,是因為實際情況中,一旦出現了不良量測,量測值與狀態真實值以及預測值都相差較多,新息就會劇增;相比之下目標運動狀態變化或者積累誤差造成的建模不準確引起的新息變化要小一些,這是因為加速度的變化傳遞到實際可量測的位置、速度并非瞬時的。

參數α是調節因子,用于平衡量測值和預測值對修正區域的影響。α可以選取在[0.6,0.9]之間,與量測精度相關。調節因子的設置一方面可以有效減小濾波過程中誤差積累帶來的狀態偏差,同時也可以根據實際的系統量測誤差來調整修正區域,避免過度修正。

實際的量測系統通常都能對位置和速度進行量測,因此利用新息對k時刻位置和速度狀態預測值進行直接補償修正,對加速度預測值利用速度修正前后變化量進行補償。因此當量測預測值在r1和r2之間的區域時,采用的修正準則如下:

(20)

為了避免過度修正或者修正補償效果不足,實際選取k1、k2、k3時,一方面要考慮量測系統對應變量的量測精度,另一方面也要根據新息中所提供的變化信息比重,比如一些量測系統得到的新息只包含位置和速度的變化,則對k1和k2的選取可以稍大。

當量測預測值在r2之外的區域時,主要是因為不良量測的影響,直接對增益項進行修正以減小量測值在濾波中的影響。修正準則如下：

(21)

式中:Kk+1(i,i)是增益矩陣對角線上第i個分量;βi是增益調節因子,可以選取在[0.5,0.9]之間,為了避免出現過度修正,選取時保持與量測系統的量測精度成正相關。

參數α和βi的選取范圍是基于實驗得到的,在該范圍內算法的濾波性能可以達到最優。盡管是基于實驗的經驗參數,α和βi的選取也必須把握一個基本原則,即避免對濾波過程產生過度修正。這一原則的主要目的是通過選取α和βi來設置合理的修正范圍,以此平衡先驗的預測值和后驗反饋的量測值在濾波中所占的比重,進而減小狀態估計誤差。比如,在仿真中α設置為小于0.6或者βi設置為小于0.5時,濾波精度會明顯降低甚至濾波發散,這就是對預測值或者量測值過度修正的結果。

不論是在哪一個修正區域,修正之后都會對濾波估計誤差協方差造成影響。為了避免狀態估計穩態精度受到較大影響,在上述兩種方式的修正之后對估計誤差協方差矩陣平方根進行補償,公式如下:

(22)

2.3 算法流程

圖2是算法流程示意圖。具體步驟如下。

圖2 算法流程圖Fig.2 Algorithm flow chart

步驟 1由式(2)～式(12)求出k+1時刻的新息;

步驟 2由修正判定準則判定是否進行修正以及量測預測值處于哪一修正區域內,若不需要修正則按式(13)～式(19)繼續進行濾波;

步驟 3若需要對預測值進行修正,則按式(20)進行修正后,再進行式(6)～式(19)的濾波過程,并用式(22)對估計誤差協方差進行補償;

步驟 4若需要對增益進行修正,則進行式(13)～式(17)求得增益后,按式(20)進行修正,之后完成濾波過程并用式(22)對估計誤差協方差進行補償。

可見相比較SRCKF算法,本文通過增加新息比較以及直接修正補償使得算法自適應能力得到有效改善,同時相比較STF計算量有明顯的減小。而且依據實際量測系統量測精度和新息情況對修正參數進行調節后,濾波性能可以得到有效保證。

3 仿真驗證與分析

仿真采用500次蒙特卡羅實驗,并以目標位置、速度、加速度均方根誤差(root mean square error, RMSE)為評價指標,定義為

(23)

下面針對目標模型的不同對本文算法的有效性和優劣性進行驗證分析。

3.1 目標狀態突變

該仿真中目標初始狀態x0=[0,10,0]T,在前20 s加速度為0 m/s2,即目標做勻速運動,在21～40 s目標做勻加速運動,加速度為30 m/s2,在41～80 s目標做勻減速運動,加速度變為-20 m/s2,在81～100 s又恢復成加速度為30 m/s2的勻加速運動。采樣間隔為1 s,持續時間為100 s。系統噪聲協方差Q=diag[1,1,1],量測數據只有位置這一維,量測值噪聲協方差R=100 m2。初始協方差P0=diag[8,3,3],初始狀態估計值和目標初始狀態取一致。

文獻[8]在STF框架上采用自適應SRCKF構建了強跟蹤自適應SRCKF,在系統存在模型不確定性時跟蹤性能較好。表1是兩種算法采樣時間內的RMSE和平均耗時對比。圖3～圖5是本文算法和文獻[8]中算法進行目標跟蹤時位置、速度、加速度RMSE的對比。

表1 算法性能對比

圖3 位置RMSEFig.3 RMSE of position

圖4 速度RMSEFig.4 RMSE of velocity

圖5 加速度RMSEFig.5 RMSE of acceleration

由表1和圖3～圖5可以看出,兩種算法穩態跟蹤精度相差不大,但本文算法平均跟蹤精度要更優;從位置和速度平均RMSE及圖3、圖4來看,在目標狀態發生突變時刻即20 s、40 s、80 s時,本文算法在峰值誤差和收斂速度上要更優;從加速度平均RMSE及圖5來看,兩種算法區別不大,但是本文算法在誤差收斂速度上更優。從算法平均用時來看,本文算法避免求解漸消因子而直接進行修正,在計算量和算法耗時上有較大優勢。之所以加速度修正效果不明顯主要是因為量測數據僅有位置一維,新息中體現出的速度和加速度變化的信息較少,通過位置的變化傳遞到速度再傳遞到加速度修正效果不好。

3.2 量測非線性目標跟蹤

量測的非線性以及較大的量測誤差都很有可能使得不良量測出現,通過檢驗算法非線性濾波性能可以分析其數值穩定性和動態目標跟蹤能力。

角測量跟蹤(bearing-only tracking,BOT)模型是一個二維動態目標跟蹤模型,也是檢驗非線性濾波算法的一種常用模型[23-24]。

仿真中模型參數設置、初始狀態設置以及誤差定義同文獻[23]。圖6是目標真實軌跡和兩種算法跟蹤濾波軌跡對比。圖7～圖8是本文算法和文獻[23]中算法進行目標跟蹤時位置、速度均方根誤差的對比。從圖6可以看出,相比于文獻[23]中的算法,本文算法在穩定跟蹤后能更好地貼合實際軌跡。從圖7和圖8可以看出本文算法在位置、速度跟蹤精度以及濾波數值穩定性上都具有優勢,對于非線性量測系統有較好的跟蹤性能。

圖6 目標濾波軌跡Fig.6 Target filtering trajectory

圖7 位置跟蹤RMSEFig.7 RMSE of position tracking

圖8 速度跟蹤RMSEFig.8 RMSE of velocity tracking

4 結 論

目標機動、不良量測等因素都會使得目標模型與實際不匹配,從而導致濾波精度下降,濾波算法失效。本文在SRCKF算法的基礎上首先分析了誤差的主要來源,并提出了修正的思路,本質上就是通過直接修正狀態預測值或者濾波增益來平衡先驗的預測值和后驗反饋的量測值在濾波中所占的比重進而減小狀態估計誤差。

之后本文設置了與實際系統量測精度密切相關的修正判定準則和修正準則,構成了基于修正的自適應SRCKF算法。仿真結果表明,在目標狀態突變時,本文提出的算法相比于強跟蹤SRCKF算法有著更高的濾波精度和誤差收斂速度,而且在計算量和計算時間方面也有明顯優勢;同時量測非線性目標跟蹤的仿真也表明本文算法具有較強的非線性系統濾波數值穩定性。