一种室内WLAN/MEMS融合跨楼层3维定位方法与流程

本发明涉及定位技术领域，特别涉及一种室内定位系统。

背景技术：
随着智能终端的广泛使用，移动通信快速发展，同时数据和多媒体业务迅速增加，基于位置的服务(LocationBasedServices，LBS)逐渐受到重视。尤其是作为定位技术的最后一米，室内定位技术有着广泛的应用前景。比如在地下停车场，可以借助室内定位技术快速找到停车位；在大型商场，可以借助室内定位技术快速找到出口、电梯，同时商家也可以借助于室内定位向用户介绍商品信息；在火灾现场，消防人员可以借助室内定位技术快速确定被困人员的位置，从而迅速进行救援；在医院，可以利用室内定位技术对需要看护的病人进行定位等等。在室外，全球卫星导航系统(GlobalNavigationSatelliteSystem，GNSS)基本上可以满足定位精度的要求，但是在室内环境下，由于卫星信号受到遮挡且存在严重的多径效应，使得卫星定位精度严重下降，甚至出现无法正常定位的情况。目前，针对复杂的室内环境，现有的室内定位技术主要分为以下几种1)射频识别定位技术射频识别(RadioFrequencyIdentification，RFID)定位技术是一种利用射频信号自动识别目标对象并获取相关信息的技术。实现无接触信息传递并达到识别对象的目的。RFID定位系统包括电子标签和读卡器等，该系统利用标签对物体的唯一标识特性，通过读卡器接收到的电子标签发送的信号来获得电子标签的位置信息。RFID定位系统优点是体积小、造价低，但是作用范围小、不具有通信能力，且不便整合到其它系统中。2)蓝牙定位技术蓝牙(Bluetooth)技术是一种支持语音和数据业务的短距离、低功耗无线传输技术，并可以实现短距离不同设备之间的互联。基于蓝牙的室内定位技术，利用经验测试与信号传播模型结合的方式，通过实时测量的信号强度进行定位。蓝牙定位技术的优点在于设备体积小，易于集成在移动智能终端上面。理论上，只要集成了蓝牙的移动智能终端且蓝牙功能打开，蓝牙定位系统就能够对其进行定位。蓝牙定位技术的缺点在于蓝牙器件设备的价格比较昂贵，且对于复杂的室内环境而言，蓝牙定位系统的稳定性比较差。3)红外线定位技术红外线(InfraredRay，IR)是一种介于微波和可见光之间的电磁波。IR室内定位方法通过发射调制的红外射线，利用安装在室内的光学传感器接收进行定位。IR定位具有相对较高的定位精度，但由于光线不能穿过障碍物，使得IR具有传输距离短的缺点，且需要在室内的每个房间和楼层安装设备，成本较高。4)无线局域网定位技术无线局域网(WirelessLocalAreaNetwork，WLAN)是基于IEEE802.11标准，以无线信道作为传输媒介的局域网。基于WLAN的定位技术主要包括信号传播模型法和位置指纹法，都是利用接收信号的信号强度指示(ReceivedSignalStrengthIndication，RSSI)进行位置解算。信号传播模型法利用室内信号的传播特性，考虑墙壁和地板引起的衰减，建立RSSI与传播距离d之间的数学模型，实现位置解算。但是由于复杂的室内环境，很难建立准确的室内信号传播模型，因此利用该方法进行位置解算精度有限。基于WLAN的位置指纹定位技术包括离线阶段和在线阶段两个阶段，离线阶段在需要定位的环境采集相应的RSSI信息并存入位置指纹数据库，在线阶段利用实时测得的RSSI信息与位置指纹数据库进行匹配，将匹配最佳位置输出作为定位结果。基于WLAN的位置指纹定位技术具有覆盖范围广、可用性强、低成本、长时间定位精度高等优点。但是由于多径干扰等引起的RSSI扰动等因素严重影响了WLAN的定位精度。5)MEMS传感器自主定位技术随着微机电系统(Micro-Electro-MechanicalSystems，MEMS)技术的快速发展，各种传感器，如陀螺仪、加速度计、磁力计、气压计等，尺寸不断变小，成本不断变低，被广泛应用于各种移动智能终端设备中。基于MEMS传感器的定位技术具有自主性和连续性，且目前基于MEMS传感器的个人导航系统主要有两种：传统的惯性积分导航和行人航迹推算导航。传统的惯性积分导航，根据牛顿运动定律，利用测得3轴加速度计和3轴陀螺仪的数据积分计算出3维姿态、速度和位置。理论上这种方式定位精度会很高，但是实际应用中，由于惯性传感器自身的漂移、测量噪声、行人的抖动等因素影响，多次积分后会产生累积误差，几十秒之内误差可达几十米，甚至几百米。基于行人航迹推算(PedestrianDeadReckoning，PDR)方法利用行人的位移和航向进行位置解算，相比于传统的惯性积分导航更准确且更适用于行人导航，但是其定位依然受步长、航向、步态检测精度影响，随着时间的增加，依然存在累积误差。

技术实现要素：
本发明针对现有室内定位技术存在的缺陷和不足，结合WLAN位置指纹定位和MEMS传感器定位，提出一种高精度室内WLAN/MEMS融合跨楼层3维定位方法，实现室内空间的3维定位。该方法及系统能够有效的克服MEMS器件定位存在的累积误差和WLAN定位中存在的RSSI扰动对系统定位精度带来的影响。本发明方法及系统利用扩展卡尔曼滤波算法中的一步预测位置信息修正观测的RSSI，修正由于RSSI扰动给WLAN定位精度带来的影响；同时利用WLAN定位结果补偿MEMS的累积误差，提高定位精度。且该方法还综合利用气压计、地理位置等解算高度，实现室内跨楼层3维定位。本发明的技术方案如下：本发明提供一种室内WLAN/MEMS融合跨楼层3维定位方法，其特征在于，所述方法包括：(1)选择WLAN位置指纹数据库：计算前一时刻得到的行人的3维位置距每个楼梯口的物理距离，利用计算出的物理距离是否大于某个阈值判断行人是否进出楼梯间；如果判断出没有进出楼梯间，则位置指纹数据库不变，与前一时刻的位置指纹数据库相同；如果进入楼梯间，则选择楼梯间的位置指纹数据库，如果出楼梯间，则判断为在楼层中，此时还需利用前一时刻的实际高度判断所处楼层，选择相应楼层的位置指纹数据库。(2)计算行人的速度和航向：根据采集到的MEMS数据，包括3轴加速度计、3轴陀螺仪和3轴磁力计数据，利用步态检测和数据更新速率方法计算速度，四元素更新姿态矩阵方法计算航向。(3)WLAN位置指纹定位：首先利用抗差扩展卡尔曼组合导航滤波器中的一步预测信息反匹配位置指纹数据库修正观测得到RSSI，然后利用修正的RSSI结合WKNN定位算法进行位置解算，克服由于RSSI扰动对WLAN位置指纹定位精度带来的影响。同时将抗差M估计引入室内融合定位算法中，形成抗差扩展卡尔曼滤波器，解决定位在实际环境下受到粗差、干扰、模型误差等因素的影响，导致位置精度的严重下降。(4)将计算得到的速度、航向和WLAN指纹定位的位置一起带入抗差扩展卡尔曼组合导航滤波器，解算得到最优的二维位置坐标。(5)高度计算：利用气压计输出数据计算海拔高度，实际的高度信息则通过判断上下楼进行高度的累加或者累减进行计算，从而实现室内跨楼层3为定位。所述步骤(3)使用的抗差扩展卡尔曼组合导航滤波器的设计方法是：通过建立观测方程和状态方程，设计扩展卡尔曼滤波器(EKF)输出最优位置信息，并同时引入抗差M估计到室内融合定位算法中，形成抗差扩展卡尔曼滤波器。进一步，所述状态变量和状态方程如下：选用东位置、北位置、速度和航向作为状态变量，即状态方程如下：式中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位置指纹数据库中参考点的物理距离为式中(xj,yj)为位置指纹数据库中第j个参考点的位置，n为位置指纹库中参考点总数，在计算出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为加权系数，取值范围为0≤w≤1。更进一步，所述步骤(2)中MEMS输出的速度具体解算如下：首先计算加速度计输出的模值和分别表示加速度计输出的3轴加速度。然后通过检测该模值Anorm为峰值且大于某个设定的阈值A0来判断跨步，完成步态检测。最后假设MEMS传感器的输出频率为fs，且通过检测加速度计输出模值Anorm连续两个大于阈值A0的峰值之间的数据点数ΔN，可以计算第k步所需的时间为假设行人每一步为匀速运动，则可以计算出第k步的速度为式中Pk为第k步的步长，vk为第k步计算出的速度。令第k步中每个采样时刻的速度都为vk，则每一秒可计算出fs个速度，取每一秒计算出的所有速度的均值作为该秒计算出的速度。进一步，所述步骤(5)的高度计算中，仅利用气压计测量的高度值判断上下楼，而实际的高度信息则通过判断上下楼进行高度的累加或者累减进行计算，具体方法如下：将行人行走分为3种情况：楼层中行走、上楼梯和下楼梯；通过地理位置信息，判断出行人所处的大概位置(在楼层中或者在楼梯间)，如果在楼梯间，通过高度计测量的海拔高度信息判断行人上楼梯或者下楼梯，实现高度的累加或者累减，如果在楼层中，则将所处楼层的高度直接赋值给当前时刻的高度输出，实现室内空间跨楼层3维定位。更进一步，所述的判断出行人所处的大概位置(在楼层中或者在楼梯间)，其具体判断如下：测试测试环境中所有楼梯口处的3维位置坐标为(xi,yi,zi),i＝1,2K,n，其中n为楼梯口总的个数。假设行人在时刻t-1的最优位置为分别计算其与每一个楼梯口的物理距离为式中di为时刻t-1的最优位置与第i个楼梯口的物理距离；设定门限r，如果di＜r且判断为进入楼梯间(如果上个时刻在楼层中，则判断为进入楼梯间)，此时匹配楼梯间的指纹数据库，并保存此时的气压计测量高度信息h2；如果di＜r且判断为出楼梯间(如果上个时刻在楼梯间，则判断为出楼梯间)，此时还需利用前一时刻的实际高度信息判断所处楼层，选择相应的位置指纹数据库，如果di＞r，则位置指纹数据库不变，与前一时刻的位置指纹数据库相同。本发明的优点如下：1)高精度，本发明可以实现室内跨楼层3维定位最小均方根误差小于1米的高精度定位，并且本发明室内跨楼层3维定位误差小于1m的置信概率为75％；2)实现WLAN/MEMS融合定位，克服了WLAN位置指纹定位技术中存在的RSSI扰动和MEMS传感器定位技术中存在的累积误差对系统定位精度带来的影响；3)实现室内空间的跨楼层3维定位。附图说明图1为本发明所述系统的总体架构流程图；图2为本发明方法中高度计算的流程图。具体实施方式下面结合附图和具体实施例对本发明进行详细说明。如图1所示，本发明提出的室内WLAN/MEMS融合跨楼层3维定位系统主要包括：WLAN位置指纹定位模块、MEMS数据处理模块、抗差扩展卡尔曼滤波器模块、高度计算模块。●WLAN位置指纹定位模块。根据抗差扩展卡尔曼滤波器反馈的一步预测位置信息，利用位置指纹数据库反匹配修正实测RSSI信息，同时利用修正的RSSI信息，采用加权K近邻(WeightedK-NearestNeighborhood，WKNN)定位算法解算位置。●MEMS传感器数据处理模块。根据加速度计、陀螺仪、磁力计输出的信息，利用步态检测、数据更新速率、四元素法更新姿态矩阵等方法计算行人的速度和航向。●抗差扩展卡尔曼滤波器模块。建立观测方程和状态方程，设计扩展卡尔曼滤波器(ExtendedKalmanFilter，EKF)输出最优位置信息，并同时引入抗差M估计，以解决系统在实际环境下受到粗差、干扰、模型误差等因素的影响，导致位置精度严重下降。●高度计算模块。综合利用地理位置、气压计输出海拔高度和每一个楼层的实际高度实现高度的解算，并同时为WLAN定位选择位置指纹数据库。1、WLAN位置指纹定位对于室内WLAN环境下的实时跟踪定位来说，由于可供采集RSSI的时间较短、并且RSSI波动性很大，若只使用测量的RSSI信息来确定观测位置信息，将会存在很大的定位误差。解决方法为根据扩展卡尔曼滤波器反馈的一步预测位置信息，利用位置指纹数据库反匹配修正实测RSSI信息，同时利用修正的RSSI信息，采用加权K近邻(WeightedK-NearestNeighborhood，WKNN)定位算法解算位置。(1)反匹配修正实测RSSI定义t(t＝1,2,K)时刻扩展卡尔曼预测位置到WLAN位置指纹数据库中参考点的物理距离为式中(xj,yj)为位置指纹数据库中第j个参考点的位置，n为位置指纹库中参考点总数。在计算出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为加权系数，取值范围为0≤w≤1。(2)WKNN定位算法加权K近邻(WeightedK-NearestNeighborhood，WKNN)定位算法在定位精度和计算复杂度上有一个良好的折中，因此本文选用WKNN定位算法。定义测试点与参考点之间的欧式距离为式中Sij是第j个参考点处接收的第i个AP中的RSSI值，Si是经过修正过后的第i个AP的RSSI值，i＝1,2,L,m，j＝1,2,L,n，m是AP个数，n是参考点的数目。WKNN定位算法是选取K(K>1)个最小欧式距离对应的坐标，每个坐标根据欧式距离大小给予权值，欧式距离越小权值越大，计算公式如下式中是t时刻WLAN定位结果；ε为一个极小量，防止出现分母为0的情况；为测试点到第i参考点欧氏距离的倒数，测试点到K个参考点欧氏距离倒数之和。2、MEMS传感器数据处理根据MEMS传感器，即加速度计、陀螺仪、磁力计输出的信息，利用步态检测和四元素法更新姿态矩阵可以得到行人的步数和航向信息。首先计算加速度计输出的模值和分别表示加速度计输出的3轴加速度。然后通过检测该模值Anorm为峰值且大于某个设定的阈值A0来判断跨步，完成步态检测。最后假设MEMS传感器的输出频率为fs，且通过检测加速度计输出模值Anorm连续两个大于阈值A0的峰值之间的数据点数ΔN，可以计算第k步所需的时间为假设行人每一步为匀速运动，则可以计算出第k步的速度为式中Pk为第k步的步长，vk为第k步计算出的速度。令第k步中每个采样时刻的速度都为vk，则每一秒可计算出fs个速度，取每一秒计算出的所有速度的均值作为该秒计算出的速度。利用加速度计、陀螺仪、磁力计输出数据计算更新四元素，结合四元素法更新姿态矩阵方法计算每一个采样时刻的航向角，并取每一秒第一个采样时刻的航向角作为MEMS每一秒的航向角。3、抗差扩展卡尔曼滤波器(1)抗差卡尔曼滤波器在实际环境下，系统将受到粗差、模型误差等的影响，这些将严重影响融合滤波的精确度。抗差估计，亦称稳健估计，是指在这些影响不可避免的情况下通过选择适当的估计方法，使未知量估值尽可能减少受到影响的程度，从而得出正常模式下的最佳估计。抗差M估计是使用最为广泛，计算较简明的一种抗差估计法。M估计是经典的极大似然估计的推广，称为广义极大似然型估计。目前常用的抗差估计方法有Huber法、IGG法、Tukey法等。IGG是中国科学院测量与地球物理研究所的英文缩写，相比于其他权函数，IGG3权函数是一个有界、分段、连续的函数，如下式所示式中为求得的等价权矩阵中的第i个对角元素，且为对角矩阵；vi为n×1维残差向量V的第i个观测值的测量残差，n为观测变量个数，且V＝Z-HX，Z为n×1维观测向量，X为m×1维待估计向量，m为需要估计的变量个数，H为n×m维量测矩阵；pi为权矩阵P的第i个对角元素，这里假设观测值独立，权矩阵P为对角矩阵，且P＝R-1，R为观测噪声协方差矩阵；ui为标准化的残差向量，σvi为vi的均方差，实际情况下qvi为vi的权倒数，方差因子σ0可以根据求得，“med”表示取中位数；k0称为保权临界值，k1称为零权临界值，根据需要k0,k1可分别取1～1.5，2.5～3.0。假设卡尔曼滤波状态方程和观测方程分别为Xt＝Φt-1Xt-1+Γt,t-1Wt-1(8)Zt＝HtXt+Vt式中Xt为t时刻n×1维状态向量，n×n维矩阵Φt-1为系统的一步转移矩阵；Wt-1为t-1时刻n维白噪声矢量，Γt,t-1为t-1时刻n×n维系统噪声矩阵；Zt为t时刻m×1维观测矢量，Ht为t时刻m×m维观测矩阵，Vt为m维观测白噪声矢量。由卡尔曼滤波基本原理和抗差估计原理可以得到扩展抗差卡尔曼滤波基本方程如下所示：状态一步预测式中为t-1时刻最优状态估计，Φt-1系统的一步转移矩阵，为t时刻状态的一步预测值。误差协方差矩阵一步预测式中为t时刻误差协方差矩阵的一步预测，为t-1时刻的最优估计误差协方差矩阵，Qt-1为t-1时刻的过程噪声协方差矩阵。滤波增益计算式中为t时刻求得的卡尔曼滤波增益，Ht为t时刻观测矩阵，为通过等价权求得的观测噪声误差协方差矩阵，且为通过抗差M估计求得的等价权矩阵。最优状态估计式中为t时刻求得的最优状态估计。误差协方差矩阵估计式中为t时刻的最优估计误差协方差矩阵。(2)抗差扩展卡尔曼滤波器设计本发明采用抗差扩展卡尔曼滤波器作为组合导航滤波器，选用东位置、北位置、速度和航向作为状态量，选用WLAN输出的位置信息、MEMS输出的速度和航向信息作为观测量，状态方程如下：式中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为需要估计的变量个数，δij为Kronecker函数，Q为m维过程噪声协方差矩阵。观测方程如下：式中表示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为观测变量个数，δij为Kronecker函数，R为n维观测噪声协方差矩阵。4、高度计算(1)气压计测量海拔高度气压测高的基本原理是指在重力场内的大气压力随着高度的增加而减小，通过气压传感器可以测量气压的大小，通过计算转换可以得到高度信息。在重力势高度0≤H≤11000米时，国际标准压高公式如下式中H为计算出来的重力势高度，Ps为测得的气压值。重力势高度H和设备所在高度h的关系如下式中r为地球公称半径，且r＝6356766米。由于r？H，所以重力势高度H和设备所在高度h近似相等，即h＝H。(2)计算高度和选择位置指纹数据库正常情况下，将行人在室内行走分为：平路行走、上楼梯和下楼梯3种情况，在这3种情况下解算2维位置方法相同(均采用抗差扩展卡尔曼滤波进行解算)，只是在设定步长上稍有区别，即上楼梯时步长应该设为一个楼梯台阶的宽度。如果直接利用高度计测量的海拔高度信息作为当前时刻的z轴输出，由于气压计的测量误差，测量的高度会出现不准，可能出现大范围的跳变，而且在不同时间、不同季节同一地点的气压值也不同。本发明充分考虑到这些因素的影响，只利用气压计测量的高度值判断上下楼，而实际的高度信息则通过判断上下楼进行高度的累加或者累减进行计算。将行人行走分为3种情况：楼层中行走、上楼梯和下楼梯。通过地理位置信息，可以判断出行人所处的大概位置(在楼层中或者在楼梯间)，如果在楼梯间，可以通过高度计测量的海拔高度信息判断行人上楼梯或者下楼梯，因此实现高度的累加或者累减，如果在楼层中，则将所处楼层的高度直接赋值给当前时刻的z轴输出，实现室内空间跨楼层3维定位。具体的判断规则如图2所示：首先利用地理位置信息，判断行人是否进出楼梯间。测试测试环境中所有楼梯口出的3维位置坐标为(xi,yi,zi),i＝1,2K,n，其中n为楼梯口总的个数。假设行人在t-1时刻的最优位置为分别计算其与每一个楼梯口的物理距离为式中di为时刻t-1的最优位置与第i个楼梯口的物理距离。设定门限r，如果di＜r且判断为进入楼梯间(如果上个时刻在楼层中，则判断为进入楼梯间)，此时匹配楼梯间的指纹数据库，并保存此时的气压计测量高度信息h2；如果di＜r且判断为出楼梯间(如果上个时刻在楼梯间，则判断为出楼梯间)，此时还需利用前一时刻的实际高度信息判断所处楼层，选择相应的位置指纹数据库，如果di＞r，则位置指纹数据库不变，与前一时刻的位置指纹数据库相同。如果判断为在楼梯间，此时计算气压计测量的高度h1和刚进入楼梯间的气压计测量高度h2的差值Δh。设定门限λ，如果|Δh|＜λ，则此时输出的高度不变，令其等于前一时刻的实际高度，如果|Δh|＞λ且Δh＞0，则此时输出的高度在前一时刻的高度上面累加固定值hstair，如果|Δh|＞λ且Δh＜0，则此时输出的高度在前一时刻的高度上面累减固定值hstair。如果判断为在楼层中，则将所处楼层的高度直接赋值给当前时刻的z轴输出。实施例：如图1所示，室内WLAN/MEMS融合跨楼层3维定位方法包括以下步骤：1)初始化抗差扩展卡尔曼组合导航滤波器，具体为给定初始位置信息(x0,y0)，误差协方差矩阵P0，过程噪声协方差矩阵Q，和观测噪声协方差矩阵R，给定层高度初值z0，并选择初始位置指纹库；2)假设得到t-1时刻的最优估计位置为t＝1,2,K，计算与楼梯口的物理距离di，选择WLAN位置指纹数据库，具体如下：测量测试环境中所有楼梯口出的3维位置坐标为(xi,yi,zi),i＝1,2K,n，其中n为楼梯口总的个数。分别计算其与每一个楼梯口的物理距离为式中di为时刻t-1的最优位置与第i个楼梯口的物理距离。如图2所示，设定门限r，如果di＜r且判断为进入楼梯间，此时匹配楼梯间的指纹数据库；如果di＜r且判断为出楼梯间，此时还需利用前一时刻的实际高度信息判断所处楼层，选择相应的位置指纹数据库。如果di＞r，则位置指纹数据库不变，与前一时刻的位置指纹数据库相同。3)实际测量t时刻RSSI向量，并利用扩展卡尔曼滤波器中的一步预测信息反匹配位置指纹数据库修正RSSI向量，并利用修正的RSSI采用WKNN算法匹配位置指纹数据库，得到t时刻的WLAN定位结果具体如下：定义t时刻扩展卡尔曼预测位置到WLAN位置指纹数据库中参考点的物理距离为式中(xj,yj)为位置指纹数据库中第j个参考点的位置，n为位置指纹库中参考点总数。在计算出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为加权系数，取值范围为0≤w≤1。计算与参考点之间的欧式距离式中Sij是第j个参考点处接收的第i个AP中的RSSI值，Si是经过修正过后的第i个AP的RSSI值，i＝1,2,L,m，j＝1,2,L,n，m是AP个数，n是参考点的数目。利用WKNN匹配算法得到t时刻的WLAN定位结果计算公式如下式中ε为一个极小量，防止出现分母为0的情况，为测试点到第i参考点欧氏距离的倒数，测试点到K(K>1)个参考点欧氏距离倒数之和。4)处理MEMS传感器输出信息，得到t时刻速度和航向具体如下：首先计算加速度计输出的模值和分别表示加速度计输出的3轴加速度。然后通过检测该模值Anorm为峰值且大于某个设定的阈值A0来判断跨步，完成步态检测。最后假设MEMS传感器的输出频率为fs，且通过检测加速度计输出模值Anorm连续两个大于阈值A0的峰值之间的数据点数ΔN，可以计算第k步所需的时间为假设行人每一步为匀速运动，则可以计算出第k步的速度为式中Pk为第k步的步长，vk为第k步计算出的速度。令第k步中每个采样时刻的速度都为vk，取t时刻计算出的所有采样点速度的均值作为MEMS在t时刻输出的速度利用t时刻MEMS测量得到fs组加速度计、陀螺仪和磁力计数据更新四元素，从而计算得到fs个航向角，取t时刻计算得到的第一个航向角作为t时刻的航向角5)将t时刻测量得到的WLAN位置信息与MEMS输出的速度和航向带入抗差扩展卡尔曼滤波器，得到t时刻2维位置最优估计值6)通过判断出行人所处的大概位置(在楼层中或者在楼梯间)，如果在楼梯间，可以通过高度计测量的海拔高度信息判断行人上楼梯或者下楼梯，因此实现高度的累加或者累减；如果在楼层中，则将所处楼层的高度直接赋值给当前时刻的z轴输出，具体方法如下：如图2所示，如果判断为在楼梯间，此时计算气压计测量的高度h1和刚进入楼梯间的气压计测量高度h2的差值Δh。设定门限λ，如果|Δh|＜λ，则此时输出的高度不变，令其等于前一时刻的实际高度，如果|Δh|＞λ且Δh＞0，则此时输出的高度在前一时刻的高度上面累加固定值hstair，如果|Δh|＞λ且Δh＜0，则此时输出的高度在前一时刻的高度上面累减固定值hstair。如果判断为在楼层中，则将所处楼层的高度直接赋值给当前时刻的z轴输出。最后应该说明的是：以上所述仅为本发明的优选实施例，并不限制本发明，尽管利用实施例对本发明进行了详细的说明，但对于本领域的技术人员来说，其依然可以对前述实施例所记载的技术方案进行修改，或者对其中的部分技术进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。