本文采用粒子滤波算法融合WiFi定位和PDR，将基于支持向量机(support vector machines, SVM)的WiFi指纹定位获得的用户位置作为观测数据，通过PDR对用户的步数、步长和方向的估计用于对用户的行为进行建模。该融合系统的结构如图 1所示。在粒子滤波中融合室内地图进行粒子穿墙检测，并对融合后的定位结果再次进行穿墙检测，最后得到粒子滤波融合的定位结果。

图  1  粒子滤波融合WiFi定位、PDR和地图匹配

统计信号处理中的滤波问题广泛存在于导航、跟踪、自控和信息融合等领域，其基本目标是运用概率方法从充满噪声的观测数据中估计动态系统的状态。动态系统的状态方程和观测方程可以表示为：

式中，$ f( \cdot ) $是状态模型；$ h( \cdot )$是观测模型；$ {x_k} $代表系统状态；z_k代表观测值；v_k和w_k分别代表系统噪声和观测噪声。滤波的任务就是递推地在每次获得观测数据z_k后，估算出系统状态x_k的概率分布$ p({x_k}|{z_{1:k}}) $。

粒子滤波^[16]通过寻找一组状态空间中的样本(称为粒子)，对概率密度函数$ p{\rm{(}}{x_k}|{z_{1:k}}{\rm{)}} $进行近似，获得状态的最小方差估计。假定$ k - 1 $时刻系统状态的概率密度为$ p({x_{k - 1}}|{z_{1:k - 1}}) $，依据一定原则选取n个粒子$ \{ x_k^i, \omega _k^i\} (i = 1, 2, \cdots , n) $，$ x_k^i $为粒子状态，$ \omega _k^i $为粒子权重，在k时刻获得观测数据并经过时间预测和观测更新后，n个粒子的概率密度可近似为系统状态的概率密度$ p({x_k}|{z_{1:k}}) $，即：

式中，δ是Kronecker delta函数。粒子滤波不要求随机变量满足高斯分布，已在许多领域得到成功应用，包括定位问题。

WiFi定位会出现连续定位结果跳跃的现象，因此必须考虑定位的时序相关性。采用粒子滤波融合WiFi定位和PDR能有效解决时序性问题，进而减少定位误差并提高稳定性。在融合定位过程中，观测数据是基于SVM的WiFi指纹定位结果，系统状态是估计用户位置，而PDR用于分析用户运动状态并建立行为模型。假设$ {z_1}, {z_2}, \cdots , {z_k} $是截止到时刻k时连续的观测数据，即基于SVM的WiFi指纹定位估计的用户位置，x_k代表时刻k的系统状态即用户在时刻k的位置，粒子滤波就是要估计出时刻k在${z_1}, {z_2}, \cdots , {z_k} $的前提下x_k的概率分布$ p({x_k}|{z_{1:k}}) $。

WiFi定位采用SVC和SVR相结合的两级定位方案。两级定位的思想是先确定目标的大概区域，然后在区域里估计出精确的坐标位置。而定位之前，需要将整个定位区域按照楼层平面结构划分为若干子区域。如一个房间或房间外的一段走廊都是一个小的子区域等，然后分别训练区域分类模型和子区域坐标回归模型。具体为在各子区域内采集信号指纹，通过SVC训练，建立区域的分类模型，从而能够根据信号指纹确定所属的子区域。接着通过SVR训练，建立位置坐标和信号指纹之间的非线性依赖关系模型，在目标子区域可以根据信号指纹获得目标点的精确位置。WiFi指纹定位获得的位置坐标$(x, y)$作为后续粒子滤波中的观测值，和PDR进行融合。

PDR通过加速度计和磁力计来获得用户的的行走步数、步长和方向，用于在粒子滤波中对用户的行为进行建模。由于人在行走过程中速度不断变化，加速度曲线会出现上下偏移现象。采用简单移动平均算法对加速度数据进行平滑，平滑后的数据曲线分为静止、波峰和波谷3种状态，最后根据状态转换方法识别行走周期。改进PDR算法可根据加速度曲线的实时变化动态设定状态转换参数，从而使行走周期识别更加准确。识别出行走周期后，根据行走位移可得到基础步长^[4]。由于人在行走中的步长变化不大，可近似统一为固定步长，并通过卡尔曼滤波估计出更加精准的步长。方向由加速度计和磁力计确定。首先通过加速度向量和重力加速度计算出手机俯仰角和翻滚角，然后将基于手机坐标系的磁场强度向量转换成基于大地坐标系的磁场强度向量，使用大地坐标系中x和y方向的磁场强度确定行人方向。

所有粒子记为$\mathit{\boldsymbol{H}} = {\rm{\{ }}{\mathit{\boldsymbol{X}}^i}|i = 1, 2, \cdots , n{\rm{\} }} $，第i个粒子的状态空间为$ {\mathit{\boldsymbol{X}}^i} = {({x^i}, {y^i}, v_x^i, v_y^i)^{\rm{T}}} $，其中$ ({x^i}, {y^i}) $为粒子i的坐标，$ v_x^i $和$ v_y^i $分别为粒子在x和y方向的速度。初始粒子的位置$ {\rm{(}}x_0^i, y_0^i{\rm{)}} $采用高斯分布，初始观测值的均值为$ {\rm{(}}x_0^z, y_0^z{\rm{)}} $，标准差为2；初始粒子两个方向的速度采用高斯分布，$v_{x, 0}^i $和$ v_{y, 0}^i $均值为0，标准差为24；每个粒子的初始权重$ \omega _0^i $为$ 1/n $。

令k-1时刻的位置为$({x_{k - 1}}, {y_{k - 1}})$，k-1到k时刻之间持续时长记为Δt_k，该时间段内位移大小记为l_k，运动方向与y轴之间的夹角为θ。则人在k时刻的位置坐标可表示为：

假设$ {v_{x, k}} $和$ {v_{y, k}} $为在人在Δt_k时段内在x和y方向的速度。由于在正常情况下，人行走的速度变化不大，可以认为k时刻速度和k-1时刻的速度相等，即：

考虑到实际系统中存在由状态建模带来的误差和行走过程中的速度变化，假设$ {\sigma _c} $代表系统状态的噪声，$ {\sigma _v} $代表行走速度的变化，根据式(4)和式(5)可以得到系统状态方程为：

当在k时刻获得一个新的观测值(新的WiFi指纹定位结果)，并依据该观测值对所有粒子的权重进行更新。权重更新后重新筛选粒子，具体为舍弃权重较小的粒子，只保留权重较大的粒子。新的粒子权重值分布采用高斯分布，观测值$ (x_k^z, y_k^z) $为原点，各粒子与观测值之间的欧式距离为输入，标准差为1.1，即：

式中，d_i为粒子与观测值${\rm{(}}x_k^z, y_k^z{\rm{)}}$之间的距离：

然后将新的权重归一化：

粒子筛选采用随机重采样方式。根据更新后各粒子的权重，依次排列形成[0, 1]的粒子权重区间，其中各粒子的权重值决定了其区间长度。然后生成n个[0, 1]的随机数，找出其在权重区间的对应位置，将所有对应位置的粒子筛选出来形成新的粒子集。

在粒子状态和粒子权重更新后，系统当前状态$ {\mathit{\boldsymbol{X}}_k} = {{\rm{(}}{x_k}, {y_k}, {v_{x, k}}, {v_{y, k}}{\rm{)}}^{\rm{T}}} $更新为：

式中，$ ({x_k}, {y_k}) $即为行人当前的位置。

采用粒子滤波融合WiFi定位和PDR解决了定位的时序性问题，但仍存在定位轨迹穿墙问题，需要结合室内地图匹配来解决。

室内地图建模采用矢量表示法与图层表示法相结合的方式。基于矢量表示法，室内地图模型包括：点、线、面。点表示室内某个位置，用地图中的x和y坐标表示。线表示室内的墙和门，包括起点、终点和意义。面则表示房间、走廊、楼梯等空间区域，包括构成该面的所有线和该面的意义。采用图层表示法的设计原则，点、线和面分布在不同的图层中。

粒子滤波算法存在粒子转移不当等问题，例如粒子转移到不可到达的区域或穿墙到另一个室内区域。利用室内地图的位置信息约束粒子转移，从而解决粒子的不恰当转移。粒子i的状态空间为$ {\mathit{\boldsymbol{X}}^i} = {({x^i}, {y^i}, v_x^i, v_y^i)^{\rm{T}}} $，地理属性为$ {G^i} = ({r^i}, {w^i}, {d^i}, {s^i}) $，$ {r^i} $为粒子所在的室内区域，$ {w^i}$为粒子周围的墙，$ {d^i} $为粒子可穿过的门，$ {s^i} $为粒子可到达的室内区域。然后在重新计算粒子权重和归一化粒子权重之间加入粒子可用性评估：

1) 如果粒子转移的目的地属于不可到达区域，如没有入口可进入的室内或在定位空间之外的区域，则置该粒子的可达值$ {\omega _r} $为0，否则为1：

2) 如果粒子转移到不可直接到达区域，即源区域与目的区域没有门相连，则置该粒子的直达值$ {\omega _c} $为0，否则为1：

这样，粒子i便有了3个权重值：由式(7)计算得到是否进入不可达区域的权重$ \omega _r^i $、是否进入不可直达区域的权重$ \omega _c^i $和距离权重$ \bar \omega _k^i $。将这3个权重值相乘得到新的粒子权重：

根据这个新的权重值进行归一化和重采样。

解决粒子穿墙问题后，轨迹穿墙问题明显改善，但还不能杜绝。观察发现这些穿墙通常发生在门附近的墙上。因此，如果滤波融合后估计的轨迹穿墙到另一个区域，则将穿墙点重定位到门的位置。这样直接矫正穿墙会导致后续的定位延迟过大。为弥补这种延迟，采用回退N步的方式进行穿墙矫正。如图 2所示，P为当前滤波融合系统估计的轨迹，P_k为k时刻的定位结果，D₁和D₂为两个门。在K时刻检测到轨迹线${P_{k - 1}}{P_k} $与墙W相交，而最近的门是D₂，则回退N步(此例中$ N = 2 $)，调整$ k-N $时刻的定位结果到门D₂上，表示为$ {Q_{k - N}} $，调整之后新的轨迹为Q，其上$ {Q_{k - N + 1}} $到Q_k之间的轨迹点Q_i的位置坐标为：

图  2  轨迹穿墙问题

穿墙矫正后，定位点$ {P_k} $转移到了$ {Q_k} $，滤波融合系统中的粒子也做出相应转移，以$ {Q_k} $为初始点对粒子坐标值重新进行初始化采样，而速度不变。

实验场景为电子科技大学第三教学楼二楼，大小为64 m×14 m，采用校园网WiFi环境，对定位区域中的200个参考点进行了WiFi指纹采样，设备是基于Android的三星Galaxy Note。实验为穿过多个房间和走廊的行走，手机屏幕向上并手持于胸前。测试主要采集3类数据：WiFi信号指纹、加速度和磁场强度，共采集到90条WiFi信号指纹、5 974条加速度数据和5 911条磁场强度数据，用时120 s。使用同样实验数据，将基于SVM的WiFi指纹定位、改进的PDR、基于粒子滤波的WiFi+PDR融合定位和基于粒子滤波的WiFi+PDR+Map融合定位4种方法进行对比。

本文采用平均定位误差和定位误差累计概率分布作为定位性能的度量标准。定位误差指的是实际位置和估计位置之间的欧几里得距离；平均定位误差是所有测试位置的定位误差的均值；定位误差累计概率分布则为定位误差小于特定值的累计概率。

4种方法的定位误差如表 1所示。由于无线信号的不稳定性及周围环境的干扰，WiFi定位的平均误差为3.77 m；PDR短期定位效果好，但会出现误差累积的现象，实验中平均误差为2.87 m；而经过粒子滤波融合后，WiFi定位和PDR相互补偿从而使得平均误差降到2.75 m，相比WiFi定位和PDR提高了27.1%和4.2%。地图匹配后，通过将粒子和轨迹约束到合理区域内，平均误差达到1.81 m，相比粒子滤波的融合结果又提高了34.2%。图 3为4种方法的定位误差累计概率分布。可以看出基于粒子滤波的融合定位系统明显优于单独的定位系统，地图匹配能进一步明显地提高定位精度。在定位初期，PDR效果良好，甚至优于WiFi和PDR的融合结果，但随着PDR误差的累积，融合系统逐渐显现出优势。

图  3  定位精度对比

图 4为粒子滤波融合WiFi和PDR的定位结果，图 5为粒子滤波融合WiFi、PDR和地图匹配的定位结果。实验人员从图最右侧位置开始，经过走廊和4个房间，最终在左下角房间结束，接近匀速行走。两图中房间内的编号代表用于两级WiFi定位的子区域编号，带小方框的实线是真实轨迹，基于SVM的WiFi定位绘制的轨迹点和真实轨迹相差较大。由于WiFi定位是单点定位，且没有上下文关联信息，故定位轨迹难以形成连续结果，位置跳变现象经常出现。PDR绘制的轨迹和真实轨迹的重合度比WiFi结果要好，但后期的误差累积导致时间越长定位误差越大。图 4中基于粒子滤波的WiFi+PDR融合定位算法得到的行走轨迹虽然和真实轨迹不尽重合，但由于融合算法中WiFi定位修正了PDR的误差累积，而PDR修正了WiFi定位结果不连续的现象，使得WiFi+PDR融合算法明显优于WiFi定位和PDR。基于粒子滤波的WiFi+PDR+Map融合算法绘制出的轨迹，由于运用室内地图矫正了粒子穿墙和轨迹穿墙现象，使得定位结果更加合理与准确，是4种方法中最接近真实轨迹的定位曲线，误差波动较小，表现优异。由于在轨迹穿墙处采用回退N步的方式进行穿墙矫正，基于粒子滤波的WiFi+PDR+Map融合算法得到的定位轨迹，包含若干在穿墙处的断点。

图  4  WiFi+PDR估计的行走轨迹

图  5  WiFi+PDR+Map估计的行走轨迹

基于WiFi指纹和基于手持移动设备传感器的PDR是室内定位常用的方案。但是WiFi指纹的定位精度和稳定性都不高，而单一PDR方法会出现误差线性累计问题，不能应用于实际复杂的定位场景。本文融合这两个定位结果并结合地图信息，充分发挥各自的技术特点，避免了WiFi定位和PDR单一定位系统的不足，使其在稳定性、适应性和精确性等方面都得到提高。

实验先通过粒子滤波将WiFi指纹和PDR结果进行融合，使得定位系统时序相关，从而降低定位系统中定位结果跳变现象的可能性；然后建立矢量室内地图，将室内地图信息融合到基于粒子滤波的WiFi+PDR定位算法中，采用粒子穿墙约束和轨迹穿墙矫正方法，减少不正常的状态转移，显著提高了最终的定位精度。通过仿真对比实验，基于粒子滤波的融合定位算法通过对地图匹配前后粒子滤波融合系统的定位精度和行走轨迹对比，应用已知的室内地图信息能够有效地解决定位轨迹穿墙问题，使得定位结果更加合理精确。