基于卡爾曼濾波的室內運動目標實時定位算法

文/宋財華 祝向輝 游菊芬 萬建云

近年來，隨著無線通信和智能移動終端的快速發展，基于位置的服務(Location-based Services, LBS)有了越來越廣泛的應用，人們對位置信息及時、快速、準確獲取的需求也越來越強烈，從而促使定位技術的不斷提高。目前定位服務應用較多的場景是通過獲取移動終端的位置，為大型商場導購、醫療監護、社交娛樂以及其他的應用場合提供實時精確的位置信息。例如，在商場等大型公共室內場所，用戶可以利用室內定位快速熟悉商場環境，以便準確找到目的地。同時，商家可以根據用戶位置實時推送店鋪信息，吸引顧客。由于互聯網的快速發展和普及，尤其是信息時代的到來，無線局域網(Wireless Local Area Network,WLAN)實現了在全球范圍內的迅速普及和應用。WLAN目前在大部分的室內環境中都有部署，鑒于其諸多優點，例如：硬件成本低、傳輸速率高等，尤其是現代社會對WLAN的需求，更是加速了WLAN的快速發展，廣大機構和公司都進行了應用，主要是人群密集的室內場所，例如：醫院、商場、校園、酒店、高檔寫字樓和家庭等，用戶只要持有帶有感知無線熱點的移動設備，即可通過WLAN來獲取所需的位置信息。這些都給基于位置的服務應用提供了極大的便利性，同時也存在巨大的市場，用戶在享受WLAN定位帶來的方便快捷的同時，也促進了WLAN室內定位的進一步發展。

雖然WLAN定位技術在醫療、商業以及安全等行業有著廣泛的應用價值，但是目前的WLAN定位技術還存在諸多問題，有待深入的研究。基于WLAN的室內定位的難點由室內的復雜環境造成，接收信號不穩定，有噪聲，以及普遍存在的多徑效應等各個方面的影響，影響了定位的準確性。

室內的WLAN環境對于定位算法來說至關重要，但是由于室內的復雜環境，RSS信號的波動，文獻[7]提出的ISVR算法只考慮當前位置與測量到的信號之間的關系，而沒有考慮位置之間的相關性，而且在運動過程中ISVR采集到的RSS數值少，因此在待定位目標運動時定位誤差增大。針對室內實時定位的問題，本文提出了基于卡爾曼濾波的室內運動目標實時定位算法。

圖1：基于卡爾曼濾波的室內運動目標實時定位算法總體框架

圖2：實驗環境

圖3：Q=0.025,R=0.5時的定位軌跡

1 定位算法介紹

1.1 幾種常用的濾波方法

1.1.1 均值濾波

均值濾波是一種簡單實用的濾波方法，根據采集到的RSS信號集合{RSS1, RSS2, …,RSSn}，對其進行取平均值，如下式：

其中，RSSi是采集到的RSS信號，RSSavg是計算得到的RSS平均值。

1.1.2 粒子濾波

粒子濾波指的是在傳播的狀態空間中選取一組樣本數據，稱之為“粒子”，以選取的“粒子”的均值替代計算過程中的積分運算，以最小方差估計為目標，完成對概率密度的近似。具體來說，首先選取數量為n的“粒子”，根據上一時刻k-1的系統概率密度函數以及結合當前時刻k的測量結果，經過狀態和時間方程的計算，可以得到當前時刻k的n個“粒子”的概率密度，可以近似為粒子濾波可以隨著粒子數目的增加，近似的結果更加準確，逐漸逼近系統狀態的概率密度函數，此時得到的粒子濾波的結果可以稱之為最優貝葉斯估計。

1.1.3 卡爾曼濾波

卡爾曼濾波可以根據測量到的數據預測系統的運動狀態，在解決狀態空間預測估計的問題有巨大的幫助，而且所需的數據只需上一時刻和當前時刻的測量值，并不需要之前的歷史數據。

首先，我們先確定一個由線性微分方程組成的離散控制系統：

系統的測量值：

式中，A表示狀態轉移矩陣；B是系統參數；U(k)是k時刻對系統的控制量；H是測量系統的參數；W(k)表示過程噪聲，V(k)表示測量噪聲，被假設成高斯白噪聲，系統狀態噪聲的協方差是Q，測量噪聲的協方差是R。

假設系統當前時刻是k，根據系統的狀態方程，可以估計預測下一時刻k-1系統的狀態值。由系統的狀態方程預測出的當前時刻的狀態為：

式（4）中，是根據時刻k-1估計的當前時刻k的結果。經過式（4）的估計預測，系統的狀態更新到當前狀態值，還需要對時刻k的協方差進行更新，則當前時刻的協方差P為：

式（5）中，是時刻k的系統狀態的協方差，是時刻k-1的系統狀態的協方差，矩陣AT是矩陣A的轉置，Q是系統過程的協方差。根據式（4），（5）的計算，完成對系統狀態的估計預測，結合當前時刻k的測量值計算得到時刻k的最優卡爾曼濾波值

式中Kg是卡爾曼增益(Kalman Gain)：

式（7）中，Kg(k)是k時刻的卡爾曼增益，HT表示H的轉置矩陣，R是測量噪聲的協方差。

根據上面幾個式子的計算，可以計算得到當前時刻k的最優估計，但是卡爾曼濾波是一個不斷遞歸循環的濾波器，因此時刻k的協方差為：

其中I為n×n的單位矩陣，計算的結果是下一時刻k+1的輸入協方差。根據上述的五個式子，卡爾曼濾波可以不斷的遞歸運算下去。

在室內運動目標實時定位過程中，通常需要在較短的時間內完成定位，求出用戶的位置。但是，在較短的時間內采集到的RSS信息較少且樣本之間會有波動影響，導致估計出來的位置與實際的位置之間有較大的偏差，從而表現出位置波動的情況，不能真實的反應用戶實際的運動軌跡，顯然這與實際的定位軌跡是不符合的。卡爾曼濾波迭代算法將觀測值作為輸入，卡爾曼濾波器的輸出作為系統狀態的估計值輸出，因此可以糾正位置波動較大的計算結果，從而降低定位誤差的偏差，提高定位精度。

同時，卡爾曼濾波參數對定位結果也有影響。首先，文獻[11]提出自適應卡爾曼濾波算法，通過對狀態噪聲Q和測量噪聲R進行分析討論，發現狀態噪聲Q和測量噪聲R對卡爾曼增益有著重要的影響，而卡爾曼增益又決定了濾波器的反應速度，當運動目標處于保持當前狀態運動時，較小的卡爾曼增益就可以得到很好的濾波效果，反之，較大的卡爾曼增益可以對目標運動狀態發生變化時作出快速的反應，因此通過引入一個縮放因子Ф(k)來自適應的調整狀態噪聲Q，進而調整卡爾曼增益來實現自適應；其次，在文獻[12]中提出自適應擴展卡爾曼濾波器的載波跟蹤算法，在濾波的同時根據測量值的信息，在線實時估計狀態噪聲Q和測量噪聲R，以此改變卡爾曼增益的數值，實現自適應擴展卡爾曼濾波器的載波跟蹤算法。

通過上述的介紹分析可以看出：在實時定位中需要考慮前后位置之間的時間相關性，不單單是對采集的RSS進行濾波，因此均值濾波并不適用于實時定位中。粒子濾波的缺點是粒子的選取、迭代的計算量大，時間復雜度高，而且在迭代過程中會有粒子退化的現象，在實時定位中會出現滯后的情況，因此在實時定位中難以實施。而卡爾曼濾波以最小均方誤差為目標，可以從含有噪聲的測量值估計系統的狀態，且不需要存儲歷史數據，存儲量小，計算復雜度較低，因此本文采用卡爾曼濾波作為室內實時定位算法的研究對象。

1.2 基于卡爾曼濾波的室內運動目標實時定位算法

1.2.1 算法流程

通過上一小節的卡爾曼濾波介紹，本小節會將卡爾曼濾波應用到WLAN室內的實時定位當中。假設室內攜帶移動終端的用戶在室內平面的區域內進行，并且其移動方式為勻速運動，此時可以將用戶的移動速度分解成水平vx和垂直vy兩個方向的分量，根據卡爾曼濾波的位置狀態方程，可以列出用戶的狀態向量：

式中，px、py是用戶移動時的位置坐標x、y；vx、vy是用戶移動時水平方向的速度分量和垂直方向的速度分量。

由此可以得到系統的狀態方程：

式中，

是狀態轉移矩陣A；△t是第k時刻和第k-1時刻之間的時間間隔，在本文中由于信號采集的頻率為2次/秒，因此設置兩次位置之間的時間間隔△t取1。

觀測矩陣z(k)為：

式中，是測量矩陣H；

是k時刻的測量值。

狀態噪聲協方差矩陣W為：

測量噪聲協方差矩陣V為：

在卡爾曼濾波開始迭代求解最優輸出之前需要對系統進行初始化，由于卡爾曼濾波器會不斷的對預測值進行預測和修正，因此初始值的設置對最終系統的定位跟蹤影響不是很大。本文對卡爾曼濾波目標實時定位算法初始狀態的設置為：

初始協方差：

1.2.2 算法總體架構

基于卡爾曼濾波的室內運動目標實時定位算法的總體框架如圖1所示。

圖4：Q=0.000025,R=0.5時的定位軌跡

圖5：改進后的卡爾曼濾波處理步驟

圖6：改進后的卡爾曼濾波定位軌跡

定位主要步驟如下，其中step1～step3為離線訓練階段，step4～step6為在線定位階段：

Step1在定位區域選定參考位置點，在每個參考位置點處采集來自不同AP的一定數量的RSS指紋值，組成RSS位置指紋數據庫；

Step2對采集到的原始RSS指紋進行預處理，得到新的RSS位置指紋數據庫；

Step3利用ISVR構建參考點的信號強度與其位置坐標之間的非線性關系，得到離線訓練模型；

Step4用戶利用持有的移動終端實時采集周圍的信號強度RSS，得到在線階段的信號強度向量；

Step5利用ISVR算法計算用戶的位置坐標，得到卡爾曼濾波的測量值；

Step6初始化卡爾曼濾波器，代入在線測量值；

根據系統預測值和測量值，經過卡爾曼濾波器的處理，得到用戶的實時位置。

表1：不同實時定位算法的定位誤差

2 實驗結果分析

2.1 實驗環境的建立

為驗證該文提出的基于卡爾曼濾波的室內運動目標實時定位算法的室內定位效果，選取如圖3.6所示的室內環境，長45m、寬18m，面積約為810m2。定位區域包括過道和辦公室，辦公室內有辦公設備等辦公用品，而且人員走動頻繁，干擾復雜多變，因此這個環境下的數據比較有代表性。實驗軟件平臺是基于Android開發的Wi-Fi Scanner采集各個AP的信號值，硬件平臺是HUAWEI P8和10個型號為MERCURY MW325R的無線路由設備。本節中移動目標在該實驗場景中的真實移動軌跡如圖2中虛線所示，移動目標從206室出發，沿著圖中黑色虛線箭頭方向勻速走到門口，然后以直線勻速的方式走到236室的門口，在行走的途中采集周圍無線AP的RSS信息。

2.1 結果分析

本文在實驗的過程中發現，卡爾曼濾波的系統狀態噪聲Q和測量噪聲R的取值對實時定位算法的精度有較大的影響。在實驗過程中，利用對卡爾曼濾波取不同的參數組合，觀察在不同的參數下系統定位的結果，并做了統計和分析，基于分析結果提出了基于卡爾曼濾波的室內運動目標實時定位算法，最后比較了基于卡爾曼濾波的室內運動目標實時定位算法與ISVR算法的定位性能。

2.1.1 參數分析

在本次實驗中，根據ISVR算法的觀測結果，分別觀察卡爾曼濾波參數在測量噪聲R=0.5，狀態噪聲Q=0.025，Q=0.000025這兩組不同參數下的定位結果。

從圖3和圖4中可以看出，狀態噪聲Q和測量噪聲R取不同值時，當Q=0.025，R=0.5，即Q/R值較大時，實時定位的軌跡在拐角處有較明顯的方向改變，定位相對來說比較符合正常的軌跡路線；當Q=0.000025，R=0.5時，也就是Q/R較小時，實時定位的軌跡相對來說在直線部分比較準確，可以正確的反應當前的運動軌跡，但在拐角處相對來說定位軌跡有點滯后。基于此，結合卡爾曼濾波的原理，由于Q和R分別代表的是狀態噪聲和測量噪聲，Q/R的比值較大時，系統的狀態噪聲Q相對較大，因此實時定位的結果更接近測量值；當Q/R的比值較小時，即測量噪聲比狀態噪聲大的多，實時定位的結果更接近或者更傾向于系統的狀態過程，因此當Q/R的比值較大時，實時定位的軌跡路線更穩定，更連續。

根據上述的分析，可以對基于卡爾曼濾波的室內運動目標實時定位算法做相應的改進，結合室內的地圖信息，在轉角處可以設置Q/R的比值較大，不在轉角處設置Q/R的比值較小，轉角處一般在門口或者墻角轉彎處，根據室內地圖信息一般可以事先知道，在本試驗中的兩個轉角處是：

轉角處1：9.0≤x≤10，12.0≤y≤13.0轉角處2：9.0≤x≤10，8.0≤y≤9.0

因此改進后的基于卡爾曼濾波的室內運動目標實時定位算法在卡爾曼濾波階段的過程為（圖5）。

經過本文的實驗分析，驗證了文獻[11,12]中對卡爾曼濾波參數的分析。本文利用自適應卡爾曼濾波的思想，以此提出改進后的基于卡爾曼濾波的室內運動目標實時定位算法，具體改進如下：

改進后的基于卡爾曼濾波的室內運動目標實時定位算法在初始化卡爾曼濾波參數之前與前一小節的算法步驟一樣，主要的區別是狀態噪聲和測量噪聲在不同的位置設置不同的比例，結合室內的地圖信息，如果處于拐角處，則設置狀態噪聲Q=0.025，測量噪聲R=0.5；如果不是在轉角處，則設置狀態噪聲Q=0.000025，測量噪聲R=0.5。

2.1.2 算法比較分析

圖6所示是改進后的基于卡爾曼濾波的室內運動目標實時定位算法的軌跡圖，可以看出經過改進算法，實時定位軌跡比較接近實際的運動軌跡，主要原因是在拐角處設置的狀態噪聲值Q較大，定位結果更傾向于測量值，因此在拐角處不會由于狀態噪聲值Q設置的較小而造成移動軌跡滯后，在不是拐角處設置的狀態噪聲值Q較小，定位軌跡具有較好的連續性，更穩定。表1所示是不同算法的定位誤差比較，可以看出基于卡爾曼濾波的室內運動目標實時定位算法在目標移動的時候定位精度明顯高于其他算法，由于ISVR算法在目標移動時只能采集少數的RSS，而且沒有考慮運動軌跡之間的時間相關性，因此定位誤差較大，誤差標準差也較大。當卡爾曼濾波的參數取不同值的時候定位誤差也不同，狀態噪聲Q較大時，定位誤差較大，接近于ISVR算法的定位誤差，狀態噪聲Q逐漸減小，定位軌跡相對穩定，定位誤差有減小的趨勢。本文提出的改進算法的定位精度明顯高于其他定位算法，主要原因是改進的算法根據是否在轉角處設置不同的卡爾曼濾波參數，可以在直線時更穩定，在拐角處準確的轉向，提高了定位精度。

3 結語

本文針對室內運動目標實時定位時采集的RSS較少，對實時性的要求較高，以及需要考慮位置之間的時間相關性，提出一種基于卡爾曼濾波的室內運動目標實時定位算法。實驗結果表明，在室內運動目標的定位中，相較于ISVR算法，提出的算法可以降低定位時的波動，定位精度相較改進前提高了20%以上。

參考文獻

[1]GU Y Y,LO A,NEIMEGEERS I.A survey of indoor position system for wireless personal networks[J].IEEE Communications Surveys &Tutorials,2009,11(01):13-32.

[2]HE S,CHAN S H G.Wi-Fi fingerprintbased indoor positioning:Recent advances and comparisons[J].IEEE Communications Surveys &Tutorials,2016,18(01):466-490.

[3]HAQUE I T,ASSI C.Profiling-Based indoor localization schemes[J].IEEE Systems Journal,2015,9(01):76-85.

[4]ZHANG G,ZHAN X,DAN L.Research and improvement on indoor localization based on RSSI fingerprint database and K-nearest neighbor points[C].International Conference on Communications, Circuits and Systems.IEEE,2013:68-71.

[5]Mohammed Y E,Abdallah A S,Liu Y.Measurements of in-building penetration loss at 2.4GHz[J].Journal of Beijing University of Posts and Telecommunicatio ns,2004,27(02):98-102.

[6]Lin T N,Fang S H,Tseng W H,et al.A Group-Discrimination-Based Access Point Selection for WLAN Fingerprinting Localization.IEEE Transactions on Vehicular Technology,2014,8(63):3967-3976

[7]姚英彪,毛偉勇,姚瑞麗等.基于改進支持向量回歸的室內定位算法[J].儀器儀表學報,2017,38(09):2112-2119.

[8]Scholl P M,Kohlbrecher S,Sachidananda V,et al.Fast indoor radio-map building for RSSI-based localization systems[M].2012.

[9]胡士強, 敬忠良. 粒子濾波算法綜述[J].控制與決策,2005,20(04):361-365.

[10]Zarchan P,Zarchan P.Fundamentals of Kalman Filtering[J].Progress in Astronautics &Aeronautics,2015,190(08):83.

[11]張謙,景占榮.一種過程噪聲自適應調節的卡爾曼濾波算法[J].電子測量技術,2007,30(05):18-20.

[12]李理敏,龔文斌,劉會杰等.基于自適應擴展卡爾曼濾波的載波跟蹤算法[J].航空學報,2012,33(07):1319-1328.