本發(fā)明屬于室內(nèi)跟蹤定位技術(shù)領(lǐng)域，尤其涉及一種融合地圖信息的改進卡爾曼濾波器室內(nèi)定位跟蹤方法，可用于覆蓋wifi信號的室內(nèi)環(huán)境，通過融合地圖信息的改進卡爾曼濾波器室內(nèi)定位跟蹤方法，來優(yōu)化wi-fi定位跟蹤結(jié)果。

背景技術(shù)：

當前，隨著無線網(wǎng)絡(luò)的發(fā)展和無線局域網(wǎng)的廣泛部署，基于wifi的室內(nèi)定位技術(shù)受到廣泛重視。在覆蓋wifi網(wǎng)絡(luò)的室內(nèi)環(huán)境下，通過測量來自接入點aps的接收信號強度rss，結(jié)合采集好的信號強度數(shù)據(jù)庫求解，確定移動用戶的位置。實時定位時，由于信號波動、多徑干擾、陰影效應(yīng)及人員的走動等多種因素影響，定位結(jié)果常常發(fā)生抖動，導(dǎo)致定位效果不佳。為了穩(wěn)定定位結(jié)果，通常會對定位結(jié)果進行濾波，使得輸出的定位結(jié)果更加平滑。

卡爾曼濾波是對隨機信號做濾波的算法之一，應(yīng)用十分廣泛，目前在通信、導(dǎo)航、遙感、石油勘探等各個領(lǐng)域都有重要應(yīng)用。相比于維納濾波、最小二乘等其他濾波算法，卡爾曼濾波器有如下特點：

(1)在求解時不需要貯存大量的觀測數(shù)據(jù)，并且當?shù)玫玫叫碌挠^測數(shù)據(jù)時，可隨時算得新的參數(shù)濾波值，便于實時地處理觀測成果。

(2)卡爾曼濾波器是以最小均方誤差為原則的最佳線性濾波器，根據(jù)前一個估計值和最近一個觀察數(shù)據(jù)來估計信號的當前值，需要已知系統(tǒng)的狀態(tài)方程和量測方程，它的解是以估計值的形式給出的。

(3)卡爾曼濾波采用實用的遞推算法，成功地將狀態(tài)變量法引入到濾波理論中來，在時域內(nèi)設(shè)計濾波器，且不要求保存過去的測量數(shù)據(jù)。

(4)被估計量可以是平穩(wěn)的，也可以是非平穩(wěn)的，即卡爾曼濾波也適用于非平穩(wěn)過程。

利用離散卡爾曼濾波器對定位跟蹤進行處理是一種優(yōu)化結(jié)果的方法，但是由于方法在長時間平穩(wěn)移動后會更偏向預(yù)測值，一旦發(fā)生大幅度偏離原運動方向的情況，就會需要一段時間的糾正過程，不利于跟蹤與定位結(jié)果。針對此方面，還有不少的優(yōu)化空間。

綜上所述，現(xiàn)有技術(shù)存在的問題是：

利用離散卡爾曼濾波器對定位跟蹤進行處理是一種優(yōu)化結(jié)果的方法，但是由于方法在長時間平穩(wěn)移動后會更偏向預(yù)測值，一旦發(fā)生大幅度偏離原運動方向的情況，就會需要一段時間的糾正過程，不利于跟蹤與定位結(jié)果。針對此方面，還有不少的優(yōu)化空間。

技術(shù)實現(xiàn)要素：

針對上述問題，本發(fā)明的目的在于提供一種融合地圖信息的改進卡爾曼濾波器室內(nèi)定位跟蹤方法。

實現(xiàn)本發(fā)明目的的技術(shù)方案是：融合地圖信息信息，遇到拐角時，如果移動終端發(fā)生轉(zhuǎn)彎，調(diào)整卡爾曼濾波器的參數(shù)，以改善其跟蹤精度。包括兩個階段：

1)確定基本離散卡爾曼濾波器方程和對應(yīng)的變量。

1.1)確定狀態(tài)向量：卡爾曼濾波器的狀態(tài)向量為：

x＝[sx,vx,sy,vy]^t

其中sx和vx分別表示x方向的位置和速度，sy和vy分別表示y方向的位置和速度；

1.2)確定量測陣：由于用戶在定位時只有定位結(jié)果，而沒有輸出速度信息，因此觀測量只有sx和sy，從而量測陣為：

其中h為卡爾曼濾波器的量測陣；

1.3)確定狀態(tài)方程：由于發(fā)明定位跟蹤系統(tǒng)設(shè)定為各個直線段勻速運動，因此狀態(tài)方程為：

其中sxk，syk，vxk和vyk分別表示k時刻定位終端的位置和速度，sx(k-1)，sy(k-1)，vx(k-1)和vy(k-1)分別表示k-1時刻定位終端的位置和速度；

1.4)確定狀態(tài)轉(zhuǎn)移矩陣：

其中a表示卡爾曼濾波器的狀態(tài)轉(zhuǎn)移矩陣，δt表示兩次定位之間的時間間隔；

1.5)最小均方誤差陣p的初始值的選取沒有特別要求，在卡爾曼濾波過程中p矩陣會趨于穩(wěn)定；

1.6)在卡爾曼濾波器的實際應(yīng)用時，測量噪聲的協(xié)方差r一般可以直接觀測得到，是濾波器的已知條件。但是激勵噪聲協(xié)方差陣q無法直接得到，可以根據(jù)實際情況選取合適的q。

2)結(jié)合地圖對卡爾曼濾波器跟蹤算法進行優(yōu)化。

2.1)計算第k個時刻卡爾曼濾波器的時間更新方程：

其中為k時刻的預(yù)測狀態(tài)向量，為k-1時刻卡爾曼濾波器輸出的狀態(tài)向量，a為狀態(tài)轉(zhuǎn)移矩陣，b是控制輸入矩陣，uk-1為k-1時刻的驅(qū)動輸入向量，t表示矩陣的轉(zhuǎn)置；

2.2)計算第k個時刻卡爾曼濾波器的測量更新方程：

其中g(shù)k表示第k個時刻的卡爾曼濾波增益，zk為第k個時刻的觀測向量，i為單位矩陣；

2.3)如果定位跟蹤結(jié)束，則退出跟蹤算法，否則，執(zhí)行步驟2.4；

2.4)判斷定位終端是否到達拐角附近，如果到達拐角附近，執(zhí)行步驟2.5，否則，令k的值增加1后回到步驟2.1；

2.5)判斷定位終端是否發(fā)生轉(zhuǎn)彎，如果發(fā)生轉(zhuǎn)彎，執(zhí)行步驟2.6，否則，令k的值增加1后回到步驟2.1；

2.6)調(diào)整卡爾曼濾波器的參數(shù)：根據(jù)實際情況對卡爾曼濾波器的誤差協(xié)方差陣p的值進行調(diào)整，從而提高定位終端發(fā)生轉(zhuǎn)彎時的定位跟蹤精度。

2.7)令k的值增加1后回到步驟2.1，繼續(xù)執(zhí)行融合地圖信息的改進卡爾曼濾波算法，直到定位跟蹤結(jié)束。

本發(fā)明與現(xiàn)有技術(shù)相比具有以下優(yōu)點：

由于本發(fā)明結(jié)合了地圖信息對定位終端是否到達轉(zhuǎn)角處進行判斷，并利用速度的角度變化判斷出定位終端是否發(fā)生轉(zhuǎn)彎，如果發(fā)生轉(zhuǎn)彎則自動對卡爾曼濾波器的參數(shù)進行調(diào)整，從而改進了室內(nèi)定位跟蹤效果，特別是在發(fā)生轉(zhuǎn)彎時，有明顯的改進作用，達到了提升定位跟蹤精度的目的。

附圖說明

圖1是本發(fā)明提供的融合地圖信息的改進卡爾曼濾波器室內(nèi)定位跟蹤方法流程圖；

圖2是誤差協(xié)方差p和卡爾曼增益gk隨著時間的變化示意圖；

圖3是融合地圖信息的卡爾曼濾波算法的跟蹤軌跡；

圖4是融合地圖信息的卡爾曼濾波算法與其他算法結(jié)果比較圖。

具體實施方式

為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點更加清楚明白，以下結(jié)合實施例，對本發(fā)明進行進一步詳細說明。應(yīng)當理解，此處所描述的具體實施例僅僅用以解釋本發(fā)明，并不用于限定本發(fā)明。

下面將結(jié)合附圖，對本發(fā)明的優(yōu)選實例進行詳細的描述。

在室內(nèi)走廊的環(huán)境下，由于衛(wèi)星信號無法穿透建筑，因此衛(wèi)星定位技術(shù)目前還不能滿足人們對于室內(nèi)定位的需求。目前基于wifi的室內(nèi)定位一般是先在不同的坐標點對wifi信號的名稱與強度進行檢測與記錄，形成數(shù)據(jù)庫，然后在定位階段，用戶利用收到的wifi信息對應(yīng)到具體坐標，從而得到定位結(jié)果。利用卡爾曼濾波器可以在連續(xù)運動中對定位結(jié)果進行修正，對人員進行跟蹤。但是由于卡爾曼濾波器的方法在拐角處會產(chǎn)生較大誤差，為此，本發(fā)明專門提出一種融合地圖信息的改進卡爾曼濾波器定位跟蹤方法，對定位與跟蹤結(jié)果進行修正。

實現(xiàn)本發(fā)明的技術(shù)思路是：融合地圖信息信息，遇到拐角時，如果手機發(fā)生轉(zhuǎn)彎，調(diào)整卡爾曼濾波器的參數(shù)，以改善其跟蹤精度。其實現(xiàn)方案如下：

1)確定基本離散卡爾曼濾波器方程和對應(yīng)的變量。

1.1)卡爾曼濾波器的狀態(tài)向量為：

x＝[sx,vx,sy,vy]^t

其中sx和vx分別表示手機的運動的x方向的位置和速度，sy和vy分別表示移動終端設(shè)備的運動的y方向的位置和速度。

1.2)由于用戶手持移動終端設(shè)備在定位時只有定位結(jié)果，而沒有輸出速度信息，因此觀測量只有sx和sy，從而量測陣為：

1.3)由于本發(fā)明定位跟蹤系統(tǒng)設(shè)定為各個直線段勻速運動，因此狀態(tài)方程為：

1.4)在定位時，前后兩次定位間隔為1秒的情況下，δt＝1，此時狀態(tài)轉(zhuǎn)移矩陣為：

1.5)最小均方誤差陣p的初始值的選取沒有特別要求，在卡爾曼濾波過程中p矩陣會趨于穩(wěn)定，在這里取最小均方誤差陣p為：

1.6)在卡爾曼濾波器的實際應(yīng)用時，測量噪聲的協(xié)方差r一般可以直接觀測得到，是濾波器的已知條件。但是激勵噪聲協(xié)方差陣q無法直接得到，因為無法直接觀測到過程信號xk。在這里通過q給過程信號“注入”足夠的不確定性，建立一個簡單的過程模型，從而產(chǎn)生一個可以接受的結(jié)果。在q和r都是常量的情況下，過程估計誤差協(xié)方差陣r和卡爾曼濾波增益kk都會快速收斂，并且保持為常量。在這里，我們?nèi)和r分別為：

2)結(jié)合地圖對卡爾曼濾波器跟蹤算法進行優(yōu)化。

2.1)計算第k個時刻卡爾曼濾波器的時間更新方程：

其中為k時刻的預(yù)測狀態(tài)向量，為k-1時刻卡爾曼濾波器輸出的狀態(tài)向量，a為狀態(tài)轉(zhuǎn)移矩陣，b是控制輸入矩陣，uk-1為k-1時刻的驅(qū)動輸入向量，t表示矩陣的轉(zhuǎn)置；

2.2)計算第k個時刻卡爾曼濾波器的測量更新方程：

其中g(shù)k表示第k個時刻的卡爾曼濾波增益，zk為第k個時刻的觀測向量，i為單位矩陣；

2.3)如果定位跟蹤結(jié)束，則退出跟蹤算法，否則，執(zhí)行步驟2.4；

2.4)判斷定位終端是否到達拐角附近：卡爾曼濾波器每輸出一個位置估計都會計算與各個拐角的距離dk，如果dk小于某一個閾值，則認為移動終端到達拐角附近，存在轉(zhuǎn)彎的可能，在本實施例中閾值dk取0.5米。如果到達拐角附近，執(zhí)行步驟2.5，否則，令k的值增加1后回到步驟2.1；

2.5)判斷定位終端是否發(fā)生轉(zhuǎn)彎，如果發(fā)生轉(zhuǎn)彎，執(zhí)行步驟2.6，否則，令k的值增加1后回到步驟2.1；判斷是否發(fā)生轉(zhuǎn)彎的具體步驟如下：

2.5.a)通過卡爾曼濾波器估計得到的x方向和y方向的速度計算出k時刻定位終端的速度方向dirk；

2.5.b)對速度方向進行算數(shù)平均濾波；

其中dirk表示k時刻未經(jīng)過算數(shù)平均濾波之前的速度方向，表示算數(shù)平均濾波輸出的k時刻的速度方向，n表示算數(shù)平均濾波器的階數(shù)，階數(shù)越大，濾波得到的速度方向越平滑，但是會導(dǎo)致判別轉(zhuǎn)彎越不敏感，本實施例中取算數(shù)平均濾波器的階數(shù)n為2；

2.5.c)判斷速度角度的變化：

其中表示卡爾曼濾波器估計得到的速度角度的變化，如果大于某個角度閾值，那么認為定位終端發(fā)生了轉(zhuǎn)彎，否則，認為定位終端沒有發(fā)生轉(zhuǎn)彎。在本實施例中角度閾值取5度。

2.6)調(diào)整卡爾曼濾波器的參數(shù)：根據(jù)實際情況對卡爾曼濾波器的誤差協(xié)方差陣p的值進行調(diào)整，從而提高定位終端發(fā)生轉(zhuǎn)彎時的定位跟蹤精度。在本實施例中，如果判別到移動設(shè)備發(fā)生轉(zhuǎn)彎時，令誤差協(xié)方差矩陣p恢復(fù)為初始值：

2.7)令k的值增加1后回到步驟2.1，繼續(xù)執(zhí)行融合地圖信息的改進卡爾曼濾波算法，直到定位跟蹤結(jié)束。

用本實施例與現(xiàn)有的標準卡爾曼濾波算法進行定位跟蹤的效果對比。

為了驗證本發(fā)明所提融合地圖信息的改進卡爾曼濾波器定位跟蹤方法的性能，對所提方法進行了仿真實驗，在50x50米的區(qū)域內(nèi)，假設(shè)用戶手持定位終端以勻速走過帶90度轉(zhuǎn)角的軌跡，軌跡的起點坐標為(1.5,0.25)，三個轉(zhuǎn)角的坐標分別為(1.5,48.75)，(48.5,48.75)和(48.5,1.25)，在各個直線段做勻速直線運動。

本發(fā)明中誤差協(xié)方差p和卡爾曼增益gk隨著時間的變化如圖2所示。從圖中可以看出，本發(fā)明的方法可以準確的判別出四次轉(zhuǎn)彎，并在每次轉(zhuǎn)彎時通過增大誤差協(xié)方差p的值來增大卡爾曼增益gk的值，從而使拐角處的卡爾曼濾波輸出更加接近于觀測向量。

本發(fā)明所提融合地圖信息的改進卡爾曼濾波器定位跟蹤方法軌跡如圖3所示。

從圖4中可以看出，本發(fā)明所提算法的濾波軌跡在拐角處的誤差明顯減小，跟蹤效果明顯提升。對各算法的定位跟蹤誤差進行統(tǒng)計，得到誤差的cdf曲線如圖所示。經(jīng)過濾波后的定位跟蹤誤差大大減小，誤差在1米以內(nèi)的概率達到90％。

本發(fā)明所提的融合地圖信息的改進卡爾曼濾波器定位跟蹤方法雖然整體的定位跟蹤精度提高較為有限，但是在拐角處對跟蹤效果的改善非常明顯，達到了所提定位跟蹤方法的目的。

綜上，本發(fā)明融合地圖信息的改進卡爾曼濾波器定位跟蹤方法在拐角處有明顯的改進效果，提升了定位與跟蹤的精度。

以上所述僅為本發(fā)明的較佳實施例而已，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進等，均應(yīng)包含在本發(fā)明的保護范圍之內(nèi)。