本發(fā)明涉及定位技術領域，特別涉及一種室內(nèi)定位系統(tǒng)。

背景技術：
隨著智能終端的廣泛使用，移動通信快速發(fā)展，同時數(shù)據(jù)和多媒體業(yè)務迅速增加，基于位置的服務(LocationBasedServices，LBS)逐漸受到重視。尤其是作為定位技術的最后一米，室內(nèi)定位技術有著廣泛的應用前景。比如在地下停車場，可以借助室內(nèi)定位技術快速找到停車位；在大型商場，可以借助室內(nèi)定位技術快速找到出口、電梯，同時商家也可以借助于室內(nèi)定位向用戶介紹商品信息；在火災現(xiàn)場，消防人員可以借助室內(nèi)定位技術快速確定被困人員的位置，從而迅速進行救援；在醫(yī)院，可以利用室內(nèi)定位技術對需要看護的病人進行定位等等。在室外，全球衛(wèi)星導航系統(tǒng)(GlobalNavigationSatelliteSystem，GNSS)基本上可以滿足定位精度的要求，但是在室內(nèi)環(huán)境下，由于衛(wèi)星信號受到遮擋且存在嚴重的多徑效應，使得衛(wèi)星定位精度嚴重下降，甚至出現(xiàn)無法正常定位的情況。目前，針對復雜的室內(nèi)環(huán)境，現(xiàn)有的室內(nèi)定位技術主要分為以下幾種1)射頻識別定位技術射頻識別(RadioFrequencyIdentification，RFID)定位技術是一種利用射頻信號自動識別目標對象并獲取相關信息的技術。實現(xiàn)無接觸信息傳遞并達到識別對象的目的。RFID定位系統(tǒng)包括電子標簽和讀卡器等，該系統(tǒng)利用標簽對物體的唯一標識特性，通過讀卡器接收到的電子標簽發(fā)送的信號來獲得電子標簽的位置信息。RFID定位系統(tǒng)優(yōu)點是體積小、造價低，但是作用范圍小、不具有通信能力，且不便整合到其它系統(tǒng)中。2)藍牙定位技術藍牙(Bluetooth)技術是一種支持語音和數(shù)據(jù)業(yè)務的短距離、低功耗無線傳輸技術，并可以實現(xiàn)短距離不同設備之間的互聯(lián)?；谒{牙的室內(nèi)定位技術，利用經(jīng)驗測試與信號傳播模型結(jié)合的方式，通過實時測量的信號強度進行定位。藍牙定位技術的優(yōu)點在于設備體積小，易于集成在移動智能終端上面。理論上，只要集成了藍牙的移動智能終端且藍牙功能打開，藍牙定位系統(tǒng)就能夠?qū)ζ溥M行定位。藍牙定位技術的缺點在于藍牙器件設備的價格比較昂貴，且對于復雜的室內(nèi)環(huán)境而言，藍牙定位系統(tǒng)的穩(wěn)定性比較差。3)紅外線定位技術紅外線(InfraredRay，IR)是一種介于微波和可見光之間的電磁波。IR室內(nèi)定位方法通過發(fā)射調(diào)制的紅外射線，利用安裝在室內(nèi)的光學傳感器接收進行定位。IR定位具有相對較高的定位精度，但由于光線不能穿過障礙物，使得IR具有傳輸距離短的缺點，且需要在室內(nèi)的每個房間和樓層安裝設備，成本較高。4)無線局域網(wǎng)定位技術無線局域網(wǎng)(WirelessLocalAreaNetwork，WLAN)是基于IEEE802.11標準，以無線信道作為傳輸媒介的局域網(wǎng)。基于WLAN的定位技術主要包括信號傳播模型法和位置指紋法，都是利用接收信號的信號強度指示(ReceivedSignalStrengthIndication，RSSI)進行位置解算。信號傳播模型法利用室內(nèi)信號的傳播特性，考慮墻壁和地板引起的衰減，建立RSSI與傳播距離d之間的數(shù)學模型，實現(xiàn)位置解算。但是由于復雜的室內(nèi)環(huán)境，很難建立準確的室內(nèi)信號傳播模型，因此利用該方法進行位置解算精度有限?；赪LAN的位置指紋定位技術包括離線階段和在線階段兩個階段，離線階段在需要定位的環(huán)境采集相應的RSSI信息并存入位置指紋數(shù)據(jù)庫，在線階段利用實時測得的RSSI信息與位置指紋數(shù)據(jù)庫進行匹配，將匹配最佳位置輸出作為定位結(jié)果?；赪LAN的位置指紋定位技術具有覆蓋范圍廣、可用性強、低成本、長時間定位精度高等優(yōu)點。但是由于多徑干擾等引起的RSSI擾動等因素嚴重影響了WLAN的定位精度。5)MEMS傳感器自主定位技術隨著微機電系統(tǒng)(Micro-Electro-MechanicalSystems，MEMS)技術的快速發(fā)展，各種傳感器，如陀螺儀、加速度計、磁力計、氣壓計等，尺寸不斷變小，成本不斷變低，被廣泛應用于各種移動智能終端設備中。基于MEMS傳感器的定位技術具有自主性和連續(xù)性，且目前基于MEMS傳感器的個人導航系統(tǒng)主要有兩種：傳統(tǒng)的慣性積分導航和行人航跡推算導航。傳統(tǒng)的慣性積分導航，根據(jù)牛頓運動定律，利用測得3軸加速度計和3軸陀螺儀的數(shù)據(jù)積分計算出3維姿態(tài)、速度和位置。理論上這種方式定位精度會很高，但是實際應用中，由于慣性傳感器自身的漂移、測量噪聲、行人的抖動等因素影響，多次積分后會產(chǎn)生累積誤差，幾十秒之內(nèi)誤差可達幾十米，甚至幾百米?；谛腥撕桔E推算(PedestrianDeadReckoning，PDR)方法利用行人的位移和航向進行位置解算，相比于傳統(tǒng)的慣性積分導航更準確且更適用于行人導航，但是其定位依然受步長、航向、步態(tài)檢測精度影響，隨著時間的增加，依然存在累積誤差。

技術實現(xiàn)要素：
本發(fā)明針對現(xiàn)有室內(nèi)定位技術存在的缺陷和不足，結(jié)合WLAN位置指紋定位和MEMS傳感器定位，提出一種高精度室內(nèi)WLAN/MEMS融合跨樓層3維定位方法，實現(xiàn)室內(nèi)空間的3維定位。該方法及系統(tǒng)能夠有效的克服MEMS器件定位存在的累積誤差和WLAN定位中存在的RSSI擾動對系統(tǒng)定位精度帶來的影響。本發(fā)明方法及系統(tǒng)利用擴展卡爾曼濾波算法中的一步預測位置信息修正觀測的RSSI，修正由于RSSI擾動給WLAN定位精度帶來的影響；同時利用WLAN定位結(jié)果補償MEMS的累積誤差，提高定位精度。且該方法還綜合利用氣壓計、地理位置等解算高度，實現(xiàn)室內(nèi)跨樓層3維定位。本發(fā)明的技術方案如下：本發(fā)明提供一種室內(nèi)WLAN/MEMS融合跨樓層3維定位方法，其特征在于，所述方法包括：(1)選擇WLAN位置指紋數(shù)據(jù)庫：計算前一時刻得到的行人的3維位置距每個樓梯口的物理距離，利用計算出的物理距離是否大于某個閾值判斷行人是否進出樓梯間；如果判斷出沒有進出樓梯間，則位置指紋數(shù)據(jù)庫不變，與前一時刻的位置指紋數(shù)據(jù)庫相同；如果進入樓梯間，則選擇樓梯間的位置指紋數(shù)據(jù)庫，如果出樓梯間，則判斷為在樓層中，此時還需利用前一時刻的實際高度判斷所處樓層，選擇相應樓層的位置指紋數(shù)據(jù)庫。(2)計算行人的速度和航向：根據(jù)采集到的MEMS數(shù)據(jù)，包括3軸加速度計、3軸陀螺儀和3軸磁力計數(shù)據(jù)，利用步態(tài)檢測和數(shù)據(jù)更新速率方法計算速度，四元素更新姿態(tài)矩陣方法計算航向。(3)WLAN位置指紋定位：首先利用抗差擴展卡爾曼組合導航濾波器中的一步預測信息反匹配位置指紋數(shù)據(jù)庫修正觀測得到RSSI，然后利用修正的RSSI結(jié)合WKNN定位算法進行位置解算，克服由于RSSI擾動對WLAN位置指紋定位精度帶來的影響。同時將抗差M估計引入室內(nèi)融合定位算法中，形成抗差擴展卡爾曼濾波器，解決定位在實際環(huán)境下受到粗差、干擾、模型誤差等因素的影響，導致位置精度的嚴重下降。(4)將計算得到的速度、航向和WLAN指紋定位的位置一起帶入抗差擴展卡爾曼組合導航濾波器，解算得到最優(yōu)的二維位置坐標。(5)高度計算：利用氣壓計輸出數(shù)據(jù)計算海拔高度，實際的高度信息則通過判斷上下樓進行高度的累加或者累減進行計算，從而實現(xiàn)室內(nèi)跨樓層3為定位。所述步驟(3)使用的抗差擴展卡爾曼組合導航濾波器的設計方法是：通過建立觀測方程和狀態(tài)方程，設計擴展卡爾曼濾波器(EKF)輸出最優(yōu)位置信息，并同時引入抗差M估計到室內(nèi)融合定位算法中，形成抗差擴展卡爾曼濾波器。進一步，所述狀態(tài)變量和狀態(tài)方程如下：選用東位置、北位置、速度和航向作為狀態(tài)變量，即狀態(tài)方程如下：式中xt表示t時刻待估計的東位置，yt表示t時刻待估計的北位置，vt表示t時刻待估計的速度，表示t時刻待估計的航向；xt-1表示t-1時刻的東位置，yt-1表示t-1時刻的北位置，vt-1表示t-1時刻的速度，表示t-1時刻的航向角，Wt-1表示過程噪聲，是均值為0的高斯白噪聲序列。進一步，所述的觀測變量和觀測方程如下：選用WLAN輸出的位置信息、MEMS輸出的速度和航向信息作為觀測量，即觀測方程如下：式中表示t時刻WLAN定位出的東位置，表示t時刻WLAN定位出的北位置，表示t時刻MEMS輸出的速度，表示t時刻MEMS輸出的航向；xt表示t時刻待估計的東位置，yt表示t時刻待估計的北位置，vt表示t時刻待估計的速度，表示t時刻待估計的航向，Vt表示觀測噪聲，是均值為0的高斯白噪聲序列。進一步，所述步驟(3)修正RSSI的方法具體如下：定義t(t＝1,2,K)時刻擴展卡爾曼預測位置到WLAN位置指紋數(shù)據(jù)庫中參考點的物理距離為式中(xj,yj)為位置指紋數(shù)據(jù)庫中第j個參考點的位置，n為位置指紋庫中參考點總數(shù)，在計算出t時刻到所有參考點的距離Dtj后，選取K(K>1)個物理距離最小的參考點對應的RSSI向量，利用類似WKNN算法得到預測的RSSI向量為式中RSSI1為得到的RSSI預測向量，RSSIm為t時刻K個與擴展卡爾曼預測位置物理距離最小的參考點中第m點對應的RSSI向量，Dtm為其對應的物理距離，為t時刻K個與擴展卡爾曼預測位置物理距離最小的參考點對應的物理距離的和，式中q為索引號；得到RSSI1后，對實測的RSSI2進行修正：RSSIt＝w×RSSI1+(1-w)×RSSI2(30)式中RSSIt為t時刻最終的RSSI向量，w為加權(quán)系數(shù)，取值范圍為0≤w≤1。更進一步，所述步驟(2)中MEMS輸出的速度具體解算如下：首先計算加速度計輸出的模值和分別表示加速度計輸出的3軸加速度。然后通過檢測該模值Anorm為峰值且大于某個設定的閾值A0來判斷跨步，完成步態(tài)檢測。最后假設MEMS傳感器的輸出頻率為fs，且通過檢測加速度計輸出模值Anorm連續(xù)兩個大于閾值A0的峰值之間的數(shù)據(jù)點數(shù)ΔN，可以計算第k步所需的時間為假設行人每一步為勻速運動，則可以計算出第k步的速度為式中Pk為第k步的步長，vk為第k步計算出的速度。令第k步中每個采樣時刻的速度都為vk，則每一秒可計算出fs個速度，取每一秒計算出的所有速度的均值作為該秒計算出的速度。進一步，所述步驟(5)的高度計算中，僅利用氣壓計測量的高度值判斷上下樓，而實際的高度信息則通過判斷上下樓進行高度的累加或者累減進行計算，具體方法如下：將行人行走分為3種情況：樓層中行走、上樓梯和下樓梯；通過地理位置信息，判斷出行人所處的大概位置(在樓層中或者在樓梯間)，如果在樓梯間，通過高度計測量的海拔高度信息判斷行人上樓梯或者下樓梯，實現(xiàn)高度的累加或者累減，如果在樓層中，則將所處樓層的高度直接賦值給當前時刻的高度輸出，實現(xiàn)室內(nèi)空間跨樓層3維定位。更進一步，所述的判斷出行人所處的大概位置(在樓層中或者在樓梯間)，其具體判斷如下：測試測試環(huán)境中所有樓梯口處的3維位置坐標為(xi,yi,zi),i＝1,2K,n，其中n為樓梯口總的個數(shù)。假設行人在時刻t-1的最優(yōu)位置為分別計算其與每一個樓梯口的物理距離為式中di為時刻t-1的最優(yōu)位置與第i個樓梯口的物理距離；設定門限r(nóng)，如果di＜r且判斷為進入樓梯間(如果上個時刻在樓層中，則判斷為進入樓梯間)，此時匹配樓梯間的指紋數(shù)據(jù)庫，并保存此時的氣壓計測量高度信息h2；如果di＜r且判斷為出樓梯間(如果上個時刻在樓梯間，則判斷為出樓梯間)，此時還需利用前一時刻的實際高度信息判斷所處樓層，選擇相應的位置指紋數(shù)據(jù)庫，如果di＞r，則位置指紋數(shù)據(jù)庫不變，與前一時刻的位置指紋數(shù)據(jù)庫相同。本發(fā)明的優(yōu)點如下：1)高精度，本發(fā)明可以實現(xiàn)室內(nèi)跨樓層3維定位最小均方根誤差小于1米的高精度定位，并且本發(fā)明室內(nèi)跨樓層3維定位誤差小于1m的置信概率為75％；2)實現(xiàn)WLAN/MEMS融合定位，克服了WLAN位置指紋定位技術中存在的RSSI擾動和MEMS傳感器定位技術中存在的累積誤差對系統(tǒng)定位精度帶來的影響；3)實現(xiàn)室內(nèi)空間的跨樓層3維定位。附圖說明圖1為本發(fā)明所述系統(tǒng)的總體架構(gòu)流程圖；圖2為本發(fā)明方法中高度計算的流程圖。具體實施方式下面結(jié)合附圖和具體實施例對本發(fā)明進行詳細說明。如圖1所示，本發(fā)明提出的室內(nèi)WLAN/MEMS融合跨樓層3維定位系統(tǒng)主要包括：WLAN位置指紋定位模塊、MEMS數(shù)據(jù)處理模塊、抗差擴展卡爾曼濾波器模塊、高度計算模塊?！馱LAN位置指紋定位模塊。根據(jù)抗差擴展卡爾曼濾波器反饋的一步預測位置信息，利用位置指紋數(shù)據(jù)庫反匹配修正實測RSSI信息，同時利用修正的RSSI信息，采用加權(quán)K近鄰(WeightedK-NearestNeighborhood，WKNN)定位算法解算位置?！馦EMS傳感器數(shù)據(jù)處理模塊。根據(jù)加速度計、陀螺儀、磁力計輸出的信息，利用步態(tài)檢測、數(shù)據(jù)更新速率、四元素法更新姿態(tài)矩陣等方法計算行人的速度和航向?！窨共顢U展卡爾曼濾波器模塊。建立觀測方程和狀態(tài)方程，設計擴展卡爾曼濾波器(ExtendedKalmanFilter，EKF)輸出最優(yōu)位置信息，并同時引入抗差M估計，以解決系統(tǒng)在實際環(huán)境下受到粗差、干擾、模型誤差等因素的影響，導致位置精度嚴重下降?！窀叨扔嬎隳K。綜合利用地理位置、氣壓計輸出海拔高度和每一個樓層的實際高度實現(xiàn)高度的解算，并同時為WLAN定位選擇位置指紋數(shù)據(jù)庫。1、WLAN位置指紋定位對于室內(nèi)WLAN環(huán)境下的實時跟蹤定位來說，由于可供采集RSSI的時間較短、并且RSSI波動性很大，若只使用測量的RSSI信息來確定觀測位置信息，將會存在很大的定位誤差。解決方法為根據(jù)擴展卡爾曼濾波器反饋的一步預測位置信息，利用位置指紋數(shù)據(jù)庫反匹配修正實測RSSI信息，同時利用修正的RSSI信息，采用加權(quán)K近鄰(WeightedK-NearestNeighborhood，WKNN)定位算法解算位置。(1)反匹配修正實測RSSI定義t(t＝1,2,K)時刻擴展卡爾曼預測位置到WLAN位置指紋數(shù)據(jù)庫中參考點的物理距離為式中(xj,yj)為位置指紋數(shù)據(jù)庫中第j個參考點的位置，n為位置指紋庫中參考點總數(shù)。在計算出t時刻到所有參考點的距離Dtj后，選取K(K>1)個物理距離最小的參考點對應的RSSI向量，利用類似WKNN算法得到預測的RSSI向量為式中RSSI1為得到的RSSI預測向量，RSSIm為t時刻K個與擴展卡爾曼預測位置物理距離最小的參考點中第m點對應的RSSI向量，Dtm為其對應的物理距離，為t時刻K個與擴展卡爾曼預測位置物理距離最小的參考點對應的物理距離的和，式中q為索引號。得到RSSI1過后，對實測的RSSI2進行修正有RSSIt＝w×RSSI1+(1-w)×RSSI2(3)式中RSSIt為t時刻最終的RSSI向量，w為加權(quán)系數(shù)，取值范圍為0≤w≤1。(2)WKNN定位算法加權(quán)K近鄰(WeightedK-NearestNeighborhood，WKNN)定位算法在定位精度和計算復雜度上有一個良好的折中，因此本文選用WKNN定位算法。定義測試點與參考點之間的歐式距離為式中Sij是第j個參考點處接收的第i個AP中的RSSI值，Si是經(jīng)過修正過后的第i個AP的RSSI值，i＝1,2,L,m，j＝1,2,L,n，m是AP個數(shù)，n是參考點的數(shù)目。WKNN定位算法是選取K(K>1)個最小歐式距離對應的坐標，每個坐標根據(jù)歐式距離大小給予權(quán)值，歐式距離越小權(quán)值越大，計算公式如下式中是t時刻WLAN定位結(jié)果；ε為一個極小量，防止出現(xiàn)分母為0的情況；為測試點到第i參考點歐氏距離的倒數(shù)，測試點到K個參考點歐氏距離倒數(shù)之和。2、MEMS傳感器數(shù)據(jù)處理根據(jù)MEMS傳感器，即加速度計、陀螺儀、磁力計輸出的信息，利用步態(tài)檢測和四元素法更新姿態(tài)矩陣可以得到行人的步數(shù)和航向信息。首先計算加速度計輸出的模值和分別表示加速度計輸出的3軸加速度。然后通過檢測該模值Anorm為峰值且大于某個設定的閾值A0來判斷跨步，完成步態(tài)檢測。最后假設MEMS傳感器的輸出頻率為fs，且通過檢測加速度計輸出模值Anorm連續(xù)兩個大于閾值A0的峰值之間的數(shù)據(jù)點數(shù)ΔN，可以計算第k步所需的時間為假設行人每一步為勻速運動，則可以計算出第k步的速度為式中Pk為第k步的步長，vk為第k步計算出的速度。令第k步中每個采樣時刻的速度都為vk，則每一秒可計算出fs個速度，取每一秒計算出的所有速度的均值作為該秒計算出的速度。利用加速度計、陀螺儀、磁力計輸出數(shù)據(jù)計算更新四元素，結(jié)合四元素法更新姿態(tài)矩陣方法計算每一個采樣時刻的航向角，并取每一秒第一個采樣時刻的航向角作為MEMS每一秒的航向角。3、抗差擴展卡爾曼濾波器(1)抗差卡爾曼濾波器在實際環(huán)境下，系統(tǒng)將受到粗差、模型誤差等的影響，這些將嚴重影響融合濾波的精確度?？共罟烙?，亦稱穩(wěn)健估計，是指在這些影響不可避免的情況下通過選擇適當?shù)墓烙嫹椒?，使未知量估值盡可能減少受到影響的程度，從而得出正常模式下的最佳估計。抗差M估計是使用最為廣泛，計算較簡明的一種抗差估計法。M估計是經(jīng)典的極大似然估計的推廣，稱為廣義極大似然型估計。目前常用的抗差估計方法有Huber法、IGG法、Tukey法等。IGG是中國科學院測量與地球物理研究所的英文縮寫，相比于其他權(quán)函數(shù)，IGG3權(quán)函數(shù)是一個有界、分段、連續(xù)的函數(shù)，如下式所示式中為求得的等價權(quán)矩陣中的第i個對角元素，且為對角矩陣；vi為n×1維殘差向量V的第i個觀測值的測量殘差，n為觀測變量個數(shù)，且V＝Z-HX，Z為n×1維觀測向量，X為m×1維待估計向量，m為需要估計的變量個數(shù)，H為n×m維量測矩陣；pi為權(quán)矩陣P的第i個對角元素，這里假設觀測值獨立，權(quán)矩陣P為對角矩陣，且P＝R-1，R為觀測噪聲協(xié)方差矩陣；ui為標準化的殘差向量，σvi為vi的均方差，實際情況下qvi為vi的權(quán)倒數(shù)，方差因子σ0可以根據(jù)求得，“med”表示取中位數(shù)；k0稱為保權(quán)臨界值，k1稱為零權(quán)臨界值，根據(jù)需要k0,k1可分別取1～1.5，2.5～3.0。假設卡爾曼濾波狀態(tài)方程和觀測方程分別為Xt＝Φt-1Xt-1+Γt,t-1Wt-1(8)Zt＝HtXt+Vt式中Xt為t時刻n×1維狀態(tài)向量，n×n維矩陣Φt-1為系統(tǒng)的一步轉(zhuǎn)移矩陣；Wt-1為t-1時刻n維白噪聲矢量，Γt,t-1為t-1時刻n×n維系統(tǒng)噪聲矩陣；Zt為t時刻m×1維觀測矢量，Ht為t時刻m×m維觀測矩陣，Vt為m維觀測白噪聲矢量。由卡爾曼濾波基本原理和抗差估計原理可以得到擴展抗差卡爾曼濾波基本方程如下所示：狀態(tài)一步預測式中為t-1時刻最優(yōu)狀態(tài)估計，Φt-1系統(tǒng)的一步轉(zhuǎn)移矩陣，為t時刻狀態(tài)的一步預測值。誤差協(xié)方差矩陣一步預測式中為t時刻誤差協(xié)方差矩陣的一步預測，為t-1時刻的最優(yōu)估計誤差協(xié)方差矩陣，Qt-1為t-1時刻的過程噪聲協(xié)方差矩陣。濾波增益計算式中為t時刻求得的卡爾曼濾波增益，Ht為t時刻觀測矩陣，為通過等價權(quán)求得的觀測噪聲誤差協(xié)方差矩陣，且為通過抗差M估計求得的等價權(quán)矩陣。最優(yōu)狀態(tài)估計式中為t時刻求得的最優(yōu)狀態(tài)估計。誤差協(xié)方差矩陣估計式中為t時刻的最優(yōu)估計誤差協(xié)方差矩陣。(2)抗差擴展卡爾曼濾波器設計本發(fā)明采用抗差擴展卡爾曼濾波器作為組合導航濾波器，選用東位置、北位置、速度和航向作為狀態(tài)量，選用WLAN輸出的位置信息、MEMS輸出的速度和航向信息作為觀測量，狀態(tài)方程如下：式中xt表示t時刻待估計的東位置，yt表示t時刻待估計的北位置，vt表示t時刻待估計的速度，表示t時刻待估計的航向，xt-1表示t-1時刻的東位置，yt-1表示t-1時刻的北位置，vt-1表示t-1時刻的速度，表示t-1時刻的航向角，Wt-1表示過程噪聲，是均值為0的高斯白噪聲序列，且i,j＝1,2,...,m，m為需要估計的變量個數(shù)，δij為Kronecker函數(shù)，Q為m維過程噪聲協(xié)方差矩陣。觀測方程如下：式中表示t時刻WLAN定位出的東位置，表示t時刻WLAN定位出的北位置，表示t時刻MEMS輸出的速度，表示t時刻MEMS輸出的航向；xt表示t時刻待估計的東位置，yt表示t時刻待估計的北位置，vt表示t時刻待估計的速度，表示t時刻待估計的航向，Vt表示觀測噪聲，是均值為0的高斯白噪聲序列，且E[V(i)VT(j)]＝R(i,j)δij,i,j＝1,2,...,n，n為觀測變量個數(shù)，δij為Kronecker函數(shù)，R為n維觀測噪聲協(xié)方差矩陣。4、高度計算(1)氣壓計測量海拔高度氣壓測高的基本原理是指在重力場內(nèi)的大氣壓力隨著高度的增加而減小，通過氣壓傳感器可以測量氣壓的大小，通過計算轉(zhuǎn)換可以得到高度信息。在重力勢高度0≤H≤11000米時，國際標準壓高公式如下式中H為計算出來的重力勢高度，Ps為測得的氣壓值。重力勢高度H和設備所在高度h的關系如下式中r為地球公稱半徑，且r＝6356766米。由于r？H，所以重力勢高度H和設備所在高度h近似相等，即h＝H。(2)計算高度和選擇位置指紋數(shù)據(jù)庫正常情況下，將行人在室內(nèi)行走分為：平路行走、上樓梯和下樓梯3種情況，在這3種情況下解算2維位置方法相同(均采用抗差擴展卡爾曼濾波進行解算)，只是在設定步長上稍有區(qū)別，即上樓梯時步長應該設為一個樓梯臺階的寬度。如果直接利用高度計測量的海拔高度信息作為當前時刻的z軸輸出，由于氣壓計的測量誤差，測量的高度會出現(xiàn)不準，可能出現(xiàn)大范圍的跳變，而且在不同時間、不同季節(jié)同一地點的氣壓值也不同。本發(fā)明充分考慮到這些因素的影響，只利用氣壓計測量的高度值判斷上下樓，而實際的高度信息則通過判斷上下樓進行高度的累加或者累減進行計算。將行人行走分為3種情況：樓層中行走、上樓梯和下樓梯。通過地理位置信息，可以判斷出行人所處的大概位置(在樓層中或者在樓梯間)，如果在樓梯間，可以通過高度計測量的海拔高度信息判斷行人上樓梯或者下樓梯，因此實現(xiàn)高度的累加或者累減，如果在樓層中，則將所處樓層的高度直接賦值給當前時刻的z軸輸出，實現(xiàn)室內(nèi)空間跨樓層3維定位。具體的判斷規(guī)則如圖2所示：首先利用地理位置信息，判斷行人是否進出樓梯間。測試測試環(huán)境中所有樓梯口出的3維位置坐標為(xi,yi,zi),i＝1,2K,n，其中n為樓梯口總的個數(shù)。假設行人在t-1時刻的最優(yōu)位置為分別計算其與每一個樓梯口的物理距離為式中di為時刻t-1的最優(yōu)位置與第i個樓梯口的物理距離。設定門限r(nóng)，如果di＜r且判斷為進入樓梯間(如果上個時刻在樓層中，則判斷為進入樓梯間)，此時匹配樓梯間的指紋數(shù)據(jù)庫，并保存此時的氣壓計測量高度信息h2；如果di＜r且判斷為出樓梯間(如果上個時刻在樓梯間，則判斷為出樓梯間)，此時還需利用前一時刻的實際高度信息判斷所處樓層，選擇相應的位置指紋數(shù)據(jù)庫，如果di＞r，則位置指紋數(shù)據(jù)庫不變，與前一時刻的位置指紋數(shù)據(jù)庫相同。如果判斷為在樓梯間，此時計算氣壓計測量的高度h1和剛進入樓梯間的氣壓計測量高度h2的差值Δh。設定門限λ，如果|Δh|＜λ，則此時輸出的高度不變，令其等于前一時刻的實際高度，如果|Δh|＞λ且Δh＞0，則此時輸出的高度在前一時刻的高度上面累加固定值hstair，如果|Δh|＞λ且Δh＜0，則此時輸出的高度在前一時刻的高度上面累減固定值hstair。如果判斷為在樓層中，則將所處樓層的高度直接賦值給當前時刻的z軸輸出。實施例：如圖1所示，室內(nèi)WLAN/MEMS融合跨樓層3維定位方法包括以下步驟：1)初始化抗差擴展卡爾曼組合導航濾波器，具體為給定初始位置信息(x0,y0)，誤差協(xié)方差矩陣P0，過程噪聲協(xié)方差矩陣Q，和觀測噪聲協(xié)方差矩陣R，給定層高度初值z0，并選擇初始位置指紋庫；2)假設得到t-1時刻的最優(yōu)估計位置為t＝1,2,K，計算與樓梯口的物理距離di，選擇WLAN位置指紋數(shù)據(jù)庫，具體如下：測量測試環(huán)境中所有樓梯口出的3維位置坐標為(xi,yi,zi),i＝1,2K,n，其中n為樓梯口總的個數(shù)。分別計算其與每一個樓梯口的物理距離為式中di為時刻t-1的最優(yōu)位置與第i個樓梯口的物理距離。如圖2所示，設定門限r(nóng)，如果di＜r且判斷為進入樓梯間，此時匹配樓梯間的指紋數(shù)據(jù)庫；如果di＜r且判斷為出樓梯間，此時還需利用前一時刻的實際高度信息判斷所處樓層，選擇相應的位置指紋數(shù)據(jù)庫。如果di＞r，則位置指紋數(shù)據(jù)庫不變，與前一時刻的位置指紋數(shù)據(jù)庫相同。3)實際測量t時刻RSSI向量，并利用擴展卡爾曼濾波器中的一步預測信息反匹配位置指紋數(shù)據(jù)庫修正RSSI向量，并利用修正的RSSI采用WKNN算法匹配位置指紋數(shù)據(jù)庫，得到t時刻的WLAN定位結(jié)果具體如下：定義t時刻擴展卡爾曼預測位置到WLAN位置指紋數(shù)據(jù)庫中參考點的物理距離為式中(xj,yj)為位置指紋數(shù)據(jù)庫中第j個參考點的位置，n為位置指紋庫中參考點總數(shù)。在計算出t時刻到所有參考點的距離Dtj后，選取K個物理距離最小的參考點對應的RSSI向量，利用類似WKNN算法得到預測的RSSI向量為式中RSSI1為得到的RSSI預測向量，RSSIm為t時刻K個與擴展卡爾曼預測位置物理距離最小的參考點中第m點對應的RSSI向量，Dtm為其對應的物理距離，為t時刻K個與擴展卡爾曼預測位置物理距離最小的參考點對應的物理距離的和，式中q為索引號。得到RSSI1過后，對實測的RSSI2進行修正有RSSIt＝w×RSSI1+(1-w)×RSSI2(22)式中RSSIi為t時刻最終的RSSI向量，w為加權(quán)系數(shù)，取值范圍為0≤w≤1。計算與參考點之間的歐式距離式中Sij是第j個參考點處接收的第i個AP中的RSSI值，Si是經(jīng)過修正過后的第i個AP的RSSI值，i＝1,2,L,m，j＝1,2,L,n，m是AP個數(shù)，n是參考點的數(shù)目。利用WKNN匹配算法得到t時刻的WLAN定位結(jié)果計算公式如下式中ε為一個極小量，防止出現(xiàn)分母為0的情況，為測試點到第i參考點歐氏距離的倒數(shù)，測試點到K(K>1)個參考點歐氏距離倒數(shù)之和。4)處理MEMS傳感器輸出信息，得到t時刻速度和航向具體如下：首先計算加速度計輸出的模值和分別表示加速度計輸出的3軸加速度。然后通過檢測該模值Anorm為峰值且大于某個設定的閾值A0來判斷跨步，完成步態(tài)檢測。最后假設MEMS傳感器的輸出頻率為fs，且通過檢測加速度計輸出模值Anorm連續(xù)兩個大于閾值A0的峰值之間的數(shù)據(jù)點數(shù)ΔN，可以計算第k步所需的時間為假設行人每一步為勻速運動，則可以計算出第k步的速度為式中Pk為第k步的步長，vk為第k步計算出的速度。令第k步中每個采樣時刻的速度都為vk，取t時刻計算出的所有采樣點速度的均值作為MEMS在t時刻輸出的速度利用t時刻MEMS測量得到fs組加速度計、陀螺儀和磁力計數(shù)據(jù)更新四元素，從而計算得到fs個航向角，取t時刻計算得到的第一個航向角作為t時刻的航向角5)將t時刻測量得到的WLAN位置信息與MEMS輸出的速度和航向帶入抗差擴展卡爾曼濾波器，得到t時刻2維位置最優(yōu)估計值6)通過判斷出行人所處的大概位置(在樓層中或者在樓梯間)，如果在樓梯間，可以通過高度計測量的海拔高度信息判斷行人上樓梯或者下樓梯，因此實現(xiàn)高度的累加或者累減；如果在樓層中，則將所處樓層的高度直接賦值給當前時刻的z軸輸出，具體方法如下：如圖2所示，如果判斷為在樓梯間，此時計算氣壓計測量的高度h1和剛進入樓梯間的氣壓計測量高度h2的差值Δh。設定門限λ，如果|Δh|＜λ，則此時輸出的高度不變，令其等于前一時刻的實際高度，如果|Δh|＞λ且Δh＞0，則此時輸出的高度在前一時刻的高度上面累加固定值hstair，如果|Δh|＞λ且Δh＜0，則此時輸出的高度在前一時刻的高度上面累減固定值hstair。如果判斷為在樓層中，則將所處樓層的高度直接賦值給當前時刻的z軸輸出。最后應該說明的是：以上所述僅為本發(fā)明的優(yōu)選實施例，并不限制本發(fā)明，盡管利用實施例對本發(fā)明進行了詳細的說明，但對于本領域的技術人員來說，其依然可以對前述實施例所記載的技術方案進行修改，或者對其中的部分技術進行等同替換。凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進等，均應包含在本發(fā)明的保護范圍之內(nèi)。