基于Masreliez-Martin的魯棒分?jǐn)?shù)階容積卡爾曼濾波算法及應(yīng)用

穆 靜, 嚴(yán)東升, 蔡遠(yuǎn)利, 王長(zhǎng)元

(1. 西安工業(yè)大學(xué)計(jì)算機(jī)科學(xué)與工程學(xué)院, 陜西 西安 710021; 2. 北京航天長(zhǎng)征飛行器研究所,北京 100076; 3. 西安交通大學(xué)電子與信息學(xué)部, 陜西 西安 710049)

0 引 言

國(guó)內(nèi)外學(xué)者研究了許多非線性系統(tǒng)的免微分濾波算法,如無(wú)跡卡爾曼濾波(unscented Kalman filter, UKF)采用了一組確定Sigma點(diǎn)的Unscented變換[1]、離差差分卡爾曼濾波算法(divided difference Kalman filter, DDKF)利用斯特林插值公式計(jì)算均值和協(xié)方差[2]、容積卡爾曼濾波(cubature Kalman filter, CKF)利用球面徑向容積準(zhǔn)則逼近狀態(tài)后驗(yàn)分布[3]。CKF克服了UKF和DDKF在高維非線性狀態(tài)估計(jì)中的缺點(diǎn),同時(shí)能夠具有更高的濾波精度,獲得了廣泛的應(yīng)用[4-5]。

在實(shí)際應(yīng)用中,被估計(jì)系統(tǒng)的函數(shù)模型和噪聲統(tǒng)計(jì)特性常常不能夠準(zhǔn)確獲得, 直接采用上述的免微分濾波算法會(huì)導(dǎo)致估計(jì)精度大幅度降低。為了提高估計(jì)精度,許多學(xué)者提出了魯棒濾波算法。文獻(xiàn)[6]提出了基于非高斯量測(cè)噪聲下的Huber自適應(yīng)UKF算法,并討論了關(guān)鍵參數(shù)對(duì)算法的影響。文獻(xiàn)[7]提出了基于Huber方法的高階容積卡爾曼跟蹤算法,使用統(tǒng)計(jì)線性回歸模型近似非線性量測(cè)模型的基礎(chǔ)上, 利用Huber M估計(jì)算法實(shí)現(xiàn)狀態(tài)的量測(cè)更新,結(jié)合高階容積準(zhǔn)則構(gòu)成了所提的算法,同時(shí)分析了Huber代價(jià)函數(shù)的調(diào)節(jié)因子對(duì)算法跟蹤性能的影響。文獻(xiàn)[8]研究了基于魯棒CKF的自適應(yīng)目標(biāo)跟蹤算法,借鑒Huber等價(jià)權(quán)函數(shù)的思想, 構(gòu)造了基于平方根平滑逼近函數(shù)的修正因子以抑制觀測(cè)野值的影響, 并結(jié)合CKF器求解框架推導(dǎo)出該算法。文獻(xiàn)[9]討論了基于Huber的魯棒廣義高階CKF算法,采用 Huber方法處理觀測(cè)量,進(jìn)行標(biāo)準(zhǔn)的廣義CKF量測(cè)更新,從而實(shí)現(xiàn)算法的魯棒化,充分利用了容積變換的優(yōu)勢(shì),無(wú)需通過(guò)統(tǒng)計(jì)線性回歸模型對(duì)系統(tǒng)的非線性量測(cè)模型進(jìn)行近似。文獻(xiàn)[10]提出了非高斯噪聲下的非線性隨機(jī)系統(tǒng)的基于Masreliez-Martin(M-M)方法的魯棒擴(kuò)展卡爾曼濾波算法。進(jìn)一步的,文獻(xiàn)[11]提出了一種基于M-M方法的自適應(yīng)和魯棒UKF算法,可以在存在未知過(guò)程噪聲和測(cè)量噪聲協(xié)方差矩陣的情況下提供可靠的狀態(tài)估計(jì)。

另一方面,實(shí)際應(yīng)用中對(duì)有些系統(tǒng)的建模本身需要用分?jǐn)?shù)階來(lái)描述,而有些系統(tǒng)使用分?jǐn)?shù)階模型可以更好建模所研究的系統(tǒng),因此分?jǐn)?shù)階系統(tǒng)成為了越來(lái)越多學(xué)者的研究熱點(diǎn)。針對(duì)高斯環(huán)境下的離散分?jǐn)?shù)階隨機(jī)系統(tǒng),文獻(xiàn)[12]提出了分?jǐn)?shù)階(擴(kuò)展)卡爾曼濾波并應(yīng)用到目標(biāo)的狀態(tài)估計(jì)。文獻(xiàn)[13]以概率論為基礎(chǔ),從分?jǐn)?shù)階的非線性動(dòng)態(tài)系統(tǒng)模型出發(fā),推導(dǎo)了分?jǐn)?shù)階UKF(fractional UKF, FUKF),并應(yīng)用于再入飛行器的狀態(tài)估計(jì)上。文獻(xiàn)[14]使用統(tǒng)計(jì)線性方法和容積變換,提出了分?jǐn)?shù)階CKF(fractional CKF, FCKF)算法,相比于分?jǐn)?shù)階擴(kuò)展卡爾曼濾波算法,仿真實(shí)驗(yàn)證明了該算法的有效性和優(yōu)越性。文獻(xiàn)[15]提出了分?jǐn)?shù)階插值CKF(fractional interpolatory CKF, FICKF)算法,并討論了FUKF和FCKF是FICKF算法的兩種特殊形式。文獻(xiàn)[16]提出了未知先驗(yàn)信息狀態(tài)下的分?jǐn)?shù)階中心差分濾波算法,并且,從貝葉斯濾波的視角分析了分?jǐn)?shù)階非線性系統(tǒng)的狀態(tài)估計(jì)問(wèn)題[17]。而文獻(xiàn)[18-19]分別提出了基于新息的自適應(yīng)分?jǐn)?shù)階卡爾曼濾波算法和基于模糊邏輯方法的FUKF算法。在高斯分布噪聲環(huán)境下,上述方法都獲得了有益的結(jié)果。進(jìn)一步的,一些學(xué)者研究了某些特殊噪聲下分?jǐn)?shù)階系統(tǒng)的狀態(tài)估計(jì)問(wèn)題[20-21]。

針對(duì)連續(xù)分?jǐn)?shù)階系統(tǒng)的狀態(tài)估計(jì)問(wèn)題,文獻(xiàn)[22-23]討論了基于Tustin 生成函數(shù)的分?jǐn)?shù)階卡爾曼濾波器設(shè)計(jì)方法, 解決了相互關(guān)聯(lián)的分?jǐn)?shù)階有色過(guò)程噪聲和分?jǐn)?shù)階有色測(cè)量噪聲下的連續(xù)時(shí)間線性分?jǐn)?shù)階系統(tǒng)的狀態(tài)估計(jì)問(wèn)題。文獻(xiàn)[24]討論了連續(xù)分?jǐn)?shù)階系統(tǒng)的FUKF算法,相比于擴(kuò)展卡爾曼濾波算法,提出的算法具有較準(zhǔn)確的估計(jì)效果。文獻(xiàn)[25-26]討論了使用Tustin生成函數(shù)和分?jǐn)?shù)階導(dǎo)數(shù)平均值方法,提出了針對(duì)具有相關(guān)和非相關(guān)過(guò)程噪聲和量測(cè)噪聲的連續(xù)分?jǐn)?shù)階系統(tǒng)下的分?jǐn)?shù)階擴(kuò)展卡爾曼濾波算法和FUKF算法。進(jìn)一步,文獻(xiàn)[27-28]研究了針對(duì)非線性連續(xù)時(shí)間系統(tǒng)的CKF算法的初始參數(shù)問(wèn)題和未知參數(shù)和分?jǐn)?shù)階問(wèn)題。

本文針對(duì)具有非高斯量測(cè)噪聲的分?jǐn)?shù)階非線性系統(tǒng)的狀態(tài)估計(jì)問(wèn)題進(jìn)行研究,以分?jǐn)?shù)階動(dòng)態(tài)隨機(jī)系統(tǒng)為模型,結(jié)合三階容積原則和M-M方法,改變其量測(cè)更新過(guò)程,建立基于M-M方法的魯棒分?jǐn)?shù)階CKF算法(M-M based robust factional CKF, MMFCKF)。由于分?jǐn)?shù)階微積分的固有特性,FCKF器具有“記憶”特性,其下一時(shí)刻的估計(jì)值不僅與系統(tǒng)當(dāng)前時(shí)刻的狀態(tài)有關(guān),還依賴于當(dāng)前時(shí)刻之前的系統(tǒng)的運(yùn)行情況。在觀測(cè)噪聲污染率較高的情況下能夠有效地抑制污染噪聲的影響, 具有較強(qiáng)抗差能力。最后,將MMFCKF算法應(yīng)用到再入目標(biāo)的狀態(tài)估計(jì)中,同時(shí)分析不同程度的污染噪聲對(duì)算法性能的影響。仿真實(shí)驗(yàn)表明,MMFCKF優(yōu)于FUKF和FCKF,同時(shí)驗(yàn)證了新算法的有效性和魯棒性。

1 分?jǐn)?shù)階非線性系統(tǒng)和容積原則

1.1 分?jǐn)?shù)階非線性隨機(jī)系統(tǒng)

分?jǐn)?shù)階微積分是非整數(shù)階微積分或者是任意階微積分,是整數(shù)階微積分的推廣[29]。自從萊布尼茲等人提出后,數(shù)學(xué)家們從不同角度給出了分?jǐn)?shù)階微積分不同的定義, 每個(gè)定義的合理性和科學(xué)性在實(shí)踐中得到了廣泛檢驗(yàn)。為了易于處理狀態(tài)估計(jì)問(wèn)題,本文使用Grünwald-Letnikov定義[12]:

(1)

(2)

使用式(1),非高斯量測(cè)噪聲的分?jǐn)?shù)階非線性離散隨機(jī)系統(tǒng)[12,15]可表示為

(3)

(4)

zk=h(xk)+ek

(5)

(6)

式中:xk∈Rnx為狀態(tài)向量;f(·)和h(·)為非線性狀態(tài)方程和量測(cè)方程。式(4)計(jì)算機(jī)實(shí)現(xiàn)時(shí)需要使用有限個(gè)過(guò)去狀態(tài)(L),即xk-L+1,xk-L+2,…,xk。zk∈Rm是量測(cè)值,α1,α2,…,αn,i∈{1,2,…,n}是系統(tǒng)的分?jǐn)?shù)階;wk是均值為零和方差為Qk的高斯白噪聲;ek是非高斯的量測(cè)噪聲,其概率分布函數(shù)如下:

p(ek)=(1-)pN(ek)+q(ek)

(7)

式中:pN(ek)服從高斯分布函數(shù)N(ek,0,R1,k),稱為標(biāo)稱量測(cè)噪聲,q(ek)服從N(ek,0,R2,k)是污染噪聲,參數(shù)∈(0,1)代表了污染噪聲對(duì)系統(tǒng)的污染度。記是k時(shí)刻的量測(cè)集合。

1.2 三階容積原則

在貝葉斯框架下,非線性濾波問(wèn)題可歸結(jié)為以高斯概率分布為權(quán)值的非線性函數(shù)的積分問(wèn)題[3]。首先考慮如下標(biāo)準(zhǔn)正態(tài)分布的積分問(wèn)題:

(8)

式中:Integral(g)為所求積分;x為濾波估計(jì)狀態(tài)向量,x∈Rnx,Rnx為積分域;g(x)為非線性函數(shù); N(x;0,I)為標(biāo)準(zhǔn)正態(tài)分布,I為單位矩陣。使用基于三階容積原則的數(shù)值積分方法可得式(8)的近似解[3]:

(9)

式中:ξi為基本容積點(diǎn);ωi為容積點(diǎn)對(duì)應(yīng)的權(quán)值。ξi和ωi的取值分別為

(10)

實(shí)際應(yīng)用中,需要求解權(quán)值為非標(biāo)準(zhǔn)正態(tài)分布的非線性函數(shù)的積分。對(duì)于一個(gè)非標(biāo)準(zhǔn)正態(tài)分布函數(shù)N(x;μ,Σ),μ為均值,Σ為協(xié)方差矩陣,那么:

(11)

(12)

利用式(9),可得到式(12)的近似解:

(13)

2 MMFCKF方法

本文提出的算法使用文獻(xiàn)[12]中的兩個(gè)假設(shè),其合理性是顯然的[15-16]。

2.1 時(shí)間更新

(14)

(15)

(16)

(17)

已知噪聲wk是均值為零的白噪聲,在假設(shè)1下,可得式(16)的第2項(xiàng)和第3項(xiàng):

(18)

(19)

將式(17)～式(19)代入式(16),可得狀態(tài)預(yù)測(cè)為

(20)

(21)

(22)

(23)

(24)

(25)

(26)

2.2 量測(cè)更新

(1) 計(jì)算容積點(diǎn)

(27)

(2) 計(jì)算通過(guò)非線性量測(cè)方程傳播的容積點(diǎn)

γj,k=h(Xj,k)

(28)

(3) 計(jì)算量測(cè)預(yù)測(cè)、新息方差和協(xié)方差估計(jì)

(29)

(30)

(31)

(4) 使用M-M方法,計(jì)算狀態(tài)估計(jì)和協(xié)方差

(32)

(33)

其中,增益Kk和vk定義如下:

(34)

(35)

(36)

(37)

(38)

式中:L為常數(shù)。

3 仿真實(shí)驗(yàn)

為了驗(yàn)證本文所提的MMFCKF的有效性,將算法應(yīng)用到再入彈道目標(biāo)(re-entry ballistic target, RBT)跟蹤問(wèn)題[30-31]中的狀態(tài)估計(jì),并與FUKF和FCKF算法對(duì)比,進(jìn)一步分析量測(cè)污染噪聲因子對(duì)算法的性能影響。

3.1 再入目標(biāo)跟蹤模型

圖1描繪了再入彈道目標(biāo)在一維坐標(biāo)系下雷達(dá)對(duì)再入彈道目標(biāo)的跟蹤過(guò)程。彈體垂直下落,雷達(dá)跟蹤彈體并測(cè)量了彈體和雷達(dá)之間的距離。

圖1 RBT和雷達(dá)的幾何關(guān)系Fig.1 Geometry of RBT and radar

根據(jù)內(nèi)大氣階段的阻力,RBT的狀態(tài)方程[30-31]可以建模為

(39)

式中:狀態(tài)值x=[x1(t),x2(t),x3(t)]T分別表示垂直落體的高度、向下速度和彈道系數(shù),這里取彈道系數(shù)為β=5×10-5。式(39)是連續(xù)時(shí)間模型,對(duì)應(yīng)采樣時(shí)間T的離散化模型f(xk-1)為

x1(k)=x1(k-1)-tx2(k-1)+w1,k-1

(40)

x2(k)=x2(k-1)-te-βx1(k-1)x2(k-1)2x3(k-1)+w2,k-1

(41)

x3(k)=x3(k-1)+w3,k-1

(42)

這里假設(shè)wk=[w1,k,w2,k,w3,k]T是均值為零和協(xié)方差為Qk的高斯噪聲。使用式(40)～式(42)直接表示彈道目標(biāo)的運(yùn)動(dòng)模型時(shí),會(huì)損失掉有用的狀態(tài)信息,使用分?jǐn)?shù)階系統(tǒng)描述系統(tǒng)能獲得更為豐富的信息[32]。定義f(xk)=[x1(k),x2(k),x3(k)]T,再入目標(biāo)的分?jǐn)?shù)階狀態(tài)方程可表示為式(4)的形式。

根據(jù)RBT與雷達(dá)的幾何關(guān)系,雷達(dá)以采樣時(shí)間間隔t測(cè)量與再入目標(biāo)的距離r,再入目標(biāo)在k時(shí)刻相對(duì)雷達(dá)的距離rk由測(cè)量方程給出:

(43)

在實(shí)際應(yīng)用中,ek為非高斯噪聲,往往伴隨高尾分布而出現(xiàn),獨(dú)立于狀態(tài)噪聲wk,表達(dá)式如式(7)所示。M為雷達(dá)到落體路徑的最短水平距離,H為雷達(dá)距離地面的高度。本文取M=100 000 ft,H=100 000 ft[30-31]。

蒙特卡羅仿真實(shí)驗(yàn)中,使用均方根誤差(root mean square error, RMSE)驗(yàn)證算法的有效性。k時(shí)刻的位置RMSE定義為

(44)

為了進(jìn)一步分析算法的有效性,本文使用累加RMSE(accumulated RMSE, ARMSE),位置ARMSE定義為

(45)

式中:N為測(cè)量數(shù)據(jù)點(diǎn)個(gè)數(shù)。用類似的方法定義速度和彈道系數(shù)公式的ARMSE。仿真實(shí)驗(yàn)中蒙特卡羅次數(shù)為100次,使用式(44)和式(45)計(jì)算RMSE和ARMSE。為了更清晰地顯示各種算法的結(jié)果,后文仿真結(jié)果圖中y軸上使用對(duì)數(shù)坐標(biāo)。

3.2 仿真實(shí)驗(yàn)分析

3.2.1 MMFCKF與FUKF, FCKF算法性能比較

標(biāo)稱測(cè)量噪聲協(xié)方差選擇R1,k=1002ft2,污染測(cè)量噪聲協(xié)方差選取R2,k=5002ft2。圖2～圖4分別給出了污染噪聲因子=0.5的MMFEKF,FUKF和FCKF算法的位置、速度和彈道系數(shù)的均方根誤差。

圖2 目標(biāo)位置估計(jì)的RMSEFig.2 RMSE of target position estimation

從圖2中可以看出,相比于FUKF和FCKF,MMFCKF的位置狀態(tài)估計(jì)RMSE大幅度下降,濾波器收斂速度較快。圖3和圖4顯示了MMFCKF的速度和彈道系數(shù)的RMSE從32 s時(shí)開始大幅下降,說(shuō)明MMFCKF對(duì)于狀態(tài)突變具有較強(qiáng)的跟蹤能力,這是由于當(dāng)存在目標(biāo)狀態(tài)突變時(shí),雖然系統(tǒng)模型失準(zhǔn),M-M方法能根據(jù)測(cè)量殘差的變化修正一步預(yù)測(cè)協(xié)方差陣,進(jìn)而在線實(shí)時(shí)調(diào)整增益矩陣Kk,這樣就能充分提取殘差序列中的有效信息,使跟蹤精度有了明顯提高。同時(shí),濾波收斂速度較快,說(shuō)明MMFCKF對(duì)于狀態(tài)突變具有較強(qiáng)的跟蹤能力。

圖3 目標(biāo)速度估計(jì)的RMSEFig.3 RMSEs of target velocity estimation

圖4 目標(biāo)彈道系數(shù)估計(jì)的RMSEFig.4 RMSEs of target ballistic coefficient

使用ARMSE進(jìn)一步對(duì)提出的算法進(jìn)行性能分析,3種算法的ARMSE的柱狀圖如圖5所示。圖5顯示,相比于FUKF和FCKF算法,MMFCKF算法的位置、速度和彈道系數(shù)的ARMSE大幅度減小。相比于FUKF 和FCKF 算法,MMFCKF算法的位置ARMSE分別降低了35.47%和35.22%,速度ARMSE分別降低了20.98%和20.93%,彈道系數(shù)ARMSE分別降低了6.23%和6.06%,再次表明了MMFCKF 具有很強(qiáng)的跟蹤能力。從圖2～圖5也表明了FUKF 和FCKF 算法對(duì)目標(biāo)的跟蹤性能相似。

圖5 不同算法的目標(biāo)ARMSEFig.5 Target ARMSEs of different algorithms

3.2.2 噪聲污染因子對(duì)算法性能的影響

圖6 不同的MMFCKF的目標(biāo)位置RMSEFig.6 Target position RMSEs for MMFCKF with various

圖7 不同的MMFCKF的目標(biāo)速度RMSEFig.7 Target velocity RMSEs for MMFCKF with various

圖8 不同的MMFCKF的目標(biāo)彈道系數(shù)RMSEFig.8 Target ballistic coefficient RMSEs for MMFCKF with various

從圖6和圖7可以看出,隨著污染噪聲因子增加,MMFCKF算法的位置和速度RMSE稍微在增加,但增加時(shí)相對(duì)穩(wěn)定,對(duì)于彈道系數(shù)的RMSE并沒(méi)有隨著污染噪聲因子的增加出現(xiàn)明顯的增加,意味著MMFCKF算法可以有效抑制污染的測(cè)量噪聲,M-M方法可以有效抑制污染噪聲,大大的提高了估計(jì)精度,體現(xiàn)了MMFCKF算法在污染噪聲環(huán)境下對(duì)目標(biāo)跟蹤的優(yōu)越性。

4 結(jié) 論

本文研究了分?jǐn)?shù)階離散非線性系統(tǒng)狀態(tài)估計(jì)問(wèn)題,利用M-M方法對(duì)量測(cè)更新過(guò)程進(jìn)行改進(jìn),構(gòu)造了基于M-M方法的魯棒FCKF器。該算法充分使用了量測(cè)數(shù)據(jù)的有用信息,抑制了量測(cè)噪聲中的污染噪聲對(duì)算法性能的影響,具有很好的穩(wěn)定性。大量仿真結(jié)果表明,與FUKF算法、FCKF算法相比,MMFCKF算法能夠很好地抑制污染噪聲,具有更好的性能。最后,分析了不同程度的量測(cè)污染噪聲對(duì)算法的性能影響,進(jìn)一步驗(yàn)證了算法具有較高的魯棒性。