帶測(cè)量偏置估計(jì)的魯棒卡爾曼濾波算法

朱光明,蔣榮欣,周 凡,田 翔,陳耀武

（1.浙江大學(xué)數(shù)字技術(shù)及儀器研究所,浙江杭州310027；2.浙江省網(wǎng)絡(luò)多媒體技術(shù)研究重點(diǎn)實(shí)驗(yàn)室,浙江杭州310027）

帶測(cè)量偏置估計(jì)的魯棒卡爾曼濾波算法

朱光明1,2,蔣榮欣1,2,周 凡1,2,田 翔1,2,陳耀武1,2

（1.浙江大學(xué)數(shù)字技術(shù)及儀器研究所,浙江杭州310027；2.浙江省網(wǎng)絡(luò)多媒體技術(shù)研究重點(diǎn)實(shí)驗(yàn)室,浙江杭州310027）

針對(duì)測(cè)量系統(tǒng)中同時(shí)存在未知的測(cè)量偏置和隨機(jī)測(cè)量噪聲的問題,提出帶測(cè)量偏置估計(jì)的魯棒卡爾曼濾波算法.該算法利用非零均值高斯分布對(duì)測(cè)量系統(tǒng)中的測(cè)量偏置和隨機(jī)測(cè)量噪聲進(jìn)行建模,利用正態(tài)-逆威沙特分布擬合該高斯分布的均值和協(xié)方差.該算法利用變分貝葉斯方法對(duì)該高斯分布和正態(tài)-逆威沙特分布的混合模型的時(shí)變參數(shù)進(jìn)行逼近推斷,在利用容積卡爾曼濾波算法進(jìn)行系統(tǒng)狀態(tài)迭代估計(jì)的同時(shí)對(duì)測(cè)量偏置進(jìn)行估計(jì)以及對(duì)時(shí)變隨機(jī)噪聲協(xié)方差進(jìn)行跟蹤,在進(jìn)行測(cè)量偏置估計(jì)的同時(shí)增強(qiáng)了濾波算法對(duì)測(cè)量野值的魯棒性.仿真實(shí)驗(yàn)證明了該算法在保證系統(tǒng)狀態(tài)估計(jì)精度的同時(shí),能夠高精度估計(jì)出測(cè)量偏置并增強(qiáng)了濾波算法的魯棒性.

測(cè)量偏置估計(jì)；魯棒性；正態(tài)-逆威沙特分布；變分貝葉斯；容積卡爾曼濾波器

卡爾曼濾波算法是一種建立在隱馬爾科夫模型之上的高效遞歸濾波算法［1］.在隱馬爾科夫模型中,系統(tǒng)的狀態(tài)不能被直接觀測(cè),但可以通過測(cè)量系統(tǒng)的測(cè)量結(jié)果進(jìn)行推斷［2］.在測(cè)量系統(tǒng)中主要存在2種類型的誤差：隨機(jī)誤差和系統(tǒng)誤差.

隨機(jī)誤差一般表現(xiàn)為服從高斯分布的加性隨機(jī)測(cè)量噪聲.在實(shí)際工程應(yīng)用中,測(cè)量值中會(huì)存在不符合高斯分布的野值.學(xué)生t分布的重尾特性使其比高斯模型更能表征實(shí)際情況下的測(cè)量噪聲分布,在魯棒性的卡爾曼濾波器中得到應(yīng)用［3］.由于學(xué)生t分布在積分運(yùn)算時(shí)的局限性,往往會(huì)使用混合高斯模型來近似學(xué)生t分布,并對(duì)高斯模型的協(xié)方差引入符合伽瑪（Gamma）分布的尺度因子［4］.直接利用高斯分布和其他分布聯(lián)合建模測(cè)量噪聲分布的方法在近年來被不斷提出［5-8］.Sarkka等［5］利用高斯模型來擬合測(cè)量噪聲,并利用多個(gè)逆伽瑪（inverse Gamma,IG）分布來擬合高斯模型協(xié)方差矩陣對(duì)角線上的各個(gè)元素.針對(duì)多維測(cè)量噪聲的情況,Agamennoni等分別利用威沙特（Wishart）分布來擬合精度矩陣［6］和利用逆威沙特（inverse Wishart,IW）分布來擬合協(xié)方差矩陣［7］,Sarkka等［8］利用逆威沙特分布來擬合協(xié)方差矩陣,分別推導(dǎo)出了線性系統(tǒng)和非線性系統(tǒng)中自適應(yīng)的魯棒卡爾曼濾波器.然而,測(cè)量噪聲的非高斯特性使系統(tǒng)無法獲得傳統(tǒng)的解析解.針對(duì)該問題,變分貝葉斯方法在上述算法中得到應(yīng)用.變分貝葉斯利用共樂指數(shù)域函數(shù)的性質(zhì)［9］,保證了近似分布能夠充分逼近原分布.

系統(tǒng)誤差一般表現(xiàn)為一種確定性的測(cè)量偏置,需要對(duì)其進(jìn)行估計(jì)并對(duì)測(cè)量系統(tǒng)進(jìn)行誤差補(bǔ)償.基于極大似然的偏置估計(jì)法［10］和基于偽測(cè)量的偏置估計(jì)法［11］被應(yīng)用到測(cè)量系統(tǒng)的測(cè)量偏置估計(jì)中.在統(tǒng)計(jì)學(xué)研究領(lǐng)域有很多方法來同時(shí)估計(jì)測(cè)量偏置和隨機(jī)測(cè)量噪聲［12］,這些方法一般應(yīng)用于同一狀態(tài)有很多測(cè)量值的情況.在本文要處理的問題中,同一狀態(tài)在同一測(cè)量系統(tǒng)中只有單個(gè)測(cè)量值.Ozkan等［13］提出基于粒子濾波器和正態(tài)-逆威沙特分布的濾波算法來估計(jì)高斯測(cè)量噪聲的均值和協(xié)方差,但當(dāng)異常測(cè)量殘差相比測(cè)量噪聲協(xié)方差很大時(shí)會(huì)得到極小的粒子權(quán)重,造成采樣粒子的非常嚴(yán)重的快速退化現(xiàn)象,算法的魯棒性較差.增廣狀態(tài)粒子濾波算法［13］將所有未知參數(shù)都增廣到系統(tǒng)狀態(tài)向量中,利用高斯隨機(jī)行走的方法對(duì)測(cè)量偏置進(jìn)行估計(jì),利用逆伽馬分布對(duì)隨機(jī)測(cè)量噪聲協(xié)方差進(jìn)行估計(jì).但是,增廣狀態(tài)向量增加了狀態(tài)向量的維度,導(dǎo)致運(yùn)算量呈二次多項(xiàng)式級(jí)增長.

針對(duì)上述問題,為了能夠同時(shí)對(duì)測(cè)量偏置和隨機(jī)測(cè)量噪聲進(jìn)行處理,本文提出一種基于正態(tài)-逆威沙特（normal-inverse-Wishart,NIW）分布和變分貝葉斯方法的帶測(cè)量偏置估計(jì)的魯棒卡爾曼濾波算法,通過實(shí)驗(yàn)仿真驗(yàn)證了該算法的有效性.

1 模型描述

針對(duì)一般的隱馬爾科夫模型,動(dòng)態(tài)過程可以表征如下：

式中：xn為系統(tǒng)狀態(tài),zn為測(cè)量系統(tǒng)的測(cè)量值,f n為狀態(tài)轉(zhuǎn)移函數(shù),hn為測(cè)量函數(shù),un和vn分別為過程噪聲和隨機(jī)測(cè)量噪聲,bn為測(cè)量偏置.

為了便于同時(shí)對(duì)測(cè)量偏置和隨機(jī)測(cè)量噪聲進(jìn)行建模,式（2）可以轉(zhuǎn)化為如下形式：

利用非零均值的高斯模型來同時(shí)表征測(cè)量偏置和隨機(jī)測(cè)量噪聲.在本文提出的算法中,利用en來表示測(cè)量殘差,利用逆威沙特分布來擬合測(cè)量噪聲wn的協(xié)方差矩陣∑n,利用正態(tài)分布來擬合測(cè)量噪聲wn的均值μn,因此可以將測(cè)量系統(tǒng)表示如下.

式中：νn和Vn分別為逆威沙特分布的自由度和尺度矩陣,mn為參數(shù)μn的期望,κn為參數(shù)μn的協(xié)方差的尺度因子.根據(jù)式（4）中參數(shù)μn和∑n的概率分布模型,聯(lián)合分布服從正態(tài)-逆威沙特分布,即p（μn,∑n）＝NIW（mn,κn,νn,Vn）.由于正態(tài)-逆威沙特分布是多維高斯分布的均值和協(xié)方差的共樂先驗(yàn)分布,本文算法選擇正態(tài)-逆威沙特分布作為多維高斯分布的均值和協(xié)方差的概率分布模型.

2 本文算法

2.1 變分貝葉斯近似

結(jié)合式（3）、（4）,測(cè)量噪聲均值μn－1和協(xié)方差∑n－1的基于測(cè)量殘差e1：n－1的后驗(yàn)條件概率分布可以表示為

在根據(jù)貝葉斯推斷原理利用式（6）和似然函數(shù)p（enμn,∑n）對(duì)后驗(yàn)概率分布p（μn,∑ne1：n）進(jìn)行計(jì)算時(shí),得到的后驗(yàn)概率分布函數(shù)是無法直接進(jìn)行解析處理的.為了解決該問題,本文算法利用變分貝葉斯原理對(duì)后驗(yàn)概率分布進(jìn)行近似,即

在變分貝葉斯近似方法中,通過近似分布和真實(shí)后驗(yàn)分布的KL-散度［14］的最小化來對(duì)近似分布進(jìn)行求解.該近似分布和真實(shí)分布的KL-散度定義為

根據(jù)變分貝葉斯近似方法基于最小化KL-散度的求解過程,近似分布Qμ（μn）（或QΣ（∑n））相對(duì)于QΣ（∑n）（或Qμ（μn））的解的形式如下：

結(jié)合式（4）、（6）,式（9）中的對(duì)數(shù)式子部分可以推導(dǎo)得出如下形式：

式（9）中的第1個(gè)積分運(yùn)算可以推導(dǎo)如下：

同理,式（9）中的第2個(gè)積分運(yùn)算可以推導(dǎo)如下：

式中：〈·〉μ＝∫（）·Qμ（μn）dμn為相對(duì)于近似分布Qμ（μn）的期望值,C3和C4為和μn無關(guān)的常數(shù).對(duì)于服從高斯分布的μn,式（13）中第一個(gè)〈·〉μ式子可以推導(dǎo)如下：

同理,式（13）中第二個(gè)〈·〉μ式子可以推導(dǎo)如下：

結(jié)合式（12）、（14）和（15）,式（13）中的積分運(yùn)算可以化簡到式（13）的最終形式,從而可以求出近似分布QΣ（∑n）的后驗(yàn)概率分布參數(shù)如下：

2.2 帶測(cè)量偏置估計(jì)的魯棒卡爾曼濾波算法

結(jié)合變分貝葉斯近似方法,本文提出非線性的帶測(cè)量偏置估計(jì)的魯棒卡爾曼濾波算法.容積卡爾

式中：dx為狀態(tài)向量的維度,［1］i為容積點(diǎn)［15］.

為了能在利用卡爾曼濾波器進(jìn)行系統(tǒng)狀態(tài)估計(jì)過程中快速估計(jì)出測(cè)量噪聲概率分布模型的參數(shù)以便快速獲得系統(tǒng)狀態(tài)的最佳估計(jì),在濾波算法迭代過程中測(cè)量噪聲概率分布模型參數(shù)的預(yù)測(cè)起到至關(guān)重要的作用.結(jié)合系統(tǒng)狀態(tài)xn的預(yù)測(cè)p（xn＋1｜xn）過程,參數(shù)μn和∑n的概率分布模型的參數(shù)可以進(jìn)行預(yù)測(cè),即p（μn＋1｜μn）和p（∑n＋1｜∑n）.根據(jù)參數(shù)μn的穩(wěn)定特性和∑n的時(shí)變特性,本文算法使用如下參數(shù)預(yù)測(cè)模型［13］：

式中：ρ為預(yù)測(cè)更新系數(shù).

綜合容積卡爾曼濾波器和正態(tài)-逆威沙特分布參數(shù)迭代估計(jì),本文提出的魯棒濾波算法可以歸納如下.

1）系統(tǒng)狀態(tài)和參數(shù)的初始化：

2）基于時(shí)間n（n＝0,1,…）進(jìn)行迭代

a）利用容積粒子進(jìn)行系統(tǒng)狀態(tài)預(yù)測(cè)曼濾波器（cubature Kalman filter,CKF）是一種基于容積數(shù)值積分準(zhǔn)則的貝葉斯濾波器,核心思想是利用Spherical-Radial準(zhǔn)則采樣2dx個(gè)等權(quán)值容積點(diǎn)來實(shí)現(xiàn)非線性估計(jì)［15］.具體采樣過程如下：

b）根據(jù)式（18）進(jìn)行測(cè)量噪聲模型參數(shù)預(yù)測(cè)

c）利用容積粒子進(jìn)行測(cè)量結(jié)果預(yù)測(cè)

e）測(cè)量噪聲模型參數(shù)收斂后輸出估計(jì)結(jié)果：

其中,φn＋1｜n表示變量φ的先驗(yàn)預(yù)測(cè)值,φn｜n表示變量φ的后驗(yàn)估計(jì)值,Sn｜n為協(xié)方差矩陣Pn｜n的平方根.容積卡爾曼濾波器的各個(gè)步驟的詳細(xì)解釋可參考文獻(xiàn)［15］,本文不再贅述.＝0表示事先未知各個(gè)測(cè)量系統(tǒng)測(cè)量偏置的先驗(yàn)信息＝dz＋2和＝∑0的選取使得對(duì)隨機(jī)測(cè)量噪聲協(xié)方差的估計(jì)值等于各個(gè)測(cè)量系統(tǒng)的隨機(jī)測(cè)量噪聲協(xié)方差的先驗(yàn)值∑0.

本文提出的算法與標(biāo)準(zhǔn)容積卡爾曼濾波算法的重要區(qū)別如下：本文算法利用正態(tài)-逆威沙特分布來擬合測(cè)量噪聲的均值和協(xié)方差,并利用測(cè)量噪聲均值和協(xié)方差的實(shí)時(shí)估計(jì)值取代標(biāo)準(zhǔn)容積卡爾曼濾波算法中的固定值；系統(tǒng)狀態(tài)的后驗(yàn)估計(jì)要迭代多次直至測(cè)量噪聲模型參數(shù)收斂；測(cè)量偏置的估計(jì)與補(bǔ)償以及測(cè)量噪聲協(xié)方差的實(shí)時(shí)估計(jì)增強(qiáng)了算法的魯棒性.

3 實(shí)驗(yàn)結(jié)果與分析

3.1 仿真模型建立

為了驗(yàn)證本文算法的有效性,對(duì)經(jīng)典的純距離多傳感器目標(biāo)跟蹤問題進(jìn)行建模仿真.目標(biāo)的運(yùn)動(dòng)模型采用二維固定轉(zhuǎn)率模型,即

式中：rn＝［x1,n,x2,n］T為運(yùn)動(dòng)目標(biāo)位置的估計(jì)值, lj為傳感器j的位置,υ為信號(hào)傳播速率,wn為式（3）中描述的測(cè)量噪聲,· 為歐式距離.

在相同的仿真環(huán)境下,通過對(duì)比以下4種方法的仿真結(jié)果來驗(yàn)證本文算法的優(yōu)勢(shì).

·CKF-NIW：本文提出的基于正態(tài)-逆威沙特分布和容積卡爾曼濾波器的魯棒濾波算法.

·CKF-NIG：基于文獻(xiàn)［16］的正態(tài)-逆伽瑪（normal-inverse-Gamma,NIG）模型和容積卡爾曼濾波器的魯棒濾波算法.

·CKF-IW：基于CKF-NIW估計(jì)出的測(cè)量偏置期望值,只利用逆威沙特分布估計(jì)測(cè)量噪聲協(xié)方差的魯棒濾波算法.

·CKF-True：測(cè)量噪聲概率分布模型參數(shù)已知情況下的標(biāo)準(zhǔn)容積卡爾曼濾波算法.

鑒于利用逆威沙特分布擬合測(cè)量噪聲協(xié)方差的算法的魯棒性在文獻(xiàn)［7,8］中已得到驗(yàn)證,本文的實(shí)驗(yàn)部分重點(diǎn)驗(yàn)證狀態(tài)估計(jì)和測(cè)量偏置估計(jì)的性能.

3.2 結(jié)果分析

分析各種算法的目標(biāo)跟蹤結(jié)果.圖1、2分別展示了運(yùn)動(dòng)目標(biāo)狀態(tài)估計(jì)的位置分量和速度分量的均方根誤差（RMSE）結(jié)果.圖中,SRMSE、vRMSE分別為位置分量和速度分量的均方根誤差.對(duì)比算法CKFNIW、CKF-IW和CKF-True的仿真結(jié)果可知,當(dāng)只有測(cè)量噪聲協(xié)方差未知時(shí),利用逆威沙特模型擬合協(xié)方差的分布可以獲得較好的狀態(tài)估計(jì).當(dāng)測(cè)量偏置和測(cè)量噪聲協(xié)方差都未知時(shí),系統(tǒng)誤差和隨機(jī)誤差的同時(shí)存在造成測(cè)量偏置和隨機(jī)測(cè)量噪聲的相互耦合,因此在數(shù)值上很難精確分離出測(cè)量偏置和隨機(jī)測(cè)量噪聲.測(cè)量偏置的估計(jì)精度直接影響了測(cè)量殘差的計(jì)算,進(jìn)而直接影響了狀態(tài)估計(jì)的精度.隨機(jī)測(cè)量噪聲協(xié)方差的估計(jì)精度直接影響了預(yù)測(cè)測(cè)量協(xié)方差矩陣的計(jì)算,從而影響卡爾曼增益的計(jì)算,最終影響狀態(tài)估計(jì)的精度.當(dāng)測(cè)量偏置和隨機(jī)測(cè)量噪聲協(xié)方差都未知時(shí),算法CKF-NIG和CKF-NIW獲得相對(duì)較差的跟蹤結(jié)果.

圖1 位置估計(jì)均方根誤差對(duì)比(ρ＝1—exp(—4))Fig.1 RMSE comparison of position estimation（ρ＝1－exp（－4））

圖2 速度估計(jì)均方根誤差對(duì)比(ρ＝1—exp(—4))Fig.2 RMSE comparison of velocity estimation（ρ＝1－exp（－4））

對(duì)比算法CKF-NIG和CKF-NIW的仿真結(jié)果可知,無論是在位置分量的估計(jì)上,還是在速度分量的估計(jì)上,本文提出的CKF-NIW算法都要獲得較好的結(jié)果.這是因?yàn)檎龖B(tài)-逆伽瑪模型只注重協(xié)方差矩陣對(duì)角線上元素的估計(jì),而正態(tài)-逆威沙特模型對(duì)整個(gè)協(xié)方差矩陣進(jìn)行估計(jì).無論是本文的仿真結(jié)果,還是文獻(xiàn)［8］的仿真結(jié)果,逆威沙特分布都表現(xiàn)出了相較于逆伽瑪分布的優(yōu)越性.

對(duì)比圖1、2的結(jié)果可知,測(cè)量偏置對(duì)速度分量的影響小于對(duì)位置分量的影響.這是因?yàn)楸緦?shí)驗(yàn)的測(cè)量系統(tǒng)只對(duì)目標(biāo)的位置信息進(jìn)行觀測(cè),因此測(cè)量偏置和隨機(jī)測(cè)量噪聲對(duì)位置分量的估計(jì)產(chǎn)生直接的影響.利用相鄰時(shí)間位置分量的差值進(jìn)行速度分量的估計(jì)抑制了測(cè)量偏置對(duì)速度分量估計(jì)的影響.

分析測(cè)量偏置的估計(jì)結(jié)果.圖3展示了算法CKF-NIG和CKF-NIW對(duì)測(cè)量偏置估計(jì)的結(jié)果.盡管4個(gè)傳感器節(jié)點(diǎn)擁有不同的測(cè)量偏置,但是本文提出的算法仍然可以快速估計(jì)出各個(gè)傳感器節(jié)點(diǎn)的測(cè)量偏置,并在整個(gè)過程中保持相對(duì)穩(wěn)定的估計(jì)值.算法CKF-NIG和CKF-NIW的測(cè)量偏置估計(jì)值的過程均值分別為：0.1023、0.1538、0.2020、0.2513和0.1009、0.1529、0.2015、0.2504.算法CKF-NIG和本文提出的CKF-NIW算法都采用正態(tài)分布對(duì)測(cè)量偏置的概率分布進(jìn)行擬合.盡管2種算法都得到了高精度的測(cè)量偏置估計(jì)結(jié)果,但本文算法得到了相對(duì)更高的估計(jì)精度.

圖3 測(cè)量偏置估計(jì)結(jié)果(ρ＝1—exp(—4))Fig.3 Estimates of measurement biases（ρ＝1－exp（－4））

分析參數(shù)更新系數(shù)ρ的取值.在本文描述的問題中同一時(shí)刻的狀態(tài)在同一測(cè)量系統(tǒng)中只有一個(gè)測(cè)量值,因此無法利用統(tǒng)計(jì)方法中的多測(cè)量值的統(tǒng)計(jì)特性.本文算法通過參數(shù)更新系數(shù)ρ對(duì)測(cè)量偏置和隨機(jī)測(cè)量噪聲參數(shù)進(jìn)行自適應(yīng)迭代更新.根據(jù)式（12）、（16）和（18）,ρ的取值決定了單次測(cè)量值對(duì)模型參數(shù)的更新程度.圖4展示了當(dāng)ρ＝1－exp（－5）時(shí)的測(cè)量偏置估計(jì)結(jié)果,對(duì)比圖3展示的結(jié)果可以得出：當(dāng)ρ增大時(shí),測(cè)量偏置估計(jì)對(duì)單個(gè)測(cè)量值的魯棒性增加,進(jìn)而得到更加平滑的測(cè)量偏置估計(jì),但測(cè)量偏置估計(jì)的收斂速度略有降低.系統(tǒng)仿真在更新系數(shù)ρ＝1－exp（－5）時(shí)取得了更加準(zhǔn)確的測(cè)量偏置估計(jì),進(jìn)而在對(duì)目標(biāo)的跟蹤上取得了更高的跟蹤精度.當(dāng)ρ＝1－exp（－4）和ρ＝1－exp（－5）時(shí)系統(tǒng)仿真取得的目標(biāo)位置估計(jì)平均最小均方誤差分別為：17.07和14.17.這進(jìn)一步說明了測(cè)量偏置估計(jì)精度對(duì)最終目標(biāo)狀態(tài)的估計(jì)精度有直接影響.在實(shí)際應(yīng)用中,ρ的取值要兼顧算法的整體估計(jì)精度和收斂速度.

圖4 測(cè)量偏置估計(jì)結(jié)果(ρ＝1—exp(—5))Fig.4 Estimates of measurement biases（ρ＝1－exp（－5））

由于測(cè)量偏置和隨機(jī)測(cè)量噪聲的相互耦合,當(dāng)出現(xiàn)幅值較大的測(cè)量噪聲時(shí),會(huì)對(duì)測(cè)量偏置的估計(jì)造成影響.鑒于測(cè)量偏置與測(cè)量系統(tǒng)本身的特性相關(guān),一般可以認(rèn)為測(cè)量偏置在短期內(nèi)是相對(duì)穩(wěn)定的.因此,充分利用測(cè)量偏置的穩(wěn)定性,通過滑動(dòng)窗口獲取一段時(shí)期內(nèi)的測(cè)量偏置估計(jì)均值,從而求出測(cè)量偏置的高精度期望值.已知測(cè)量偏置期望值,則可以利用CKF-IW算法取代CKF-NIW算法,通過適當(dāng)調(diào)度2種算法以取得更好的目標(biāo)狀態(tài)估計(jì)結(jié)果.

為了進(jìn)一步綜合分析本文算法對(duì)測(cè)量偏置和隨機(jī)噪聲協(xié)方差進(jìn)行同時(shí)估計(jì)的效果,以下仿真實(shí)驗(yàn)參照文獻(xiàn)［13］,采用高斯隨機(jī)噪聲取代原來的服從學(xué)生t分布的隨機(jī)噪聲,以便直接評(píng)估本文算法對(duì)隨機(jī)噪聲協(xié)方差的估計(jì)精度.測(cè)量偏置分別設(shè)置為［0.01,0.05,0.1,0.15］,測(cè)量偏置估計(jì)的初值分別設(shè)置為［0.05,0.01,0.15,0.1］,高斯隨機(jī)噪聲協(xié)方差的標(biāo)準(zhǔn)差σw＝0.001.圖5、6展示了測(cè)量偏置估計(jì)結(jié)果和測(cè)量噪聲協(xié)方差標(biāo)準(zhǔn)差的估計(jì)結(jié)果.圖5顯示,當(dāng)測(cè)量偏置估計(jì)初值為小于實(shí)際值的非零值時(shí),測(cè)量偏置估計(jì)收斂速度略有增加；當(dāng)測(cè)量偏置估計(jì)初值大于實(shí)際值時(shí),收斂速度相比前一情況略有降低；收斂之后的估計(jì)結(jié)果與圖4的結(jié)果相比并未有明顯差異.這一結(jié)果證明了先驗(yàn)信息只對(duì)算法的收斂速度稍有影響,而對(duì)收斂后的估計(jì)結(jié)果影響甚微.

圖5 不同估計(jì)初值時(shí)的測(cè)量偏置估計(jì)結(jié)果Fig.5 Estimates of measurement biases with different estimation initial values

圖6結(jié)果顯示,本文算法對(duì)隨機(jī)噪聲協(xié)方差有較好的估計(jì)結(jié)果.盡管本文算法的收斂速度不夠迅速,但算法收斂之后取得了較高的估計(jì)精度,而收斂速度不足與每一時(shí)刻每個(gè)傳感器只有一個(gè)測(cè)量值有關(guān).由于測(cè)量偏置比隨機(jī)測(cè)量噪聲具有更強(qiáng)的確定性,測(cè)量偏置估計(jì)的收斂速度大于隨機(jī)噪聲協(xié)方差估計(jì)的收斂速度.由于本文算法的重點(diǎn)在測(cè)量偏置估計(jì)和對(duì)非高斯噪聲的魯棒性,對(duì)高斯隨機(jī)噪聲的情況不作詳細(xì)討論.

圖6 測(cè)量噪聲協(xié)方差估計(jì)結(jié)果Fig.6 Estimates of measurement noise covariance

4 結(jié) 語

針對(duì)測(cè)量系統(tǒng)中測(cè)量偏置和隨機(jī)測(cè)量噪聲同時(shí)存在的問題,本文提出帶測(cè)量偏置估計(jì)的魯棒容積卡爾曼濾波算法.通過將測(cè)量偏置和隨機(jī)測(cè)量噪聲分別建模成高斯模型的均值和協(xié)方差,本文算法利用正態(tài)-逆威沙特分布對(duì)高斯模型的均值和協(xié)方差參數(shù)的概率分布模型進(jìn)行擬合.基于變分貝葉斯近似算法和容積卡爾曼濾波器,本文推導(dǎo)出非線性的帶測(cè)量偏置估計(jì)的魯棒卡爾曼濾波算法.通過建模仿真,本文提出的算法具有優(yōu)良的系統(tǒng)狀態(tài)估計(jì)性能,對(duì)測(cè)量偏置具有更高的估計(jì)精度,對(duì)隨機(jī)測(cè)量噪聲具有更好的魯棒性.

［1］WELCH G,BISHOP G.An introduction to Kalman filtering［R］.Chapel Hill,NC：University of North Carolina,2006.

［2］RABINER L,JUANG B.An introduction to hidden Markov models［J］.IEEE ASSP Magazine,1986, 3（1）：4- 16.

［3］ZHU H,LEUNG H,HE Z.A variational Bayesian approach to robust sensor fusion based on Student-t distribution［J］.Information Sciences,2013,221（0）：201- 214.

［4］JO-ANNE T,THEODOROU E,SCHAAL S.A Kalman filter for robust outlier detection［C］//IEEE／RSJ International Conference on Intelligent Robots and Systems.San Diego：IEEE,2007：1514- 1519.

［5］SARKKA S,NUMMENMAA A.Recursive noise adaptive Kalman filtering by variational Bayesian approximations［J］.IEEE Transactions on Automatic Control, 2009,54（3）：596- 600.

［6］AGAMENNONI G,NIETO J I,NEBOT E M.An outlier-robust Kalman filter［C］//IEEE International Conference on Robotics and Automation.Shanghai：IEEE, 2011：1551- 1558.

［7］AGAMENNONI G,NIETO J I,NEBOT E M.Approximate inference in state-space models with heavy-tailed noise［J］.IEEE Transactions on Signal Processing,2012,60（10）：5024- 5037.

［8］SARKKA S,HARTIKAINEN J.Non-linear noise adaptive Kalman filtering via variational Bayes［C］//IEEE International Workshop on Machine Learning for Signal Processing.Southampton：IEEE,2013：1- 6.

［9］BEAL M J.Variational algorithms for approximate Bayesian inference［D］.London：University of London,2003.

［10］OKELLO N,RISTIC B.Maximum likelihood registration for multiple dissimilar sensors［J］.IEEE Transactions on Aerospace and Electronic Systems,2003, 39（3）：1074- 1083.

［11］TAGHAVI E,THARMARASA R,KIRUBARAJAN T,et al.Bias estimation for practical distributed multiradar-multitarget tracking systems［C］//16th International Conference on Information Fusion.Istanbul：ISIF,2013：1304- 1311.

［12］GELMAN A,CARLIN J B,STERN H S,et al.Bayesian data analysis［M］.Florida：CRC,2013.

［13］OZKAN E,SMIDL V,SAHA S,et al.Marginalized adaptive particle filtering for nonlinear models with unknown time-varying noise parameters［J］.Automatica,2013,49（6）：1566- 1575.

［14］HERSHEY J R,OLSEN P A.Approximating the Kullback Leibler divergence between Gaussian mixture models［C］//IEEE International Conference on Acoustics,Speech and Signal Processing.Hawaii：IEEE, 2007：317- 320.

［15］ARASARATNAM I,HAYKIN S.Cubature Kalman filters［J］.IEEE Transactions on Automatic Control, 2009,54（6）：1254- 1269.

［16］COWLES M K.Applied Bayesian statistics［M］.New York：Springer,2013.

Robust Kalman filtering algorithm with estimation of measurement biases

ZHU Guang-ming1,2,JIANG Rong-xin1,2,ZHOU Fan1,2,TIAN Xiang1,2,CHEN Yao-wu1,2
（1.Institute of Advanced Digital Technology and Instrumentation,Zhejiang University,Hangzhou 310027,China；2.Zhejiang Provincial Key Laboratory for Network Multimedia Technologies,Hangzhou 310027,China）

A robust Kalman filtering algorithm with estimation of measurement biases was proposed in order to handle the problem that unknown measurement biases and random measurement noises exist in the measurement system.The nonzero-mean Gaussian distribution model was utilized to model the measurement biases and the random measurement noises of the measurement system.The Normal-Inverse-Wishart distribution was utilized to estimate the mean and covariance parameters of the Gaussian distribution.The time-variant parameters of the mixed model between the Gaussian distribution and the Normal-Inverse-Wishart distribution were inferred by the variational Bayesian approximation.The measurement biases and the time-variant covariances of the random measurement noises were estimated as the system states were recursively inferred by the cubature Kalman filter.The proposed algorithm's robustness to the measurement outliers was enhanced with the estimation of the measurement biases.The simulation results demonstrate that the proposed algorithm can also precisely estimate the measurement biases and enhance its robustness with the guarantee of the high state estimation precision.

estimation of measurement biases；robustness；Normal-Inverse-Wishart distribution；variational Bayesian；cubature Kalman filter

蔣榮欣,男,副研究員.E-mail：rongxinj＠zju.edu.cn

TB 56

A

1008- 973X（2015）07- 1343- 07

10.3785／j.issn.1008-973X.2015.07.020

2014- 05- 16. 浙江大學(xué)學(xué)報(bào)(工學(xué)版)網(wǎng)址：www.journals.zju.edu.cn／eng

中央高校基本科研業(yè)務(wù)費(fèi)專項(xiàng)資金資助項(xiàng)目；浙江省重點(diǎn)科技創(chuàng)新團(tuán)隊(duì)資助項(xiàng)目（2011R09021-06）；浙江大學(xué)基本科研業(yè)務(wù)費(fèi)專項(xiàng)資金資助項(xiàng)目（2014FZA5020）.

朱光明（1987－）,男,博士生,從事無線傳感器網(wǎng)絡(luò)及信息融合的研究.ORCID：0000-0003-3214-4095.E-mail：zhgm＠zju.edu.cn