技术领域
[0001] 本
发明涉及室内定位追踪领域，尤其涉及一种基于粒子滤波算法的室内定位方法。
背景技术
[0002] 随着现代移动通信技术的快速发展，基于
位置的服务LBS(Location Based Services)得到了人们越来越多的青睐，实现低成本且高
精度的室内定位算法受到了很多研究者的关注。现在，基于WiFi、红外线、蓝牙、超宽带广播(UWB)、地磁、惯性
传感器的室内定位方法得到了研究界广泛的关注，但是超宽带广播(UWB)、红外技术进行定位都需要重新部署设备，成本昂贵。
[0003] 而近年来，随着无线局域网(WiFi)的快速发展，在大多数室内环境中都广泛部署了WiFi网络，即广泛布置了AP，不同位置处的智能设备通过接收来自不同AP的
信号进行局域网的连接，即在不同位置处智能设备会接收到来自不同AP的信号，或者接收到相同AP的信号但其信号强度值会存在差异。因此，可以将AP的接收信号强度看作一种位置指纹进行室内定位。
[0004] 另一方面，受到
生物学中信鸽、龙虾等生物可以利用地球
磁场进行寻路的启发，研究人员发现可以利用地球磁场进行室内定位。在室内环境下，地球磁场受到
建筑物的
钢筋
混凝土结构、内部管道
电缆及大型电磁设备等的影响而发生扭曲，从而造成了室内
地磁场的不均匀分布，所以可以将地磁场看作一种位置指纹进行室内定位。
[0005] 利用WiFi指纹或地磁指纹的室内定位技术适用于静态定位，在实时定位及
跟踪中不能很好的发挥作用。而惯性导航技术是一种完全自主的导航定位方法，利用智能手机集成的
加速度计、
陀螺仪、
电子罗盘、磁
力计等传感器的测量数据，进行检步、步长估计、航向
角估计，因此可以使用行人航位推测(Pedestrian Dead Reckoning，PDR)算法进行行人轨迹的定位。但是PDR算法需要知道精确的初始位置，而在实际中，初始位置较难获得；由于惯性传感器的误差，在较短时间内定位精度较高，随着时间增长，会出现较大的累积误差。
[0006] 在现有方案中，利用基于WiFi与PDR的粒子滤波算法(WiFi-PF)或者基于地磁与PDR的粒子滤波算法(Mag-PF)可以实现动态定位，得到连续的用户行走轨迹。但是WiFi-PF或Mag-PF算法在长时间的定位跟踪后，容易出现定位偏离行人真实轨迹，定位跟踪失败。尤其是Mag-PF算法，由于地磁指纹是三维向量，空间唯一性差，一旦出现定位偏离真实位置，粒子的地磁指纹序列仍可能与当前观测到的地磁序列匹配，从而估计定位坐标，而算法本身无法识别定位已失败。而WiFi的指纹匹配算法的优点是前后两次的定位结果相互独立，且定位误差有一定的范围，因此利用WiFi-PF、Mag-PF与WiFi的指纹匹配算法的独立性，在每步的定位中，相互校验，进行融合从而使定位保持正确跟踪。
发明内容
[0007] 本发明主要针对现有的单独使用基于WiFi与PDR的粒子滤波算法和基于地磁与PDR的粒子滤波算法容易出现定位跟踪失败的问题，目的在于提供一种基于粒子滤波算法的室内定位方法，该方法利用基于WiFi与PDR的粒子滤波算法、基于地磁与PDR的粒子滤波算法及基于WiFi的指纹匹配算法在定位中的独立性，相互校验，使定位保持跟踪，提高定位的鲁棒性。
[0008] 为实现上述目的，本发明采用的技术方案如下：
[0009] 一种基于粒子滤波算法的室内定位方法，包括以下步骤：采用初始位置定位算法确定初始点坐标，然后通过融合PDR、WiFi与地磁的粒子滤波算法实现定位，具体过程如下：
[0010] 步骤1：以L0(x0,y0)为初始点分别进行WiFi-PF算法和Mag-PF算法的粒子初始化；
[0011] 步骤2：利用PDR算法进行实时估计用户行走步数以及方向，对于PDR算法检测到的第k步，得到对应的WiFi-PF算法的定位坐标 Mag-PF算法的定位坐标及WiFi-KNN算法的定位坐标
[0012] 步骤3：第k步时，通过融合Mag-PF算法的定位坐标与地图信息进行定位跟踪是否失败判断；
[0013] 步骤4：若判断Mag-PF算法的定位跟踪失败，则以WiFi-PF算法的定位坐标为参考重新初始化粒子，即以当前第k步的WiFi-PF的定位坐标 为圆心，R为半径画圆，在圆内随机生成Num个粒子，代替原粒子集，作为下一步Mag-PF算法进行粒子滤波的粒子集，再进行步骤6；若判断Mag-PF算法的定位跟踪正常，则进行步骤5；
[0014] 步骤5：第k步时，将定位坐标 与定位坐标进行融合，得到WiFi-PF算法与Mag-PF算法的定位坐标融合后的定位坐标为[0015] 步骤6：将WiFi-PF算法的定位坐标作为融合后的定位坐标，即
[0016] 步骤7：计算WiFi-KNN算法的定位坐标 与融合后的定位坐标之间的距离 若WiFi-KNN算法的定位坐
标 与融合后的定位坐标 之间的距离disk大于等于
门限δ，则
说明WiFi-PF算法与Mag-PF算法定位跟踪失败，对WiFi-PF算法与Mag-PF算法中的粒子集分别进行处理，增加一部分分布在位置 周围的随机粒子，更新粒子集，那么更新后的粒子在第k+1步时起作用，当前步的定位坐标 若WiFi-KNN
算法的定位坐标 与融合后的定位坐标 之间的距离disk小于
门限δ，则将 作为当前步的定位坐标
[0017] 步骤8：若检测到计步，k＝k+1，跳到步骤3；否则，定位结束。
[0018] 本发明进一步的改进在于，步骤1)中初始化具体过程为：以L0(x0,y0)为圆心，R为半径画圆，在圆内随机分布Num个粒子，作为初始粒子集，其中，对WiFi-PF算法和Mag-PF算法生成各自的初始粒子集。
[0019] 本发明进一步的改进在于，步骤3中进行定位跟踪是否失败判断的具体过程如下：
[0020] 检测第k步前的20步内，Mag-PF算法的定位坐标LM-PF(xM-PF,yM-PF)进入不可达区域的次数RIM-PF，即Mag-PF算法的定位坐标LM-PF(xM-PF,yM-PF)进入不可达区域后启动重新初始化的次数RIM-PF，比较Mag-PF的定位结果进入不可达区域的次数RIM-PF与门限TM-PF的关系，若RIM-PF>TM-PF，则说明Mag-PF算法的定位跟踪失败；否则，说明Mag-PF算法的定位跟踪正常。
[0021] 本发明进一步的改进在于，启动重新初始化的具体过程如下：
[0022] ①粒子初始化：以n步前的定位坐标为圆心，R为半径画圆，在圆内随机生成Num个粒子，其中第k步前沿同一方向行走的步数为p，那么
[0023] ②状态转移：粒子运动的方向为n步内的平均方向，步长为n倍的每步步长；
[0024] ③观测值：行人在n步内采集到的地磁三维序列Bnow；
[0025] ④权值更新：第i个粒子在n步内的运动轨迹对应的地磁三维序列Bi，根据粒子
运动估计的起点与终点坐标通过
数据库中地磁指纹进行线性插值可得；计算第i个粒子的地磁三维序列Bi与观测值Bnow间的DTW距离记为Di，则第i个粒子的权重为 然后对权重进行归一化，其中σD2表示DTW距离的方差；
[0027] ⑥位置估计：对所有粒子的位置进行平均，得到第k步的定位坐标。
[0028] 本发明进一步的改进在于，步骤5的具体过程为：根据定位坐标与WiFi-KNN算法定位坐标 间的距离 定位坐标
与定位坐标 间的距离 将定位坐标 和定位坐标
进行加权得到定位坐标 二者加权的权重分别为
其中， 为定位坐标 的权重， 为定位坐标
的权重，ωk为参数。
[0029] 本发明进一步的改进在于，参数ωk的计算方式有两种：
[0030] ①倒数法：
[0031] ②指数法：
[0032] 本发明进一步的改进在于，WiFi-PF算法与Mag-PF算法的定位坐标融合后的定位坐标为 计算公式为
[0033]
[0034] 本发明进一步的改进在于，采用初始位置定位算法确定初始点坐标的具体过程如下：
[0035] 步骤1：行人开始行走后的N步，通过行人航位推测算法对智能手机的加速度计数据、陀螺仪数据、
电子罗盘数据及磁力计数据估计出行人行走的步数及每步的方向；
[0036] 步骤2：利用WiFi确定初始点区域，在行人开始行走的N步时间内，智能手机的WiFi扫描模
块进行周期性WiFi信号扫描，因此采集到多次WiFi的信号强度向量，对多次WiFi的信号强度向量进行均值处理，得到该段时间内的WiFi平均信号强度向量，利用最近邻算法进行定位，得到定位结果 然后将初始点区域设置为以定位结果 为圆心，R0为半径的圆；
[0037] 步骤3：利用地磁三维序列数据进行精确定位，确定初始点坐标，原理是利用最大后验估计准则，通过观测值来估计样本的后验概率，从而估计初始点坐标。
[0038] 本发明进一步的改进在于，步骤3中估计初始点坐标的具体过程如下：
[0039] ①在步骤2确定的初始点区域内，产生服从均匀分布的NumI个样本，样本位置集合为
[0040] {pi＝(xi,yi),i＝1,2,...,NumI}；
[0041] ②每个样本i根据步骤1中估计的步数N、步长di及前N步方向的平均方向 向前运动，运动轨迹记为li，第i个样本运动公式为
[0042]
[0043] 其中，步长di与运动方向θi都添加了高斯噪声，d表示每步步长， 表示N步步长Nd的方差， 表示平均方向 的方差；
[0044] 对于第i个样本，得到其运动轨迹li对应的地磁三维向量序列Mi，地磁三维向量序列Mi通过数据库中地磁指纹线性插值得到；
[0045] ③观测值为前N步中采集到的地磁三维向量序列Mnow，通过Mnow计算第i个样本的后验概率Pi，第i个样本的后验概率计算方式如下：计算第i个样本对应的地磁三维向量序列Mi与前N步中采集到的地磁序列Mnow间的DTW距离记为Di，则第i个样本的权重为 则后验概率为权重归一化后的值 其中σD2表示DTW距离的方差；
[0046] ④选择后验概率最大的K个样本，并进行3σ准则剔除异常样本，剩余K0个样本{pj＝(xj,yj),j＝1,2,...,K0}，最后对K0个样本的位置坐标进行均值处理，计算出初始点坐标[0047] 与
现有技术相比，本发明具有的有益效果：本发明利用基于WiFi的最近邻匹配算法(WiFi-KNN)、基于WiFi与PDR的粒子滤波算法(WiFi-PF)及基于地磁与PDR的粒子滤波算法(Mag-PF)的定位结果的独立性，相互校验，使定位保持跟踪，提高定位的鲁棒性。融合PDR、WiFi与地磁的粒子滤波算法的关键技术主要包括：WiFi-PF与Mag-PF定位结果融合策略，基于地磁与PDR的粒子滤波算法定位失败自我检测策略，WiFi-KNN辅助判断定位失败策略，粒子重新初始化策略，Mag-PF重新初始化策略。该系统在得到较精确的行人航位信息(包括计步及行人方向)的前提下，适用于平端、甩手、口袋、背包四种手机放置模式，且定位精度高。该方法利用基于WiFi与PDR的粒子滤波算法、基于地磁WiFi与PDR的粒子滤波算法及基于WiFi的指纹匹配算法在定位中的独立性，相互校验，使定位保持跟踪，提高定位的鲁棒性。
附图说明
[0048] 图1为精确的初始位置定位算法
流程图；
[0049] 图2为融合PDR、WiFi与地磁的粒子滤波算法的算法流程图；
[0050] 图3为测试环境的平面图。
具体实施方式
[0051] 为了更清楚地说明本发明
实施例的技术方案，下面将对实施例描述中所有需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域的普通技术人员来说，在不付出创造性劳动的前提下所获得的其他附图都属于本发明的保护范围。
[0052] 本发明中WiFi-PF算法为基于WiFi与PDR的粒子滤波算法，Mag-PF算法为基于地磁与PDR的粒子滤波算法，基于WiFi的指纹匹配算法使用的是基于WiFi的最近邻匹配算法，WiFi-KNN算法为基于WiFi的最近邻匹配算法，PDR算法为行人航位推测算法，KNN算法为最近邻算法。
[0053] 采用初始位置定位算法确定初始点坐标，然后通过融合PDR、WiFi与地磁的粒子滤波算法实现定位，具体过程如下：
[0054] 采用精确的初始位置定位算法，确定初始点坐标，具体包括以下步骤：
[0055] 步骤1：行人开始行走后的N步，通过行人航位推测算法(PDR算法)对智能手机的加速度计数据、陀螺仪数据、电子罗盘数据及磁力计数据估计出行人行走的步数及每步的方向；
[0056] 步骤2：利用WiFi确定初始点区域，在行人开始行走的N步时间内，智能手机的WiFi扫描模块进行周期性WiFi信号扫描，因此可以采集到多次WiFi的信号强度向量，对多次WiFi的信号强度向量进行均值处理，得到该段时间内的WiFi平均信号强度向量，利用最近邻算法(KNN算法)进行定位，得到定位结果 然后将初始点区域设置为以定位结果为圆心，R0为半径的圆；
[0057] 步骤3：利用地磁三维序列数据进行精确定位，确定初始点坐标，原理是利用最大后验估计准则，通过观测值来估计样本的后验概率，从而估计初始点坐标，具体过程如下：
[0058] ①在步骤2确定的初始点区域内，产生服从均匀分布的NumI个样本，样本位置集合为{pi＝(xi,yi),i＝1,2,...,NumI}；
[0059] ②每个样本i根据步骤1中估计的步数N、步长di及前N步方向的平均方向 向前运动，运动轨迹记为li，第i个样本运动公式为
[0060]
[0061] 其中，步长di与运动方向θi都添加了高斯噪声，d表示每步步长， 表示N步步长Nd的方差， 表示平均方向 的方差；
[0062] ③对于第i个样本，得到其运动轨迹li对应的地磁三维向量序列Mi，地磁三维向量序列Mi通过数据库中地磁指纹线性插值得到；观测值为前N步中采集到的地磁三维向量序列Mnow，通过Mnow计算第i个样本的后验概率Pi，第i个样本的后验概率计算方式如下：计算第i个样本对应的地磁三维向量序列Mi与前N步中采集到的地磁序列Mnow间的DTW距离记为Di，2
则第i个样本的权重为 则后验概率为权重归一化后的值 其中σD 表示
DTW距离的方差；
[0063] ④选择后验概率最大的K个样本，并进行3σ准则剔除异常样本，剩余K0个样本{pj＝(xj,yj),j＝1,2,...,K0}，最后对K0个样本的位置坐标进行均值处理，计算出初始点坐标[0064] 通过融合航位信息、WiFi与地磁的粒子滤波算法实现定位，具体过程如下：
[0065] 步骤1：以L0(x0,y0)为初始点分别进行基于WiFi与PDR的粒子滤波算法(WiFi-PF算法)和基于地磁与PDR的粒子滤波算法(Mag-PF算法)的粒子初始化，初始化的具体过程为以L0(x0,y0)为圆心，R为半径画圆，在圆内随机分布Num个粒子，作为初始粒子集，其中，需要对WiFi-PF算法和Mag-PF算法生成各自的初始粒子集；
[0066] 步骤2：利用PDR算法进行实时估计用户行走步数以及方向，对于PDR算法检测到的第k步，可以得到对应的WiFi-PF算法的定位坐标 Mag-PF算法的定位坐标 及WiFi-KNN的定位坐标
[0067] 步骤3：第k步，通过融合基于地磁与PDR的粒子滤波算法(Mag-PF算法)的定位坐标与地图信息进行定位跟踪是否失败判断，具体过程如下：
[0068] 检测第k步前的20步内，Mag-PF算法的定位坐标LM-PF(xM-PF,yM-PF)进入不可达区域的次数RIM-PF，即Mag-PF算法的定位坐标LM-PF(xM-PF,yM-PF)进入不可达区域后启动重新初始化的次数RIM-PF，比较Mag-PF算法的定位结果进入不可达区域的次数RIM-PF与门限TM-PF的关系，若RIM-PF>TM-PF，则说明Mag-PF算法的定位跟踪失败；否则，说明Mag-PF算法的定位跟踪正常；
[0069] 步骤4：若判断Mag-PF算法的定位跟踪失败，则以WiFi-PF算法的定位坐标为参考重新初始化粒子，即以当前第k步的WiFi-PF算法的定位坐标 为圆心，R为半径画圆，在圆内随机生成Num个粒子，代替原粒子集，作为下一步Mag-PF算法进行粒子滤波的粒子集，再进行步骤6；若判断Mag-PF算法的定位跟踪正常，则进行步骤5；
[0070] 步骤5：第k步时，将定位坐标 与定位坐标进行融合：根据定位坐标 与WiFi-KNN算法定位坐标 间
的距离 定位坐标 与定位坐标 间的距离
将定位坐标 和定位坐标 进行加权得到定位坐标
二者加权的权重分别为 其中， 为定位坐标
的权重， 为定位坐标 的权重；
[0071] 参数ωk的计算方式有两种：
[0072] ①倒数法：
[0073] ②指数法：
[0074] 那么，WiFi-PF算法与Mag-PF算法的定位坐标融合后的定位坐标为计算公式为
[0075]
[0076] 步骤6：将WiFi-PF算法的定位坐标作为融合的定位坐标，即
[0077] 步骤7：计算WiFi-KNN的定位坐标 与融合的粒子滤波定位坐标之间的距离 若WiFi-KNN算法的定位坐
标 与融合的粒子滤波定位坐标 之间的距离disk大于等于门
限δ，则说明WiFi-PF算法与Mag-PF算法定位跟踪失败，对WiFi-PF算法与Mag-PF算法中的粒子集分别进行处理，增加一部分分布在位置 周围的随机粒子，更新粒子集，那么更新后的粒子在第k+1步时起作用，而当前步的定位坐标 若
WiFi-KNN算法的定位坐标 与融合的粒子滤波定位坐标 之间
的距离disk小于门限δ，则把 作为当前步的定位坐标
[0078] 步骤8：若检测到计步，k＝k+1，跳到步骤3；否则，定位结束。
[0079] 其中，步骤3中Mag-PF算法中启动重新初始化的具体过程：是指当第k步Mag-PF算法的定位坐标到达不可达区域时，根据历史定位信息进行重新初始化，具体步骤为：
[0080] ①粒子初始化：以n步前的定位坐标为圆心，R为半径画圆，在圆内随机生成Num个粒子，其中第k步前沿同一方向行走的步数为p，那么
[0081] ②状态转移：粒子运动的方向为n步内的平均方向，步长为n倍的每步步长；
[0082] ③观测值：行人在n步内采集到的地磁三维序列Bnow；
[0083] ④权值更新：第i个粒子在n步内的运动轨迹对应的地磁三维序列Bi，可根据粒子运动估计的起点与终点坐标通过数据库中地磁指纹进行线性插值可得；计算第i个粒子的地磁三维序列Bi与观测值Bnow间的DTW距离记为Di，则第i个粒子的权重为 然后对权重进行归一化，其中σD2表示DTW距离的方差；
[0084] ⑤重采样：进行简单随机重采样；
[0085] ⑥位置估计：对所有粒子的位置进行平均，得到第k步的定位坐标。
[0086] 下面结合本发明的附图，对本发明实施例中的技术方案进行清楚、完整地描述：
[0087] 测试环境为某开放型办公室，测试环境大小为52m×60m，具体测试环境平面图如图3所示，实验测试手机为华为荣耀magic手机。
[0088] 离线阶段，WiFi指纹采用在固定
锚点处静态采集1min，锚点间距为3m，将每个锚点处采集到的多条WiFi信号强度向量进行均值处理，然后将锚点坐标及对应的WiFi信号强度均值向量上传至数据库；地磁指纹采用连续行走采集的方式，行走路线需要无重叠的
覆盖所有可达区域，然后对采集到的地磁序列离散化处理，生成锚点间隔0.5m的地磁指纹数据库，即每个锚点对应一条三维地磁向量。
[0089] 在线阶段，行人在测试环境中手持手机行走，行走距离约为280m，定位流程如下：
[0090] 采用精确的初始位置定位算法，确定初始点坐标，具体包括以下步骤：
[0091] 如图1所示，为在线阶段进行初始点定位的算法流程图，具体步骤如下，图1中：
[0092] 步骤1：行人开始行走后的N步，通过行人航位推测算法(PDR算法)对智能手机的加速度计数据、陀螺仪数据及磁力计数据估计出行人行走的步数及每步的方向，在实施例中N＝5。
[0093] 步骤2：利用WiFi确定初始点区域，在行人开始行走的N步时间内，智能手机的WiFi扫描模块进行周期性WiFi信号扫描，因此可以采集到多次WiFi的信号强度向量，对多次WiFi的信号强度向量进行均值处理，得到该段时间内的WiFi平均信号强度向量，利用最近邻算法(KNN)进行定位得到定位结果 然后将初始点区域设置为以定位结果 为圆心，R0为半径的圆。
[0094] 步骤3：利用地磁三维序列数据进行精确定位，原理是利用最大后验估计准则，通过观测值来估计样本的后验概率，从而估计初始点坐标，具体过程如下：
[0095] 在步骤2确定的初始点定位区域内，产生服从均匀分布的NumI个样本，样本位置集合为{pi＝(xi,yi),i＝1,2,...,NumI}；
[0096] ①每个样本i根据步骤1中估计的步数N、步长di及前N步方向的平均方向 向前运动，运动轨迹记为，第i个样本运动公式为
[0097]
[0098] 其中，步长di与运动方向θi都添加了高斯噪声，d表示每步步长， 表示N步步长Nd的方差， 表示平均方向 的方差；
[0099] 对于第i个样本，得到其运动轨迹li对应的地磁三维向量序列Mi，地磁三维向量序列Mi通过数据库中地磁指纹线性插值得到；
[0100] ②观测值为前N步中采集到的地磁三维向量序列Mnow，通过Mnow计算第i个样本的后验概率Pi，第i个样本的后验概率计算方式如下：计算第i个样本对应的地磁三维向量序列Mi与前N步中采集到的地磁序列Mnow间的DTW距离记为Di，则第i个样本的权重为 则后验概率为权重归一化后的值 其中σD2表示DTW距离的方差；
[0101] ③选择后验概率最大的K个样本，并进行3σ准则剔除异常样本，剩余K0个样本{pj＝(xj,yj),j＝1,2,...,K0}，最后对K0个样本的位置坐标进行均值处理，计算出初始点坐标[0102] 通过融合航位信息、WiFi与地磁的粒子滤波算法实现定位，具体步骤如下：
[0103] 如图2所示，为在线阶段的融合算法流程图，图2中：
[0104] 步骤1：以L0(x0,y0)为初始点分别进行基于WiFi与PDR的粒子滤波算法(WiFi-PF算法)和基于地磁与PDR的粒子滤波算法(Mag-PF算法)的粒子初始化，初始化的具体过程为以L0(x0,y0)为圆心，R为半径画圆，在圆内随机分布Num个粒子，作为初始粒子集，其中，需要对WiFi-PF算法和Mag-PF算法生成各自的初始粒子集；
[0105] 步骤2：利用PDR算法进行实时估计用户行走步数以及方向，对于PDR算法检测到的第k步，可以得到对应的WiFi-PF算法的定位坐标 Mag-PF算法的定位坐标 及WiFi-KNN的定位坐标
[0106] 步骤3：对于第k步，利用Mag-PF算法估计的定位坐标 判断其是否在不可达区域，若是，则启动重新初始化，并记录重新初始化的步数。利用历史信息进行重新初始化。其中，不可达区域为图3中无网格区域。重新初始化的流程如下：
[0107] ①粒子初始化：以n步前的定位坐标为圆心，R为半径画圆，在圆内随机生成Num个粒子，其中第k步前沿同一方向行走的步数为p，那么 其中R为定位开始时粒子初始化的半径；
[0108] ②状态转移：粒子运动的方向为n步内的平均方向，步长为n倍的每步步长；
[0109] ③观测值：行人在n步内智能手机采集到的地磁三维序列Bnow；
[0110] ④权值更新：第i个粒子在n步内的运动轨迹对应的地磁三维序列Bi，地磁三维序列Bi可根据粒子运动轨迹的起点与终点坐标通过数据库中地磁指纹进行线性插值可得；计算第i个粒子的地磁三维序列Bi与观测值Bnow间的DTW距离记为Di，则第i个粒子的权重为2
然后对权重进行归一化，其中σD表示DTW距离的方差；
[0111] ⑤重采样：根据粒子集位置坐标及粒子权重进行简单随机重采样；
[0112] ⑥位置估计：对所有粒子的位置进行平均得到第k步的定位坐标。
[0113] 步骤4：在第k步，对基于地磁的粒子滤波算法(Mag-PF算法)的定位坐标通过融合地图信息进行Mag-PF算法定位失败自我检测：
[0114] 检测第k步前的20步内，Mag-PF算法的定位坐标LM-PF(xM-PF,yM-PF)进入不可达区域的次数，即Mag-PF算法的定位坐标LM-PF(xM-PF,yM-PF)进入不可达区域后启动重新初始化的次数RIM-PF，比较Mag-PF算法的定位结果进入不可达区域的次数RIM-PF与门限TM-PF的关系，若RIM-PF>TM-PF，则说明Mag-PF算法的定位跟踪失败；否则，说明Mag-PF算法的定位跟踪正常；其中门限TM-PF＝5。
[0115] 步骤5：若判断Mag-PF算法的定位跟踪失败，则以WiFi-PF算法的定位坐标为参考重新初始化粒子，即以当前第k步的WiFi-PF算法的定位坐标 为圆心，R为半径画圆，在圆内随机生成Num个粒子，代替原粒子集，作为下一步Mag-PF算法进行粒子滤波的粒子集，再进行步骤7；若判断Mag-PF算法的定位跟踪正常，则进行步骤6；
[0116] 步骤6：第k步，Mag-PF算法未检测出粒子跟踪失败，将定位坐标与定位坐标 进行融合：根据定位坐标 和定位坐标
与WiFi-KNN算法定位坐标 间的距离 和 将
定位坐标 和定位坐标 进行加权得到定位坐标
二者加权的权重分别为 其中， 为定位坐标
的权重， 为定位坐标 的权重；参数ωk的计算方式有两种：
[0117] ①倒数法：
[0118] ②指数法：
[0119] 其中，定位坐标 与WiFi-KNN算法定位坐标 间的距离计算公式为
[0120]
[0121] 定位坐标 与定位坐标 间的距离计算公式为
[0122]
[0123] 那么，WiFi-PF算法与Mag-PF算法定位结果融合后的定位坐标为 计算公式为
[0124]
[0125] 若Mag-PF算法检测出粒子跟踪失败，则将WiFi-PF算法的定位坐标作为融合的定位坐标，即
[0126] 本实施例中，采用指数法进行WiFi-PF算法与Mag-PF算法定位坐标的加权。
[0127] 步骤7：计算WiFi-KNN算法的定位坐标 与融合的粒子滤波定位坐标 之间的距离 若WiFi-KNN算法的定位坐
标 与融合的粒子滤波定位坐标 之间的距离disk大于等于门
限δ，则说明WiFi-PF算法与Mag-PF算法定位失败。因此需要进行更新粒子集操作，对WiFi-PF算法与Mag-PF算法的粒子群分别进行处理，增加一部分分布在坐标 周围的随机粒子，更新粒子集，那么更新后的粒子在第k+1步时起作用，而当前步的定位坐标为WiFi-PF算法与Mag-PF算法加权的定位坐标 若WiFi-KNN算法的
定位坐标 与融合的粒子滤波定位坐标 之间的距离disk小于门
限δ，则直接把 作为当前步的定位坐标
[0128] 步骤8：若检测到计步，k＝k+1，跳到步骤3；否则，定位结束。
[0129] 本实施例的定位结果如表1所示，表1为WiFi-KNN(wifi)算法、Mag-PF算法、WiFi-PF算法及融合定位算法(Hybrid-PF)的平均定位误差及Hybrid-PF与其他三种算法相比，定位精度提升的百分比：
[0130]算法名称 WiFi-KNN Mag-PF WiFi-PF Hybrid-PF
平均定位误差(m) 2.86 2.79 3.54 2.41
定位精度提升(％) 15.73 13.62 31.92 -
[0131] 从表1可以看出，本发明的定位方法精度高。
