董祥祥,呂潤(rùn)妍,蔡云澤

(上海交通大學(xué) 自動(dòng)化系；系統(tǒng)控制與信息處理教育部重點(diǎn)實(shí)驗(yàn)室；海洋智能裝備與系統(tǒng)集成技術(shù)教育部實(shí)驗(yàn)室，上海 200240)

卡爾曼濾波及其改進(jìn)算法由于濾波性能良好而應(yīng)用廣泛[1-3]，其效果主要取決于噪聲統(tǒng)計(jì)的先驗(yàn)特性，且受限于正態(tài)分布的量測(cè)噪聲統(tǒng)計(jì)[4].在進(jìn)行目標(biāo)跟蹤時(shí)，傳感器的不可靠因素常誘導(dǎo)出量測(cè)野值，導(dǎo)致離均值較遠(yuǎn)處的量測(cè)噪聲的不確定性增大，呈現(xiàn) “厚尾特性”[5].而傳統(tǒng)的卡爾曼濾波及其改進(jìn)算法使用標(biāo)準(zhǔn)高斯分布，無(wú)法準(zhǔn)確描述實(shí)際的厚尾噪聲，存在估計(jì)誤差過(guò)大的問(wèn)題.對(duì)此，學(xué)者們進(jìn)行了大量研究，提出了廣義最大似然估計(jì)法[6]、多模型方法[7]和自適應(yīng)聯(lián)合估計(jì)算法[8].

廣義最大似然估計(jì)方法利用量測(cè)中的高階統(tǒng)計(jì)信息對(duì)狀態(tài)進(jìn)行估計(jì)，相比于卡爾曼濾波使用的二階統(tǒng)計(jì)信息，其可提高量測(cè)非高斯條件下的狀態(tài)估計(jì)精度，但該方法沒(méi)有考慮量測(cè)噪聲的厚尾特性，從而忽略了部分先驗(yàn)信息，限制了狀態(tài)估計(jì)精度的提高[9].多模型方法利用不同模式的有限高斯分布之和對(duì)特殊形式的非高斯厚尾噪聲進(jìn)行建模，進(jìn)而通過(guò)加權(quán)高斯和計(jì)算狀態(tài)后驗(yàn)概率密度函數(shù)，但該方法受限于模型集的選擇方式和模型數(shù)量[10].基于自適應(yīng)聯(lián)合估計(jì)的期望最大化方法利用E-Step和M-Step步驟對(duì)目標(biāo)狀態(tài)和隱變量進(jìn)行辨識(shí)和估計(jì)[11]，但其估計(jì)效果受隱變量維數(shù)的影響較大，不利于期望極大化估計(jì)[12].

針對(duì)以上問(wèn)題，Piché等[13]采用多變量Student’s t分布對(duì)厚尾噪聲進(jìn)行建模，并利用變分貝葉斯理論求解狀態(tài)的后驗(yàn)分布.但該方法假設(shè)Student’s t分布的尺度矩陣和自由度參數(shù)固定不變，令算法不能自適應(yīng)調(diào)整量測(cè)噪聲的統(tǒng)計(jì)特性，易降低濾波器性能.Yun等[14]提出利用Pearson Type VII 分布描述厚尾噪聲，并據(jù)此描述包含在正常量測(cè)噪聲中的隨機(jī)離群厚尾噪聲.在測(cè)量建模中引入遵循β-Bernoulli 分布的判斷因子，利用變分貝葉斯理論實(shí)現(xiàn)狀態(tài)的自適應(yīng)魯棒估計(jì).Pearson Type VII 分布不受“雙自由度參數(shù)必須相等”的約束，因此對(duì)隨機(jī)厚尾噪聲的適應(yīng)性更強(qiáng)，在厚尾噪聲估計(jì)中具有更好的魯棒性[15].

為了解決在尺度矩陣和自由度參數(shù)固定不變的約束下，傳統(tǒng)魯棒濾波難以適應(yīng)復(fù)雜時(shí)變的厚尾噪聲的問(wèn)題，本文以容積卡爾曼濾波器為框架，研究一種基于Pearson Type VII 分布的自適應(yīng)濾波算法 (VBCKF Pearson).基于貝葉斯估計(jì)理論，選擇Pearson Type VII 分布作為不確定厚尾噪聲的共軛先驗(yàn)分布；建立基于目標(biāo)狀態(tài)、尺度矩陣、雙自由度參數(shù)、輔助參數(shù)的聯(lián)合后驗(yàn)概率密度函數(shù)，利用變分貝葉斯方法，求解聯(lián)合后驗(yàn)分布中待估計(jì)參數(shù)的變分近似分布；在濾波更新中，利用容積采樣規(guī)則，更新非線性函數(shù)的傳遞和新息協(xié)方差矩陣，實(shí)現(xiàn)對(duì)目標(biāo)狀態(tài)和不確定厚尾噪聲的聯(lián)合估計(jì).

1 問(wèn)題描述

考慮如下模型，

式中：k為采樣時(shí)刻；x、f(·)和w分別為狀態(tài)向量、狀態(tài)轉(zhuǎn)移函數(shù)和過(guò)程噪聲，且w服從均值為0，協(xié)方差陣為Q的高斯分布，記作w～N(w;0,Q)；z、h(·)分別為量測(cè)向量和觀測(cè)函數(shù)；v為不確定厚尾噪聲.

目前，針對(duì)厚尾噪聲的魯棒濾波算法假設(shè)量測(cè)噪聲的尺度矩陣和自由度參數(shù)精確已知且不隨時(shí)間變化，濾波結(jié)果依賴(lài)于給定的參數(shù)，且根據(jù)經(jīng)驗(yàn)選取尺度矩陣和自由度參數(shù)存在一定的盲目性.對(duì)此，本文基于式(1)和(2)的模型，利用Pearson Type VII 分布和變分貝葉斯方法，將魯棒濾波固定自由度參數(shù)的估計(jì)轉(zhuǎn)化為Pearson Type VII 分布雙自由度參數(shù)的估計(jì)，并對(duì)尺度矩陣和雙自由度參數(shù)進(jìn)行自適應(yīng)聯(lián)合估計(jì).選擇容積卡爾曼濾波器(CKF)為基礎(chǔ)濾波器，具體算法見(jiàn)文獻(xiàn)[16].

2 不確定厚尾噪聲條件下的狀態(tài)估計(jì)

2.1 共軛先驗(yàn)分布和時(shí)間更新

選擇Pearson Type VII 分布作為未知量測(cè)噪聲的先驗(yàn)分布.在此基礎(chǔ)上，通過(guò)遺忘因子對(duì)噪聲參數(shù)進(jìn)行時(shí)間更新.

Pearson Type VII 分布可被描述為高斯分布與Gamma分布的卷積[17]，其定義如下：

P(θ|ε,R,φ,σ)=

(3)

式中：θ為統(tǒng)計(jì)量；ε為均值；R為尺度矩陣；φ和σ為雙自由度參數(shù)，且當(dāng)φ=σ時(shí)，Pearson Type VII 分布即為Student’s t分布；N(·)為高斯分布；G(·)為Gamma分布；κ為輔助參數(shù).P(θ|ε,R,φ,σ)表示θ服從于參數(shù)為ε、R、φ、σ的Pearson Type VII 分布.考慮量測(cè)噪聲為不確定厚尾噪聲，可選擇Pearson Type VII 分布作為先驗(yàn)分布，表示為

p(vk)=P(vk|0,Rk,φk,σk)=

(4)

考慮Rk、κk、φk和σk的不確定性與時(shí)變性，分別選擇inverse Wishart分布和Gamma分布作為先驗(yàn)分布，表示為

(5)

通過(guò)遺忘因子(ρ)對(duì)上述參數(shù)進(jìn)行時(shí)間更新，

(6)

(7)

(8)

(9)

(10)

(11)

式中：nz為量測(cè)向量的維度；k|k-1為k-1到k時(shí)刻的預(yù)測(cè)；k-1|k-1為k-1時(shí)刻的估計(jì).

(12)

(13)

(14)

(15)

(16)

2.2 基于變分貝葉斯的近似后驗(yàn)

在先驗(yàn)分布和時(shí)間更新的基礎(chǔ)上，基于變分迭代思想對(duì)xk、Rk、κk、φk和σk的近似后驗(yàn)分布進(jìn)行推導(dǎo)和求解.

在非線性系統(tǒng)中，系統(tǒng)狀態(tài)和待估計(jì)參數(shù)的聯(lián)合后驗(yàn)估計(jì)通常難以得到解析解.對(duì)此，可以利用變分貝葉斯方法得到近似后驗(yàn)分布.變分貝葉斯方法通過(guò)尋找真實(shí)分布(p(·))的近似分布(q(·))，使兩者的相對(duì)熵(KL散度)取最小值[6]，即

{q(·)}=arg min KLD(q(·)‖p(·))

(17)

KL散度可描述為

KLD(q(·)‖p(·))≡

(18)

在不確定厚尾噪聲條件下，系統(tǒng)狀態(tài)向量和尺度矩陣、雙自由度參數(shù)以及輔助參數(shù)的聯(lián)合后驗(yàn)分布及其近似分布可表示為

p(xk,Rk,φk,σk,κk|z1∶k)≈

q(xk)q(Rk)q(κk)q(φk)q(σk)

(19)

式中：1∶k表示時(shí)刻1到k；q(xk)、q(Rk)、q(κk)、q(φk)、q(σk) 分別為xk、Rk、κk、φk、σk的近似后驗(yàn)分布.則問(wèn)題等價(jià)于

logq(ζ)=EΩ(-ζ)(logp(Ω,z1∶k))+cζ

(20)

Ω≡{xk,Rk,κk,φk,σk}

式中：Ω為待估計(jì)參數(shù)xk、Rk、κk、φk和σk的集合；ζ為Ω中的元素；E(·) 為期望操作符；Ω(-ζ)為ζ在Ω中的補(bǔ)集；cζ為與ζ有關(guān)的常數(shù).則有

logq(xk)=cxk+

ERk,κk,φk,σk(logp(xk,Rk,κk,φk,σk,z1∶k))

(21)

logq(Rk)=cRk+

Exk,κk,φk,σk(logp(xk,Rk,κk,φk,σk,z1∶k))

(22)

logq(κk)=cκk+

Exk,Rk,φk,σk(logp(xk,Rk,κk,φk,σk,z1∶k))

(23)

logq(φk)=cφk+

Exk,κk,Rk,σk(logp(xk,Rk,κk,φk,σk,z1∶k))

(24)

logq(σk)=cσk+

Exk,κk,Rk,φk(logp(xk,Rk,κk,φk,σk,z1∶k))

(25)

聯(lián)合后驗(yàn)概率密度函數(shù)為

p(Ω,z1∶k)=p(xk,Rk,κk,φk,σk,z1∶k)=

(26)

在每次的量測(cè)更新過(guò)程中進(jìn)行變分迭代循環(huán)(j=0,1,2…,N′-1)，N′為變分迭代次數(shù).各參數(shù)估計(jì)如下.

當(dāng)ζ=xk時(shí)，

logq(j+1)(ζ)=logq(j+1)(xk)=

0.5E(j)(κk)(zk-h(xk))T×

(27)

當(dāng)ζ=Rk時(shí)，

logq(j+1)(ζ)=logq(j+1)(Rk)=

E[(zk-h(xk))(zk-h(xk))T]+cRk≈

(28)

(29)

E(j)(κk)E(j)[(zk-h(xk))(zk-h(xk))T]

(30)

當(dāng)ζ=κk時(shí)，

logq(j+1)(ζ)=logq(j+1)(κk)=

E(j)[(zk-h(xk))(zk-h(xk))T]≈

E(i)[(zk-h(xk))(zk-h(xk))T]}κk

(31)

(32)

h(xk))(zk-h(xk))T]}+0.5E(j)(σk)

(33)

當(dāng)ζ=φk時(shí)，

logq(j+1)(ζ)=logq(j+1)(φk)=

(34)

(35)

(36)

當(dāng)ζ=σk時(shí)，

logq(j+1)(ζ)=logq(j+1)(σk)

(37)

(38)

0.5log(E(j)(φk))

(39)

根據(jù)各估計(jì)參數(shù)的后驗(yàn)近似分布，有

(40)

(41)

(42)

(43)

3 基于Pearson Type VII 分布的不確定厚尾噪聲濾波

圖1 算法流程圖Fig.1 Flowchart of algorithm

步驟2根據(jù)式(6)～(11)，通過(guò)遺忘因子對(duì)不確定厚尾噪聲各參數(shù)進(jìn)行時(shí)間更新.

(44)

(45)

Zi,k|k-1=h(Xi,k|k-1)

(46)

(49)

式中：Zi,k|k-1為將容積采樣點(diǎn)Xi,k|k-1經(jīng)量測(cè)方程傳遞后得到的量測(cè)預(yù)測(cè)值.

步驟4變分迭代.包括初始化變分參數(shù)和變分迭代循環(huán)(分為修正量測(cè)噪聲協(xié)方差矩陣和量測(cè)誤差協(xié)方差矩陣更新、狀態(tài)更新、變分參數(shù)更新).其中，初始化變分參數(shù)包括

修正量測(cè)噪聲協(xié)方差矩陣和量測(cè)誤差協(xié)方差矩陣更新為

(50)

(51)

狀態(tài)更新為

(52)

(53)

(54)

(55)

(56)

(57)

(58)

尺度矩陣的分布參數(shù)更新見(jiàn)式(29)～(30)，雙自由度參數(shù)的分布參數(shù)更新分別見(jiàn)式(35)～(36)和式(38)～(39)，輔助參數(shù)分布參數(shù)更新見(jiàn)式(32)～(33)，各待估計(jì)參數(shù)期望更新見(jiàn)式(40)～(43).

步驟5狀態(tài)估計(jì)、誤差協(xié)方差矩陣和分布參數(shù)更新.

與魯棒容積卡爾曼(Robust CKF)算法相比，本文提出的VBCKF-Pearson算法可以實(shí)現(xiàn)尺度矩陣和雙自由度參數(shù)的同時(shí)估計(jì).現(xiàn)有Robust CKF算法假設(shè)尺度矩陣和自由度參數(shù)固定不變，根據(jù)經(jīng)驗(yàn)選取參數(shù)存在一定的盲目性.本文算法選擇Pearson Type VII 分布作為共軛先驗(yàn)，動(dòng)態(tài)尺度矩陣描述了不確定量測(cè)噪聲的矩陣參數(shù)，動(dòng)態(tài)雙自由度參數(shù)反映了不確定尺度矩陣和雙自由度參數(shù)的自適應(yīng)性.在相鄰2個(gè)時(shí)刻的濾波過(guò)程中，Rk、φk、σk和κk的后驗(yàn)分布中的參數(shù)利用k時(shí)刻含厚尾噪聲的量測(cè)進(jìn)行更新；在一次濾波更新變分迭代中，Rk、κk、φk和σk相互耦合，其后驗(yàn)分布中的參數(shù)經(jīng)式(29)～(44)不斷被更新至近似分布與真實(shí)分布的KL散度最小，實(shí)現(xiàn)Rk、κk、φk和σk的同時(shí)估計(jì).

4 仿真及分析

仿真系統(tǒng)為高維非線性目標(biāo)跟蹤系統(tǒng)，其目標(biāo)在笛卡爾Oxy坐標(biāo)系內(nèi)運(yùn)動(dòng).狀態(tài)向量x=[sxvxsyvyω]T，sx和sy分別為x、y方向的位置，vx和vy分別為x、y方向的速度，ω為轉(zhuǎn)彎速率，取 -π/60，T為采樣周期，取1 s.目標(biāo)運(yùn)動(dòng)方程[16]為

xk=Fxk-1+wk-1

(59)

式中：

wk-1～N(0,Qk-1)

Qk-1=

假設(shè)傳感器位于坐標(biāo)系原點(diǎn)，則量測(cè)方程可表示為

(60)

式中：

vk的協(xié)方差矩陣取Σk和100Σk的概率(p)分別為0.9和0.1.初始時(shí)刻的狀態(tài)向量和狀態(tài)誤差協(xié)方差矩陣分別為

x0=[1 000 300 1 000 0 -π/60]

進(jìn)行100次蒙特卡洛仿真，每次仿真時(shí)長(zhǎng)(t)為100 s，變分迭代次數(shù)取N′=10.

仿真1濾波精度比較.比較Robust CKF和VBCKF Pearson算法的平均均方根誤差(MRMSE)，如表1所示，以及狀態(tài)估計(jì)效果和均方根誤差(RMSE)，如圖2所示.圖中，VBCKF Pearson算法對(duì)各狀態(tài)量的估計(jì)均方根誤差均低于Robust CKF算法.表中，VBCKF Pearson算法對(duì)估計(jì)的sx、vx、sy、vy和ω的平均均方根誤差比Robust CKF算法分別低22.2%、12.1%、17.6%、6.5%和2.4%.結(jié)果表明，在不確定厚尾噪聲條件下，本文算法對(duì)目標(biāo)的跟蹤精度更高.

表1 平均均方根誤差Tab.1 Mean of root mean square error

圖2 Robust CKF和VBCKF Pearson算法的均方根誤差Fig.2 RMSE of Robust CKF and VBCKF Pearson algorithms

仿真2參數(shù)分析.為了進(jìn)一步研究VBCKF Pearson算法的性能，對(duì)比了3種算法對(duì)目標(biāo)狀態(tài)的估計(jì)效果和均方根誤差，如圖3所示，以及各算法的平均均方根誤差，已在表1中展示.其中，VBCKF Pearson-R算法固定參數(shù)φ、σ并同時(shí)估計(jì)尺度矩陣和狀態(tài)量，VBCKF Pearson-r算法固定尺度矩陣并同時(shí)估計(jì)狀態(tài)量和φ、σ，VBCKF Pearson算法同時(shí)估計(jì)狀態(tài)量、尺度矩陣和參數(shù)φ、σ.

圖3 不同VBCKF Pearson算法的均方根誤差Fig.3 RMSE of different VBCKF Pearson algorithms

圖3中，VBCKF Pearson算法對(duì)sx估計(jì)的均方根誤差最小，VBCKF Pearson-r算法次之，VBCKF Pearson-R算法最大.VBCKF Pearson算法對(duì)vx的估計(jì)誤差均低于20 m/s.且其目標(biāo)狀態(tài)量估計(jì)的均方根誤差均最低.表1中，VBCKF Pearson算法對(duì)sx、vx、sy、vy和ω的平均均方根誤差比VBCKF Pearson-R算法分別低33.6%、21.4%、33.9%、19.8%和7.1%，比VBCKF Pearson-r算法分別低23.6%、17.7%、24.6%、15.1%和6.1%.這是由于量測(cè)噪聲的不確定信息不僅包含在尺度矩陣中，還包含在參數(shù)φ、σ中.僅估計(jì)尺度矩陣而固定φ和σ，或僅估計(jì)φ和σ而固定尺度矩陣都會(huì)限制信息的自適應(yīng)更新.所以，同時(shí)估計(jì)各參數(shù)更利于充分利用量測(cè)信息對(duì)目標(biāo)狀態(tài)進(jìn)行估計(jì).

仿真3計(jì)算復(fù)雜度分析.采用浮點(diǎn)運(yùn)算數(shù) (flops)進(jìn)行計(jì)算復(fù)雜度分析[18].Robust CKF和VBCKF Pearson算法的浮點(diǎn)運(yùn)算數(shù)分別為

式中：flops3和flops4分別為對(duì)數(shù)運(yùn)算和digamma函數(shù)的浮點(diǎn)運(yùn)算數(shù).

5 結(jié)語(yǔ)

提出一種不確定厚尾噪聲條件下的自適應(yīng)濾波算法.針對(duì)傳統(tǒng)魯棒容積卡爾曼濾波器無(wú)法估計(jì)時(shí)變不準(zhǔn)確的尺度矩陣和自由度參數(shù)的問(wèn)題，選擇Pearson Type VII 分布對(duì)不確定厚尾噪聲進(jìn)行建模，并分別選擇inverse Wishart和Gamma分布作為Pearson Type VII 分布參數(shù)的先驗(yàn)分布.在此基礎(chǔ)上，利用變分貝葉斯方法，實(shí)現(xiàn)狀態(tài)向量、雙自由度參數(shù)、輔助參數(shù)和尺度矩陣的聯(lián)合估計(jì).仿真結(jié)果表明，在不確定厚尾噪聲條件下，本文算法的濾波精度高于傳統(tǒng)魯棒容積卡爾曼濾波算法.