一种基于EKF滤波的初始状态自适应融合定位方法

本发明涉及定位追踪的技术领域，具体涉及一种基于ekf滤波的初始状态自适应融合定位方法。

背景技术：

随着新一代蜂窝网技术的商用落地，室内定位技术再次引起人们的广泛关注，日益增长的生活与商业需求的位置服务刺激了室内定位系统和相关技术的快速发展。典型的室内定位技术包括wi-fi、蓝牙、rfid、超宽带等，其中，wi-fi位置指纹定位的网络覆盖广，可满足大多数室内定位需求，但其信号易受环境干扰，导致定位结果产生较大波动；行人航位推算(pedestriandeadreckoning,pdr)定位方法利用惯性传感单元(inertialmeasurementunit，imu)实现航位推算定位，目前移动终端设备支持良好，但存在较大的惯性累积误差。

为提高上述单一定位系统的性能，目前的研究对有关wi-fi与pdr的融合定位系统与技术进行了探讨与分析。多数情况下，可以通过融合定位的方法来改善单一系统的定位性能，这样一方面可以减少wi-fi定位的波动性所带来的影响，另一方面可以降低pdr在定位过程中所带来的累积误差，但目前基于wi-fi/pdr的融合定位方法仍存在如下问题：

1)忽略了包括始发位置与航向角在内的初始状态确定过程，这样可能会导致定位系统在有限时间内无法快速收敛；

2)在使用扩展卡尔曼滤波ekf算法进行融合时，wi-fi信号强度数据的误差衡量往往使用了自定义的固定值，没有很好的将真实环境下wi-fi信号强度数据波动所带来的影响反馈给ekf系统。

技术实现要素：

本发明提供了一种基于ekf滤波的初始状态自适应融合定位方法，解决了现有融合算法没有考虑初始状态对系统快速收敛的影响，使用了自定义的固定值衡量wi-fi信号强度数据的误差影响系统最终定位误差等问题。

本发明可通过以下技术方案实现：

一种基于ekf滤波的初始状态自适应融合定位方法，将待测区域进行网格化处理，建立离线状态下的wi-fi位置指纹数据库，再利用扩展卡尔曼滤波方法ekf将wi-fi位置指纹定位方法和行人航位推算方法pdr进行融合，其用户运动时的初始状态是在wi-fi位置指纹定位方法的基础上，结合多点卡尔曼滤波方法获得，同时，在扩展卡尔曼滤波方法ekf的迭代过程中，则以用户运动过程中相邻两个状态的信号强度数据rssi的欧氏距离来构成动态的观测噪声协方差矩阵，以完成对用户运动时下一个状态的位置预测。

进一步，所述初始状态包括初始位置l0和初始航向角θ0，以wi-fi位置指纹数据库为基础，利用wi-fi位置指纹定位方法解算得到用户由静止开始运动m步后预测的各个位置点坐标记为在每个位置点i重新采集得到m组wi-fi信号强度数据，再利用wi-fi位置指纹定位方法解算得到每个位置点i对应的m个定位结果，记为l′i(j)(j＝1,2，…,m)，然后利用多点卡尔曼滤波方法对所有的定位结果进行迭代解算获得滤波后的各个位置点i坐标(xi,yi)(i＝0,1,2，…,m)，记为则初始位置初始航向角

进一步，在扩展卡尔曼滤波方法ekf的迭代过程中，利用如下方程式计算观测噪声协方差矩阵rk

其中，

当k＝0时，初始值δ0设置为常量，为wi-fi指纹定位方法的平均误差；

当k≥1时，表示用户运动时第k个状态和第k-1个状态对应的rssi信号强度数据的欧式距离，表示用户运动时第k个状态对应的位置点所采集到一组共t个wi-fi信号强度数据，[dmin,dmax]表示在一个稳定环境下dk的取值范围，[smin,smax]表示采用如权要求1所述的一种基于ekf滤波的初始状态自适应融合定位方法的定位精度范围，dmin、dmax、smin、smax均设置为经验值，δd表示用户运动时的步长误差，δθ表示用户运动时的航向角误差，均设置为常量。

进一步，利用扩展卡尔曼滤波方法ekf将wi-fi位置指纹定位方法和行人航位推算方法pdr进行融合，以完成对用户运动时下一个状态的位置预测包括以下步骤：

步骤一、在扩展卡尔曼滤波方法ekf的预测阶段，利用如下公式计算得到用户运动时第k个状态对应的系统状态矩阵xk′和噪声协方差矩阵pk′，

其中，表示状态转移矩阵，xk-1、yk-1、θk-1分别表示采用扩展卡尔曼滤波方法ekf获得的用户运动时第k-1个状态对应的位置坐标和航向角，x′k、y′k、θ′k分别表示通过行人航位推算方法pdr预测后的用户运动时第k个状态对应的位置坐标和航向角，dk表示用户运动时第k个状态对应的步长，w表示系统状态的高斯白噪声向量，△θ表示航向角的估计增量，此处设置为一个常量；

p′k＝apk-1a^t+q；

其中，pk-1表示采用扩展卡尔曼滤波方法ekf获得的用户运动时第k-1个状态的噪声协方差矩阵，其初始值为表示预测模型本身所带来的噪声，由行人航位推算方法pdr各要素的平均误差组成，δx、δy表示位置的误差，δθ表示航向角的平均误差；

步骤二、在扩展卡尔曼滤波方法ekf的更新阶段，利用权利要求3所述的方法计算得到用户运动时第k个状态对应的观测噪声协方差矩阵rk，再利用如下方程式计算得到卡尔曼增益kk，从而获得用户运动时第k个状态的估计状态矩阵则用户运动时第k个状态的定位结果为(xk,yk)，

kk＝p′kh^t(hp′kh^t+rk)^-1，其中，观测矩阵

xk＝x′k+kk(zk-hx′k)，其中，表示观测方程，v表示观测方程的高斯白噪声向量，w、v相互独立，分别表示采用wi-fi指纹定位方法所得到用户运动时第k个状态的位置坐标，

然后，利用方程式pk＝(1-kkh)p′k，计算更新后的用户运动时第k个状态的噪声协方差矩阵；

步骤三、执行步骤一至二，直至完成用户运动过程中所有的位置点检测。

进一步，通过weinberg步长估计模型确定步长dk，

其中，c表示步长比例因子，为固定常量，amax和amin分别表示用户运动一个状态的过程中所检测到的最大加速度和最小加速度。

进一步，建立离线状态下的wi-fi位置指纹数据库的方法包括以下步骤：

1)、采用均匀等分的方式将定位待测区域划分为若干个网格，用每个网格的顶点作为参考点rp；

2)、通过信号采集设备获取每个参考点rp的多个接入点在单一信道上的n个wi-fi信号强度数据，并进行滤波处理，从而构成一组信号指纹数据其中，l表示第l个参考点rp位置，l＝1,2，…,l；

3)、每个参考点rp同时还需要获取该位置的坐标信息和mac地址，以此作为一组位置指纹数据，建立离线状态下的位置指纹数据库rpx

本发明有益的技术效果在于：

在wi-fi/pdr算法的基础上，采用ekf将wi-fi与pdr的定位信息进行融合，并提出了一种自适应解算初始状态的方法以避免初始值偏差所带来的影响，从而保证结果能快速收敛，同时使用了一种动态调节ekf系统的方法，融合系统在扩展卡尔曼滤波的迭代过程中对相邻两个状态的rssi欧氏距离进行度量，并反馈给ekf系统的下一个状态，从而保证了有效的位置轨迹约束，降低了wi-fi数据波动对系统所造成的影响，获得了精确且稳定的融合定位结果。另外，本发明的方法简单可靠，操作方便，易于实现，便于推广应用。

附图说明

图1是本发明的融合定位方法的流程图；

图2是本发明的实验测试场景图；

图3是本发明的不同的初始位置对定位结果所造成的影响分析图；

图4是本发明的不同的初始航向角对定位结果所造成的影响分析图；

图5是本发明的融合定位方法中不同m取值对应的性能对比图；

图6是本发明的融合定位方法中不同m取值对应的复杂度对比图；

图7是本发明的融合定位方法的初始状态解算示意图；

图8是本发明所提方法与wi-fi定位、pdr定位、传统基于ekf的融合算法的定位轨迹对比图。

具体实施方式

下面结合附图及较佳实施例详细说明本发明的具体实施方式。

wi-fi定位波动性较大，而pdr定位存在累积误差，如图1所示，本发明提供了一种基于ekf滤波的初始状态自适应融合定位方法，将待测区域进行网格化处理，建立离线状态下的wi-fi位置指纹数据库，再利用扩展卡尔曼滤波方法ekf将wi-fi位置指纹定位方法和行人航位推算方法pdr进行融合，其用户运动时的初始状态是在wi-fi位置指纹定位方法的基础上，结合多点卡尔曼滤波方法获得，同时，在扩展卡尔曼滤波方法ekf的迭代过程中，则以用户运动过程中相邻两个状态的信号强度数据rssi的欧氏距离来构成动态的观测噪声协方差矩阵，以完成对用户运动时下一个状态的位置预测，从而降低了因对用户运动时的初始状态设定不当而导致系统无法快速收敛的影响，同时在扩展卡尔曼滤波ekf的迭代过程中对相邻状态的rssi欧氏距离进行度量，并反馈给ekf系统的下一状态，从而保证了有效的位置轨迹约束，降低了wi-fi数据波动对系统所造成的影响，获得了精确且稳定的融合定位结果。具体包括以下步骤：

步骤一、建立离线状态下的wi-fi位置指纹数据库

首先、采用均匀等分的方式将定位待测区域划分为若干个网格，用每个网格的顶点作为参考点rp；

然后、利用信号采集设备获取每个参考点rp的多个接入点在单一信道上的n个wi-fi信号强度数据，对不同参考点rp的数据均进行高斯滤波处理，选取大概率区间的wi-fi信号强度数据作为采样数据的有效值，再使用均值法进行平滑处理，从而构成一组信号指纹数据其中，l＝1,2，…,l，l表示第l个参考点rp位置；

最后，对于每个参考点rp同时还需要获取该位置的坐标信息和mac地址，以此作为一组位置指纹数据，建立离线状态下的位置指纹数据库rpx

步骤二、由于卡尔曼滤波器的初始偏差可能会导致滤波计算的前若干步滤波结果产生较大偏差，即使在算法进入收敛状态之后，也可能会因为初始值设置不当，导致收敛的滤波结果难以收敛到状态的真实值，因此，采用wi-fi位置指纹定位方法解算出的位置点不能直接作为扩展卡尔曼ekf系统的初始位置值，并且后续步骤需要与行人航位推算方法pdr融合，也很难得到ekf系统的初始航向角，从而需要相对精准的初始状态作为系统初值，确保融合系统能够快速收敛，以提高最终的融合定位精度，本发明通过基于多点卡尔曼滤波的方法获得用户运动时的初始状态，该初始状态包括初始位置l0与初始航向角θ0。

1)、以wi-fi位置指纹数据库为基础，利用wi-fi位置指纹定位方法如基于wknn的在线匹配算法解算得到用户由静止开始运动m步后预测的各个位置点坐标记为

2)、在上述的每个位置点i重新采集得到m组wi-fi信号强度数据，再利用wi-fi位置指纹定位方法解算得到每个位置点i对应的m个定位结果，记为l′i(j)(j＝1,2，…,m)，然后利用多点卡尔曼滤波方法对所有的定位结果进行m次迭代解算获得滤波后的各个位置点i坐标(xi,yi)(i＝0,1,2，…,m)，记为则初始位置初始航向角

步骤三、利用扩展卡尔曼滤波方法ekf将wi-fi位置指纹定位方法和行人航位推算方法pdr进行融合，以完成对用户运动时下一个状态的位置预测，具体如下：

步骤ⅰ、在扩展卡尔曼滤波方法ekf的预测阶段，利用如下公式计算得到用户运动时第k个状态对应的系统状态矩阵xk′和噪声协方差矩阵pk′，

其中，表示状态转移矩阵，xk-1、yk-1、θk-1分别表示采用扩展卡尔曼滤波方法ekf获得的用户运动时第k-1个状态对应的位置坐标和航向角，x′k、y′k、θ′k分别表示通过行人航位推算方法pdr预测后的用户运动时第k个状态对应的位置坐标和航向角，w表示系统状态的高斯白噪声向量，△θ表示航向角的估计增量，此处设置为一个常量，dk表示用户运动时第k个状态对应的步长，可通过weinberg步长估计模型确定，具体可以参考文献harveyweinberg.usingtheadxl202inpedometerandpersonalnavigationapplications[j].applicationnotesamericandevices,usa:analogdevicesinc,2002，

其中，c表示步长比例因子，为固定常量，amax和amin分别表示用户运动一个状态的过程中所检测到的最大加速度和最小加速度；

p′k＝apk-1a^t+q；

其中，pk-1表示采用扩展卡尔曼滤波方法ekf获得的用户运动时第k-1个状态的噪声协方差矩阵，其初始值为表示预测模型本身所带来的噪声，由行人航位推算方法pdr各要素的平均误差组成，δx、δy表示位置的误差，δθ表示航向角的平均误差；

步骤ⅱ、在扩展卡尔曼滤波方法ekf的更新阶段，利用如下方程式计算得到用户运动时第k个状态对应的观测噪声协方差矩阵rk和卡尔曼增益kk，从而获得用户运动时第k个状态的估计状态矩阵则用户运动时第k个状态的定位结果为(xk,yk)。

在现有融合系统中，wi-fi信号强度数据的误差衡量往往使用了自定义的固定值，但在复杂的定位环境中，由于多径效应等因素的影响，wi-fi位置指纹定位系统的噪声并不严格满足高斯分布，这会对最终的定位结果产生很大的影响。因此，本发明在基于ekf的室内wi-fi/pdr融合定位算法的基础上，可以根据电磁环境与其反馈的信息进行动态调整，即采用了以相邻状态下的rssi欧氏距离为基础的噪声反馈机制，从而能够提高最终的定位精度。

用户在运动行走过程中，对相邻两个状态的rssi信号强度数据进行欧氏距离的求解，在每个系统中，它会保持在稳定的取值范围里面，记为[dmin,dmax]，然后将其线性放缩到了一个定位精度的阈值范围，即[smin,smax]，最后将其动态作为ekf的观测协方差矩阵的参数，以此来动态衡量wi-fi信号强度数据的噪声，具体如下：

在扩展卡尔曼滤波方法ekf的迭代过程中，利用如下方程式计算观测噪声协方差矩阵rk

其中，

当k＝0时，初始值δ0设置为常量，为wi-fi指纹定位方法的平均误差；

当k≥1时，表示用户运动时第k个状态和第k-1个状态对应的rssi信号强度数据的欧式距离，表示用户运动时第k个状态对应的位置点所采集到一组共t个wi-fi信号强度数据，[dmin,dmax]表示在一个稳定环境下dk的取值范围，[smin,smax]表示采用如权要求1所述的一种基于ekf滤波的初始状态自适应融合定位方法的定位精度范围，dmin、dmax、smin、smax均设置为经验值，δd表示用户运动时的步长误差，δθ表示用户运动时的航向角误差，均设置为常量。

kk＝p′kh^t(hp′kh^t+rk)^-1，其中，观测矩阵

xk＝x′k+kk(zk-hx′k)，其中，表示观测方程，v表示观测方程的高斯白噪声向量，w、v相互独立，分别表示采用wi-fi指纹定位方法所得到用户运动时第k个状态的位置坐标，

然后，利用方程式pk＝(1-kkh)p′k，计算更新后的用户运动时第k个状态的噪声协方差矩阵；

步骤ⅲ、执行步骤ⅰ至ⅱ，直至完成用户运动过程中所有的位置点检测。

为了验证本发明方法的可行性，我们进行了仿真实验，如图2所示，仿真环境为某校电子电气工程学院大楼7楼，在地面标定1m×1m方格，并以其顶点作为实验样本采集点即参考点rp，实验选取了8个tp-link型号为tl-wr886n的路由器作为定位信标，采用华为荣耀8作为pdr和wi-fi数据的采集工具，分别采用wi-fi定位、pdr定位、传统基于ekf的融合算法和本发明的改进的基于ekf的wi-fi/pdr融合定位算法进行定位实验。实验的行走轨迹为l型路线，起始坐标为(2，1)，终点坐标为(14，13)，总共行走步伐为30步，即总计31个位置状态，实验采用了固定步长的行走方式，每一步的距离即步长为0.8m。

如图3所示，对于初始位置，在初始航向角已知的情况下，即设为0°，以1m为间隔，将初始位置偏差值设置为1～10m，对于其中1m、5m、10m轨迹的偏移情况如图4所示。通过输出的轨迹图可以发现，虽然在开始的时候会有较大的偏离，但是最后都能收敛到正常值附近。往往位置偏差的距离越大，收敛的迭代次数也会越大，但是偏差超过一定界限以后，收敛次数会相对稳定下来。

如图4所示，对于初始航向角，在初始位置已知的情况下，即设为(2，1)，以5°为间隔，将初始角度偏差值设置为10～60°，对于其中10°、30°、60°轨迹偏移情况。通过输出的轨迹图可以发现，初始航向角的偏差不会影响输出结果的收敛速度，但是结果会产生持续的偏差，偏差结果会以非常慢的速度向正常值附近逼近。

如图5与图6所示，实验分析了式(12)中m取值不同时初始状态的偏差情况，随着m增加，航向角初始状态越好，尽管随之复杂度也会增加，但m＝5时算法耗时也在20ms内，不会影响最终的用户体验。

如图7所示，通过本发明提出的初始状态解算方法，定位结果能够快速收敛，四步内即可达到收敛状态。

如图8所示，结果表明wi-fi位置指纹定位方法虽然能够输出一个绝对定位值，但数据波动较大，pdr定位在短时间内可以保持良好的精确度，但随着迭代次数的增加，累积误差会越来越大。而本发明提出的改进ekf算法能很好地融合wi-fi定位和pdr定位方法的优势，相较于其他ekf算法，其精度有较为明显的改善，表明本发明所提方法能够有效抑制误差，提高定位精度。

虽然以上描述了本发明的具体实施方式，但是本领域的技术人员应当理解，这些仅是举例说明，在不背离本发明的原理和实质的前提下，可以对这些实施方式做出多种变更或修改，因此，本发明的保护范围由所附权利要求书限定。