處理飛行時間測距數(shù)據(jù)的自適應(yīng)卡爾曼濾波的方法
【專利摘要】本發(fā)明公開了一種處理飛行時間測距數(shù)據(jù)的自適應(yīng)卡爾曼濾波的方法。它包括對移動節(jié)點和錨節(jié)點之間飛行時間測距數(shù)據(jù)的卡爾曼濾波處理，特別是，一是通過建立目標(biāo)狀態(tài)方程和有色噪聲量測方程，使測量誤差自適應(yīng)于測距數(shù)據(jù)的變化，二是通過設(shè)立自適應(yīng)于測距數(shù)據(jù)變化的門限值來濾除偏差較大的測距數(shù)據(jù)。本發(fā)明所建的誤差模型符合實際的誤差模型，使得濾波過程更準確，而且在濾去偏差過大的測距數(shù)據(jù)時能夠自適應(yīng)測距數(shù)據(jù)的變化，使得對大偏差的數(shù)據(jù)濾去效果更好，最終所得的處理結(jié)果非常接近真實數(shù)據(jù)；使其可廣泛地用于無線傳感器網(wǎng)絡(luò)中飛行時間的準確測距。
【專利說明】處理飛行時間測距數(shù)據(jù)的自適應(yīng)卡爾曼濾波的方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種卡爾曼濾波的方法，尤其是一種處理飛行時間測距數(shù)據(jù)的自適應(yīng) 卡爾曼濾波的方法。

【背景技術(shù)】
[0002] 在無線傳感器網(wǎng)絡(luò)（WSN)的應(yīng)用中，傳感器節(jié)點定位技術(shù)是無線傳感器網(wǎng)絡(luò)定位 的關(guān)鍵技術(shù)，在無線傳感器網(wǎng)絡(luò)中占有重要的地位。基于飛行時間（TOF)的節(jié)點定位技術(shù) 具有定位精度較高、能耗小、對硬件要求低等特點，是目前應(yīng)用最廣泛的定位方法之一。飛 行時間測距技術(shù)由來已久，它是通過測量節(jié)點間信號傳輸?shù)臅r間來得到兩節(jié)點間的距離， 其數(shù)值即為飛行時間測距數(shù)據(jù)。由于非視距（NLOS)的存在，飛行時間測距數(shù)據(jù)會存在NLOS 誤差，其誤差一般具有正偏性，飛行時間所測量的距離會偏大于實際距離。如果直接將測距 數(shù)據(jù)用于節(jié)點的定位，其結(jié)果會大大地偏離節(jié)點的實際位置。所以如何對飛行時間的測距 數(shù)據(jù)進行處理，消除其中的NLOS誤差顯得尤為重要。
[0003] 卡爾曼（Kalman)濾波作為一種最優(yōu)狀態(tài)估計的方法，給出了一種遞推算法，其由 實時獲得的受噪聲污染的離散觀測數(shù)據(jù)，對系統(tǒng)狀態(tài)進行線性、無偏，及最小誤差方差的最 優(yōu)估計。卡爾曼濾波應(yīng)用于飛行時間測距數(shù)據(jù)的估計由來已久，但現(xiàn)有的卡爾曼濾波存在 著不足，其測量誤差只作為一種固定的未知模型，模型的不確定性對最后的濾波輸出產(chǎn)生 較大影響，實際的測量誤差是有色噪聲且不斷變化的，而現(xiàn)有的卡爾曼濾波僅將其協(xié)方差 作為固定值，顯然與實際不符。為解決這一問題，人們做出了各種努力，如題為"基于Kalman 濾波器的非視距誤差抑制算法"，張美楊，等，《計算機中文》2010年第36卷第11期的文章。 該文將到達時間（TOA)測量值及非視距誤差作為Kalman濾波器的狀態(tài)變量；其雖能夠同時 估計出測量值和非視距誤差，但由于測量噪聲的模型不確定，且測量噪聲協(xié)方差R固定不 變，從而導(dǎo)致了濾波結(jié)果的精確度不高。
[0004]NLOS誤差的隨機性引起了NLOS誤差的劇烈變化，使得某些飛行時間測距數(shù)據(jù)的 偏差特別大。這些受NLOS誤差污染嚴重的測距數(shù)據(jù)將嚴重影響對飛行時間的正確估計。所 以，如果能消除這些包含較大誤差測距數(shù)據(jù)的影響，就可以從很大程度上消除NLOS誤差。 為此，人們做了一些嘗試，如在"用卡爾曼濾波器消除TOA中NLOS誤差的三種方法"，李靜， 等，《通信學(xué)報》第26卷第1期于2005年公開的測距數(shù)據(jù)丟棄法，該法將偏離實際值較大的 測距點丟棄，用狀態(tài)預(yù)測值代替狀態(tài)測量值；此方法雖能夠減小含有較大NLOS誤差的測距 數(shù)據(jù)，然其門限值的確定是靠實驗得到的，并不能根據(jù)測距數(shù)據(jù)的變化而實時地變化。


【發(fā)明內(nèi)容】

[0005] 本發(fā)明要解決的技術(shù)問題為克服上述各種技術(shù)方案的局限性，提供一種能夠獲得 較準確濾波結(jié)果的處理飛行時間測距數(shù)據(jù)的自適應(yīng)卡爾曼濾波的方法。
[0006] 為解決本發(fā)明的技術(shù)問題，所采用的技術(shù)方案為：處理飛行時間測距數(shù)據(jù)的自適 應(yīng)卡爾曼濾波的方法包括對移動節(jié)點和錨節(jié)點之間飛行時間測距數(shù)據(jù)的卡爾曼濾波處理， 特別是主要步驟如下：
[0007] 步驟1，建立目標(biāo)狀態(tài)方程和有色噪聲量測方程
[0008] xk+1 =Axk+Bak,
[0009] vk =Cxk+βk,
[0010] β k - Nlrf β η+ Yk，
[0011] 其中，
[0012] A為狀態(tài)矩陣，B為誤差系數(shù)矩陣，C為測量矩陣，
[0013] xk為目標(biāo)在k(k= 1，2,3,......η)時刻的二維狀態(tài)向量,? =Ok匕]'其中的 rk為待估計的飛行時間值A(chǔ).為其一階導(dǎo)數(shù)，
[0014] ak為系統(tǒng)噪聲，是一個高斯白噪聲序列，協(xié)方差用Q表不，
[0015] Vk為一維測量向量，表示k時刻的測距數(shù)據(jù)，
[0016] βk為k時刻的測量誤差,協(xié)方差矩陣為Rk,
[0017] Nh為自回歸系數(shù)，
[0018] Yk為1?斯白噪聲序列；
[0019] 步驟2,根據(jù)原始卡爾曼濾波算法，由k-Ι時刻的濾波結(jié)果得到目標(biāo)的k時刻的預(yù) 測值以及k時刻的新息值zk，計算公式為

【權(quán)利要求】
1. 一種處理飛行時間測距數(shù)據(jù)的自適應(yīng)卡爾曼濾波的方法，包括對移動節(jié)點和錨節(jié)點 之間飛行時間測距數(shù)據(jù)的卡爾曼濾波處理，其特征在于主要步驟如下： 步驟1，建立目標(biāo)狀態(tài)方程和有色噪聲量測方程xk+1 = Axk+B a k, vk - Cxk+ β k, β k - Nk-! β k-!+Ykj 其中， A為狀態(tài)矩陣，B為誤差系數(shù)矩陣，C為測量矩陣， xk為目標(biāo)在k(k= 1，2,3,……η)時刻的二維狀態(tài)向量，Xfc = [rkfk]T,其中的rk為 待估計的飛行時間值、4.為其一階導(dǎo)數(shù)， ak為系統(tǒng)噪聲，是一個高斯白噪聲序列，協(xié)方差用Q表示， Vk為一維測量向量，表示k時刻的測距數(shù)據(jù)，βk為k時刻的測量誤差，協(xié)方差矩陣為Rk， Nlrf為自回歸系數(shù)， Yk為_斯白噪聲序列； 步驟2,根據(jù)原始卡爾曼濾波算法，由k-Ι時刻的濾波結(jié)果得到目標(biāo)的k時刻的預(yù)測值 %/k-i以及k時刻的新息值zk，計算公式為
其中，％-i/k-i為k-Ι時刻的濾波結(jié)果； 步驟3,求出測量噪聲協(xié)方差k時刻的估計值 先計算參數(shù)Sk，其由下式來估計得到§k，
其中， ^是前i次新息值的均值，計算為 ? 1=1 5 再按照下兩式得出測量噪聲協(xié)方差在k時刻的估計值Sk, Pk, k-i一APh，hA +BQB， Rjc = §|^ - J 其中，Γ\，η是k時刻預(yù)測誤差方差陣，Pm1是k_l時刻估計誤差方差陣； 步驟4,計算Nlrf 根據(jù)和Yk獨立，且有協(xié)方差的關(guān)系得出Nlrt的計算式為
其中，var(Yk)是Yk的方差； 步驟5，計算k時刻的門限值mk 計算前k次測距數(shù)據(jù)的方差諱，取門限值為方差值,即mk = 4; 步驟6,更新卡爾曼濾波增益 將k時刻的新息值與門限值進行比較，若新息值大于門限值，則將卡爾曼增益置為零， 否則按以下兩步計算濾波增益Gk， R, =「CA-N,.Cl·
步驟7,更新估計誤差方差陣 由P0 =Cbar(X0)FHCTRi1Cr^出估計誤差方差陣的初值， 其中， Xtl取為第一個測距數(shù)據(jù)，R3.為誤差協(xié)方差初值，靈β取作前幾次試驗測距數(shù)據(jù)的方差， 估計誤差方差陣更新表達式為 Pk,k= (A-GA-i) -Pk-!,^1At+(I-GkC)BQBT； 步驟8,根據(jù)k時刻濾波輸出方程輸出k時刻的濾波結(jié)果，濾波方程為 愛k/k = + Gk * Ok - Nfe-ivk-i - Hk-:#k-：Mk-i)， 如此循環(huán)往復(fù)，輸出I到n時刻的濾波結(jié)果，得到對飛行時間測距數(shù)據(jù)的處理結(jié)果。
2. 根據(jù)權(quán)利要求1所述的處理飛行時間測距數(shù)據(jù)的自適應(yīng)卡爾曼濾波的方法，其特征 是步驟1中的狀態(tài)矩陣A的取值為A=BI]其中，T為濾波采樣間隔。 5
3. 根據(jù)權(quán)利要求1所述的處理飛行時間測距數(shù)據(jù)的自適應(yīng)卡爾曼濾波的方法，其特征 是步驟1中的誤差系數(shù)矩陣B的取值為B
4. 根據(jù)權(quán)利要求1所述的處理飛行時間測距數(shù)據(jù)的自適應(yīng)卡爾曼濾波的方法，其特征 是步驟1中的測量矩陣C的取值為C= [1 0]。
5. 根據(jù)權(quán)利要求1所述的處理飛行時間測距數(shù)據(jù)的自適應(yīng)卡爾曼濾波的方法，其特征 是步驟1中的Xk的下標(biāo)k=η時的取值為1000。
6. 根據(jù)權(quán)利要求1所述的處理飛行時間測距數(shù)據(jù)的自適應(yīng)卡爾曼濾波的方法，其特征 是步驟2中的0時刻的濾波結(jié)果根據(jù)初始測距數(shù)據(jù)由下式得出， S0 = E(X0) - [var(X(j)]CT [C-Var(X0) ·Ct+S0]-1[CE(X0) - vc]， 其中，Xtl取為第一個測距數(shù)據(jù)、V。為第一個測距數(shù)據(jù)、Ke.取為前30次測距數(shù)據(jù)的方差。
【文檔編號】G01S5/02GK104316905SQ201410557259
【公開日】2015年1月28日 申請日期:2014年10月20日 優(yōu)先權(quán)日:2014年10月20日
【發(fā)明者】李奇越, 吳忠, 王建平, 黎潔, 孫偉 申請人:合肥工業(yè)大學(xué)