董祥祥,呂潤妍,蔡云澤

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

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

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

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

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

1 問題描述

考慮如下模型，

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

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

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

2.1 共軛先驗分布和時間更新

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

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

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

(3)

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

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

(4)

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

(5)

通過遺忘因子(ρ)對上述參數(shù)進行時間更新，

(6)

(7)

(8)

(9)

(10)

(11)

式中：nz為量測向量的維度；k|k-1為k-1到k時刻的預測；k-1|k-1為k-1時刻的估計.

(12)

(13)

(14)

(15)

(16)

2.2 基于變分貝葉斯的近似后驗

在先驗分布和時間更新的基礎(chǔ)上，基于變分迭代思想對xk、Rk、κk、φk和σk的近似后驗分布進行推導和求解.

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

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

(17)

KL散度可描述為

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

(18)

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

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

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

(19)

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

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

(20)

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

式中：Ω為待估計參數(shù)xk、Rk、κk、φk和σk的集合；ζ為Ω中的元素；E(·) 為期望操作符；Ω(-ζ)為ζ在Ω中的補集；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)合后驗概率密度函數(shù)為

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

(26)

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

當ζ=xk時，

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

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

(27)

當ζ=Rk時，

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)

當ζ=κk時，

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)

當ζ=φk時，

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

(34)

(35)

(36)

當ζ=σk時，

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

(37)

(38)

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

(39)

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

(40)

(41)

(42)

(43)

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

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

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

(44)

(45)

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

(46)

(49)

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

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

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

(50)

(51)

狀態(tài)更新為

(52)

(53)

(54)

(55)

(56)

(57)

(58)

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

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

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

4 仿真及分析

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

xk=Fxk-1+wk-1

(59)

式中：

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

Qk-1=

假設(shè)傳感器位于坐標系原點，則量測方程可表示為

(60)

式中：

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

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

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

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

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

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

圖3中，VBCKF Pearson算法對sx估計的均方根誤差最小，VBCKF Pearson-r算法次之，VBCKF Pearson-R算法最大.VBCKF Pearson算法對vx的估計誤差均低于20 m/s.且其目標狀態(tài)量估計的均方根誤差均最低.表1中，VBCKF Pearson算法對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%.這是由于量測噪聲的不確定信息不僅包含在尺度矩陣中，還包含在參數(shù)φ、σ中.僅估計尺度矩陣而固定φ和σ，或僅估計φ和σ而固定尺度矩陣都會限制信息的自適應(yīng)更新.所以，同時估計各參數(shù)更利于充分利用量測信息對目標狀態(tài)進行估計.

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

式中：flops3和flops4分別為對數(shù)運算和digamma函數(shù)的浮點運算數(shù).

5 結(jié)語

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