一種新的多漸消因子容積卡爾曼濾波

, ,

(1.西北工業(yè)大學(xué) 電子信息學(xué)院，西安 710129; 2.西北工業(yè)大學(xué) 航空學(xué)院，西安 710072)

0 引言

卡爾曼濾波(Kalman filter, KF)是一種經(jīng)典高效的遞歸濾波方法，克服維納濾波的不足，被應(yīng)用在許多實(shí)際領(lǐng)域中。然而，實(shí)際中大部分系統(tǒng)都是非線性的，不滿足KF中模型為線性的假設(shè)。為此，學(xué)者們提出擴(kuò)展卡爾曼濾波(extended Kalman filter, EKF)、無跡卡爾曼濾波[1](unscented Kalman filter, UKF)、容積卡爾曼濾波[2](cubature Kalman filter, CKF)等非線性濾波算法。UKF和CKF都是確定性采樣濾波算法，將采樣點(diǎn)進(jìn)行傳遞后加權(quán)求和來近似后驗(yàn)均值和協(xié)方差，采樣點(diǎn)數(shù)量由相關(guān)狀態(tài)向量維數(shù)確定，濾波的計(jì)算量也主要由采樣點(diǎn)的數(shù)量決定。

在實(shí)際應(yīng)用中，由于系統(tǒng)的高度復(fù)雜性和強(qiáng)非線性，一階泰勒展開并不能很好的近似實(shí)際系統(tǒng)，模型參數(shù)和過程參數(shù)存在較大差異，導(dǎo)致EKF的濾波精度極大下降，甚至發(fā)散。為此，周東華等人將用于線性系統(tǒng)的最佳漸消因子卡爾曼濾波推廣到EKF中，提出強(qiáng)跟蹤濾波器(Strong Tracking Filter，STF)[3]?；谡辉?，STF在預(yù)測協(xié)方差陣中引入漸消因子，充分利用殘差序列中的有效信息，增強(qiáng)了EKF應(yīng)對(duì)模型不準(zhǔn)確性、狀態(tài)突變的魯棒性。STF目前仍被廣泛的用于諸多領(lǐng)域[4-6]，文獻(xiàn)[4]提出了一種對(duì)稱STF用于感應(yīng)電機(jī)無線傳感器驅(qū)動(dòng)，將多漸消因子引入到預(yù)測協(xié)方差陣，采用Cholesky分解來保證預(yù)測協(xié)方差陣在迭代中的對(duì)稱性，以獲得更好的穩(wěn)定性。文獻(xiàn)[6]認(rèn)為STF相對(duì)于EKF的優(yōu)越性通常是基于仿真和以前的實(shí)驗(yàn)，缺少對(duì)應(yīng)的理論分析，進(jìn)而基于均方誤差、濾波增益等方面對(duì)比分析STF和EKF，最后認(rèn)為傳統(tǒng)卡爾曼濾波理論的可用性受到嚴(yán)重限制，其結(jié)構(gòu)易被破壞，導(dǎo)致濾波性能下降，在工程應(yīng)用中研究使用強(qiáng)跟蹤類濾波器是有重大意義的。

UKF和CKF是現(xiàn)在最具有代表性的非線性濾波方法。與EKF相比，UKF和CKF不需要求解雅克比矩陣，具有更高的泰勒展開逼近精度，逐漸替代EKF，應(yīng)用于各個(gè)領(lǐng)域[7-9]。CKF采用三階向徑容積規(guī)則，數(shù)學(xué)推導(dǎo)嚴(yán)謹(jǐn)，設(shè)計(jì)簡潔，調(diào)節(jié)參數(shù)少，有效克服了UKF中高階非線性時(shí)不穩(wěn)定的問題[10]。同樣地，UKF和CKF也延續(xù)了傳統(tǒng)卡爾曼濾波理論思想的不足，當(dāng)理論模型和實(shí)際模型不匹配時(shí)，會(huì)導(dǎo)致濾波性能下降，故強(qiáng)跟蹤濾波思想在非線性濾波方法中法中同樣適用，引起了眾多學(xué)者的關(guān)注[11-14]。文獻(xiàn)[11]基于預(yù)測協(xié)方差和互協(xié)方差的理論關(guān)系，得到雅克比矩陣在UKF中的等價(jià)求解方法，建立漸消因子在非線性濾波方法中的等效求解方法，避免了強(qiáng)跟蹤UKF在使用中對(duì)雅克比矩陣的依賴，擴(kuò)大了應(yīng)用范圍。這些研究均證明與UKF/CKF相比，強(qiáng)跟蹤UKF/CKF具有更好的魯棒性，獲得更好的濾波性能。文獻(xiàn)[12]基于飛行器姿態(tài)估計(jì)模型中量測函數(shù)的特性，建立新的多重漸消因子求解方法，獲得了比單漸消因子強(qiáng)跟蹤UKF更好的魯棒性和濾波性能。

然而，現(xiàn)有研究雖然嘗試對(duì)多漸消因子的求解方法進(jìn)行提高完善， 總的來說仍然沿用了傳統(tǒng)多重漸消因子的引入方法，只利用了協(xié)方差矩陣中對(duì)角線的信息，即各個(gè)狀態(tài)本身的方差，而沒有考慮各個(gè)變量之間的相關(guān)性。為此，本文提出漸消因子矩陣，建立新的多漸消因子強(qiáng)跟蹤C(jī)KF(multiple fading factors strong tracking cubature Kalman filter,，MSTCKF)，克服原來方法的不足，充分提取協(xié)方差矩陣中的信息，提高漸消因子對(duì)各個(gè)變量保持良好跟蹤的能力。仿真表明，MSTCKF在魯棒性和濾波精度方面均明顯優(yōu)于CKF，具有良好應(yīng)對(duì)狀態(tài)突變的能力。

1 容積卡爾曼濾波

考慮如下非線性離散系統(tǒng)：

xk=f(xk-1)+wk-1

(1)

yk=h(xk)+vk

(2)

其中:xk∈Rn為系統(tǒng)狀態(tài)向量，yk∈Rm為量測向量，n和m為對(duì)應(yīng)維數(shù)；f(·)和h(·)分別為非線性系統(tǒng)的狀態(tài)轉(zhuǎn)移函數(shù)和量測函數(shù)；wk-1∈Rn為系統(tǒng)噪聲，vk∈Rm為量測噪聲，二者均為高斯白噪聲，且互不相關(guān)，協(xié)方差矩陣為Qk和Rk。

CKF算法流程如下：

2)時(shí)間更新

①選取容積點(diǎn)i=(1,2,…l)

(3)

其中:l=2n，其中Sk-1為Pk-1平方根。

②計(jì)算傳遞后容積點(diǎn)

(4)

③計(jì)算k時(shí)刻狀態(tài)預(yù)測值

(5)

④計(jì)算k時(shí)刻未引入漸消因子的預(yù)測誤差協(xié)方差陣平方根

(6)

3)量測更新

①選取容積點(diǎn)i=(1,2,…l)

(7)

②計(jì)算傳遞后容積點(diǎn)

yi,k/k-1=h(Xi,k/k-1)

(8)

③計(jì)算k時(shí)刻觀測預(yù)測值

(9)

④計(jì)算互相關(guān)協(xié)方差陣

Pxy,k/k-1=χk/k-1(Yk/k-1)T

(10)

其中:

(11)

(12)

⑤計(jì)算k時(shí)刻新息協(xié)方差陣

(13)

⑥計(jì)算k時(shí)刻濾波增益陣

Kk=Pxy,k/k-1/Pyy,k/k-1

(14)

⑦計(jì)算k時(shí)刻狀態(tài)估計(jì)值

(15)

⑧計(jì)算k時(shí)刻狀態(tài)誤差協(xié)方差陣

(16)

2 多漸消因子容積卡爾曼濾波

2.1 多漸消因子計(jì)算方法分析

與通常濾波器相比，STF具有以下優(yōu)良的特性：

1)較強(qiáng)的關(guān)于模型不確定性的魯棒性。

2)極強(qiáng)的關(guān)于突變狀態(tài)的跟蹤能力，甚至在系統(tǒng)達(dá)到平穩(wěn)狀態(tài)時(shí)，仍保持對(duì)緩變狀態(tài)與突變狀態(tài)的跟蹤。

3)適中的計(jì)算復(fù)雜度。

STF基于正交原理，在線選取一個(gè)適當(dāng)?shù)臅r(shí)變增益矩陣Kk，使得以下條件成立

(17)

(18)

其中:ek為k時(shí)刻的殘差。式(17)是濾波器性能指標(biāo)，式(18)要求濾波器中不同時(shí)刻的殘差序列保持處處正交，也是正交性原理這一名稱的由來。

為此，STF利用衰減記憶濾波思想，將漸消因子引入預(yù)測誤差協(xié)方差矩陣，進(jìn)而實(shí)時(shí)調(diào)整增益矩陣，強(qiáng)迫濾波殘差滿足式。多漸消因子引入位置具體如下：

(19)

其中:LMDk=diag[λ1,k,λ2,k,…,λn,k]為多重漸消因子，F(xiàn)k為雅可比矩陣。

目前對(duì)LMDk的計(jì)算主要有兩種方法：

(1)基于先驗(yàn)知識(shí)的比例系數(shù)[15]

當(dāng)系統(tǒng)的先驗(yàn)知識(shí)可以大致確定λ1,k:λ2,k:…:λn,k=a1:a2:…:an時(shí)，多重漸消因子計(jì)算方法如下：

(20)

其中:

λ1,k=aiCk

(21)

(22)

(23)

(24)

其中:tr(·)為矩陣求跡運(yùn)算，Hk和Fk分別為對(duì)應(yīng)雅克比矩陣，Vk為實(shí)際輸出殘差序列的協(xié)方差矩陣，估算如下：

(25)

其中是ρ遺忘因子，0<ρ≤1，通常取ρ=0.95。

從式(22)可以看出，該多漸消因子計(jì)算方法實(shí)際上只計(jì)算一個(gè)標(biāo)量漸消因子，然后通過提前設(shè)定的好的比例因子，得出最后的多漸消因子。

該方法的優(yōu)點(diǎn)是計(jì)算簡潔，與單漸消因子類似，不需要對(duì)雅克比矩陣Hk進(jìn)行求逆等運(yùn)算。不足是：

1) 基于先驗(yàn)知識(shí)的比例系數(shù)在實(shí)際應(yīng)用中難以確定，局限了它的使用范圍；

2) 提前確定的比例系數(shù)，實(shí)際上只能對(duì)應(yīng)特定類型的故障，不能很好處理實(shí)際應(yīng)用中可能出現(xiàn)復(fù)雜多變的故障。例如：

λ1,k:λ2,k:…:λn,k=5:1:…:1時(shí)，則與第一個(gè)狀態(tài)相關(guān)的故障更容易被檢測到，而與第一個(gè)狀態(tài)無關(guān)的故障，一定程度上會(huì)被弱化，且求得其他的漸消因子分量也會(huì)小于λ1,k，不符合實(shí)際。

(2)基于部分量測函數(shù)雅克比矩陣信息[12-14]

當(dāng)n≠m時(shí)，若量測矩陣Hk的秩為:

rank(Hk)=min(n,m)

(26)

則有:

(27)

其中:Dk為Hk中不為0的行或者列組成的滿秩矩陣，Jk為LMDkPk/k-1中對(duì)應(yīng)行列形成的滿秩矩陣。則有:

(28)

Dk為滿秩矩陣，故Dk可逆，有:

(29)

令上式中兩邊矩陣對(duì)角線元素相等，則有:

(30)

從而求得:

(31)

該多漸消因子計(jì)算方法的優(yōu)點(diǎn)是避免了對(duì)先驗(yàn)知識(shí)的依賴，降低應(yīng)用限制，使多漸消因子能適用于更多場景。

該方法的不足也很明顯，它能求得漸消因子個(gè)數(shù)為量測矩陣Hk的秩，即min(n,m)，顯然的，當(dāng)n>m時(shí)，并不能為每一個(gè)狀態(tài)提供一個(gè)漸消因子，不能達(dá)到每個(gè)狀態(tài)單獨(dú)優(yōu)化的目的。另一方面，從Hk中選取不為0的行或者列組成的滿秩Dk時(shí)，并沒有給出合理的選取方法，僅當(dāng)Hk中不為0的行或者列恰好為min(n,m)時(shí)，Dk是唯一的。當(dāng)Hk中不為0的行或者列大于min(n,m)時(shí)，Dk并不唯一，不能分辨哪個(gè)Dk是最合理的，也就無法確定多漸消因子與各個(gè)狀態(tài)變量之間的對(duì)應(yīng)關(guān)系，導(dǎo)致不能使用。

(3)基于量測函數(shù)雅克比矩陣信息

與方法(2)相比，該方法不考慮量測矩陣Hk的秩，直接使用量測矩陣Hk的廣義逆，即:

(32)

令:

(33)

(34)

則LMDk計(jì)算如下:

(35)

其中:

(36)

與前面兩種方法相比，該方法即避免了對(duì)先驗(yàn)知識(shí)的依賴，也不存在無法確定Dk的問題，具有更好的實(shí)用性。

綜上，3種多漸消因子求解方法均存在明顯的不足。從它們計(jì)算多漸消因子的過程中也可以看出，它們只利用了協(xié)方差矩陣中對(duì)角線上的信息，并沒有考慮協(xié)方差矩陣中其他元素對(duì)濾波精度和魯棒性的影響，即沒有充分考慮狀態(tài)之間的相關(guān)性，這一定程度上會(huì)導(dǎo)致降低強(qiáng)跟蹤濾波方法的魯棒性和濾波精度，不能充分發(fā)揮多漸消因子對(duì)各個(gè)變量分開修正的優(yōu)勢。

2.2 一種新的多漸消因子求解方法

自從強(qiáng)跟蹤思想提出以來，對(duì)于多漸消因子的定義一直為：

LMDk=diag[λ1,k,λ2,k,…,λn,k]

(37)

其中:LMDk是一個(gè)對(duì)角矩陣。

從LMDk定義可知，LMDk在一開始就沒有考慮各個(gè)狀態(tài)之間的相關(guān)性，認(rèn)為各個(gè)變量是獨(dú)立的。從前面分析的LMDk計(jì)算方法看，均只利用了各個(gè)狀態(tài)本身的方差，而并沒有充分利用各個(gè)狀態(tài)之間的相關(guān)性。

為此，本文提出一種新的多漸消因子定義，將多漸消因子從向量擴(kuò)展為漸消因子矩陣：

MDk=λi,j,i,j=1,2,…n

(38)

則CKF中引入漸消因子矩陣如下式：

(39)

對(duì)于式(18)，在CKF中有：

Fk+1,k(I-Kk+1Hk+1)Fk+1,k(Pxy,k/k-1-KkVk)=0

(40)

即有:

Pxy,k/k-1-KkVk=0

(41)

式成立的充要條件為:

Pyy,k=Vk

(42)

即理論殘差協(xié)方差矩陣和實(shí)際殘差協(xié)方差矩陣相等。將式(39)帶入上式，并進(jìn)行簡化后得:

(43)

當(dāng)Hk不是方陣時(shí)，(Hk)-1為廣義逆矩陣。令:

(44)

(45)

則MDk計(jì)算如下:

(46)

式中,Mk的計(jì)算比較復(fù)雜，結(jié)合式進(jìn)一步簡化得:

(47)

其中:上標(biāo)‘s’為引入漸消因子前的對(duì)應(yīng)變量。

在傳統(tǒng)多漸消因子方法中，需要對(duì)各個(gè)漸消因子的值進(jìn)行判斷，保證漸消因子的值大于1，而在本文提出漸消因子矩陣MDk中，對(duì)角線元素的變化，也會(huì)影響和別的變量之間的相關(guān)系數(shù)，故不在做漸消因子大于1的要求，直接使用求解得到的漸消因子矩陣MDk。

2.3 算法流程

基于式(1)和式(2)所確定的非線性模型，MSTCKF算法流程如下：

3)計(jì)算漸消因子：由式(44)、(45)和(46)計(jì)算得到多漸消因子，帶入式(39)得到引入漸消因子后的預(yù)測誤差協(xié)方差矩陣Pk/k-1。

4)量測更新：由式(7)～式(16)求解得k時(shí)刻狀態(tài)估計(jì)值xk和k時(shí)刻引入漸消因子前的誤差協(xié)方差矩陣Pk。

3 仿真結(jié)果與分析

采用某一強(qiáng)耦合非線性隨機(jī)系統(tǒng)[16]來驗(yàn)證MSTSCKF的有效性，并對(duì)量測方程做了相應(yīng)修改，具體如下：

為驗(yàn)證本文提出的MSTSCKF處理狀態(tài)突變的能力，仿真中采用3種方法進(jìn)行對(duì)比：SCKF、基于第3種多漸消因子求解方法的STSCKF和本文提出的MSTSCKF。

假設(shè)在k=70和k=140時(shí)，由于外部干擾，xk,1分別有+4和-3的突變。3種方法對(duì)4個(gè)狀態(tài)的估計(jì)如圖1～4所示。

從圖1中可知，在突變發(fā)生前，3種方法對(duì)各狀態(tài)的估計(jì)都相當(dāng)準(zhǔn)確，能完美跟蹤狀態(tài)。當(dāng)k=70突變發(fā)生后，SCKF已經(jīng)處于穩(wěn)定狀態(tài)，增益矩陣趨于極小值，失去跟蹤突變狀態(tài)的能力，不能及時(shí)響應(yīng)狀態(tài)突變，需要很長一段時(shí)間才能對(duì)回到正常誤差區(qū)間，應(yīng)對(duì)干擾能力不足，魯棒性較差。STSCKF和MSTSCKF由于采用多漸消因子，對(duì)各個(gè)狀態(tài)分開修正，均能有效的從殘差中提取信息，實(shí)現(xiàn)對(duì)xk,1在k=70和k=140突變狀態(tài)的跟蹤，響應(yīng)速度快，魯棒性好，并沒有因?yàn)閤k,1突變而導(dǎo)致濾波精度的下降。

圖1 3種方法對(duì)xk,1的狀態(tài)估計(jì)

圖2 3種方法對(duì)xk,2的狀態(tài)估計(jì)

圖3 3種方法對(duì)xk,3的狀態(tài)估計(jì)

圖4 3種方法對(duì)xk,4的狀態(tài)估計(jì)

從圖2、圖3和圖4可見，SCKF受xk,1的影響，對(duì)其他三個(gè)狀態(tài)的估計(jì)也很差，產(chǎn)生了一個(gè)明顯的偏差和跳變，且到仿真結(jié)束也沒有收斂到正常值。

在圖1和圖2中，STSCKF和MSTSCKF有近乎一樣的濾波性能，從圖上并不能明顯看出兩者的區(qū)別。而在圖3和圖4中，STSCKF僅考慮各個(gè)狀態(tài)的方差，沒有考慮狀態(tài)間相關(guān)性的不足凸顯出來。STSCKF在突變發(fā)生后，對(duì)xk,3和xk,4的估計(jì)值都產(chǎn)生了一個(gè)明顯的跳躍，且在剩下的時(shí)間里面，估計(jì)值一直沒有重新收斂到理論值。與STSCKF相反，MSTCKF，對(duì)xk,3和xk,4的估計(jì)值在突變前后一直都很穩(wěn)定，并沒有受xk,1狀態(tài)突變的影響。因此，采用漸消因子矩陣的MSTCKF能真正意義上實(shí)現(xiàn)了對(duì)各個(gè)狀態(tài)變量進(jìn)行多通道優(yōu)化，互不影響，有效地保證了濾波器的最優(yōu)性，克服了現(xiàn)有多漸消因子方法沒有考慮狀態(tài)間相關(guān)性的不足。

4 結(jié)束語

通過分析當(dāng)前常用的3種多漸消因子強(qiáng)跟蹤計(jì)算方法優(yōu)缺點(diǎn)，發(fā)現(xiàn)它們均只利用協(xié)方差矩陣中對(duì)角線元素，沒有考慮各個(gè)狀態(tài)之間的相關(guān)性。針對(duì)該問題，本文將多漸消因子從向量擴(kuò)展為漸消因子矩陣，克服了當(dāng)前方法的不足，并根據(jù)正交原理推導(dǎo)建立漸消因子矩陣的求解方法，提出MSTCKF。與傳統(tǒng)多漸消因子不同，漸消因子矩陣由于考慮狀態(tài)之間的相關(guān)性，修改對(duì)角線元素將會(huì)影響其他位置元素，故不再限制漸消因子取值必須大于1。仿真表明，在狀態(tài)突變情況下，MSTCKF的濾波性能和魯棒性優(yōu)于STCKF，能真正實(shí)現(xiàn)各個(gè)狀態(tài)分開估計(jì)，保證其他狀態(tài)的估計(jì)不受突變狀態(tài)的影響。MSTCKF也能被用于信號(hào)處理、目標(biāo)跟蹤、故障檢測、捷聯(lián)慣導(dǎo)等領(lǐng)域中，提高定位跟蹤精度，具有一定的實(shí)際應(yīng)用價(jià)值。