CN111683337B - 基于Wifi和传感器的融合定位方法、系统、电子设备及存储介质 - Google Patents

基于Wifi和传感器的融合定位方法、系统、电子设备及存储介质 Download PDF

Info

Publication number
CN111683337B
CN111683337B CN202010526503.8A CN202010526503A CN111683337B CN 111683337 B CN111683337 B CN 111683337B CN 202010526503 A CN202010526503 A CN 202010526503A CN 111683337 B CN111683337 B CN 111683337B
Authority
CN
China
Prior art keywords
time
variable
state variable
equation
observation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010526503.8A
Other languages
English (en)
Other versions
CN111683337A (zh
Inventor
曹敏
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Spreadtrum Communications Shanghai Co Ltd
Original Assignee
Spreadtrum Communications Shanghai Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Spreadtrum Communications Shanghai Co Ltd filed Critical Spreadtrum Communications Shanghai Co Ltd
Priority to CN202010526503.8A priority Critical patent/CN111683337B/zh
Publication of CN111683337A publication Critical patent/CN111683337A/zh
Application granted granted Critical
Publication of CN111683337B publication Critical patent/CN111683337B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/023—Services making use of location information using mutual or relative location information between multiple location based services [LBS] targets or of distance thresholds
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/025—Services making use of location information using location based information parameters
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/025—Services making use of location information using location based information parameters
    • H04W4/027—Services making use of location information using location based information parameters using movement velocity, acceleration information
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/30—Services specially adapted for particular environments, situations or purposes
    • H04W4/38—Services specially adapted for particular environments, situations or purposes for collecting sensor information

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明提供一种基于Wifi和传感器的融合定位方法、系统、电子设备及存储介质，该方法包括：获取移动终端的初始坐标位置；构建包含坐标位置、速度的状态变量；建立状态方程；根据Wifi RTT定位算法获取第一观测变量；根据第一观测变量、k‑1时刻的状态变量和状态方程，估计第k时刻的初始状态变量；根据传感器数据识别移动终端所处的运动模式；静止模式时，将零速度作为第k时刻的第二观测变量；根据第k时刻的第二观测变量对第k时刻的初始状态变量进行修正；步行模式时，获取第k时刻移动终端的PDR位置作为第三观测变量；根据第三观测变量对第k时刻的初始状态变量进行修正。本发明根据不同运动模式获取不同观测变量与Wifi RTT估计结果进行融合，定位更加准确。

Description

基于Wifi和传感器的融合定位方法、系统、电子设备及存储 介质
技术领域
本发明涉及定位技术领域，尤其涉及一种基于Wifi和传感器的融合定位方法、系统、电子设备及存储介质。
背景技术
Wifi(基于IEEE802.11的无线局域网技术)广泛应用于家庭、办公场所、机场、商场等各类建筑物内，智能手机、笔记本电脑等大部分移动终端均内嵌了Wifi模块，Wifi几乎无处不在，因此Wifi定位技术成为室内定位的热点。随着无线通信技术的发展，IEEE802.11mc标准增加了Wifi RTT(Round-Trip-Time，既“往返时延”)的相关协议，使得Wifi定位精度可以达到1-2米，相比基于Wifi RSSI(接收信号强度)的定位精度要高出许多，同时谷歌Android P也引入了Wifi RTT定位功能。
Wifi RTT定位技术是指通过获取MT(Mobile Terminal，移动终端)与AP(AccessPoints，无线局域网接入点)之间往返信号的到达时间RTT，并根据RTT时间计算出MT与AP之间的距离，当MT获取到3个及以上的AP的位置和相应的RTT后，通过三边定位算法估算出MT位置的一种定位技术。
Wifi RTT的定位精度与RTT的测量误差直接相关，因此在使用RTT进行定位之前一般会进行RTT工厂离线校准。具体方法如下：首先需要准确的测量出AP与MT的真实距离，且AP到MT之间保持开阔无遮挡，不断的增加AP与MT的距离，通过多次、多点测量，记录真实距离与实测RTT距离，通过拟合算法估计出RTT的时间偏差。离线校准的缺陷在于需要提前部署与多次测量，该方法无法在线使用。
除了RTT离线校准技术外，还可以采用sensor(传感器)与Wifi进行在线融合，进而提升Wifi RTT在遮挡、多径情况下的定位精度。随着移动终端的发展，加速度计、陀螺仪、磁强计等成为移动终端的标配传感器，基于sensor的PDR定位可以弥补Wifi定位在遮挡、多径场景下的缺陷。因此，sensor/Wifi融合定位成为室内定位技术发展趋势。然而，现有融合定位方案仅仅将sensor PDR推算得到的位置与wifi RTT的位置进行简单融合，没有考虑根据移动终端的不同运动模式获取不同观测变量与wifi RTT进行融合，因而定位精度有待提高；此外，该方案也没有考虑将wifi RTT的时钟误差作为估计量，没有对时钟误差进行在线估计与校准。
发明内容
本发明的目的在于提供一种基于Wifi和传感器的融合定位方法、系统、电子设备及存储介质，可根据移动终端的不同运动模式获取不同观测变量与wifi RTT定位结果进行融合，使定位更加精确。
为了实现上述目的，本发明提供一种基于Wifi和传感器的融合定位方法，包括：
获取移动终端的初始坐标位置；
构建包含坐标位置和速度的状态变量，其中所述坐标位置的初值赋为所述初始坐标位置，所述速度的初值赋为0；
建立从第k-1时刻到第k时刻的状态方程，其中k>1；
根据Wifi RTT定位算法，获取第k时刻所述移动终端与预设的多个AP之间的距离并将所述距离作为第k时刻的第一观测变量；
根据所述第k时刻的第一观测变量、第k-1时刻的状态变量和所述状态方程，采用滤波算法估计得到第k时刻的初始状态变量；
根据所述移动终端内的传感器采集的传感器数据，识别第k时刻所述移动终端所处的运动模式；
当所述运动模式为静止模式时，将零速度作为第k时刻的第二观测变量；
根据所述第k时刻的第二观测变量，采用滤波算法对所述第k时刻的初始状态变量进行修正，得到第k时刻的目标状态变量；
当所述运动模式为步行模式时，基于第k-1时刻所述移动终端的坐标位置、以及所述传感器数据进行PDR计算，得到第k时刻所述移动终端的PDR位置并将所述PDR位置作为第k时刻的第三观测变量；
根据所述第k时刻的第三观测变量，采用滤波算法对所述第k时刻的初始状态变量进行修正，得到第k时刻的目标状态变量。
在本发明一个优选实施例中，所述状态变量还包含所述移动终端的时钟误差。
在本发明一个优选实施例中，所述获取移动终端的初始坐标位置的步骤包括：
采集所述移动终端到各所述AP的往返时间RTT值、以及各所述AP的坐标位置；
根据所述移动终端到各所述AP的往返时间RTT值，获取所述移动终端到各所述AP的初始距离；
根据所述移动终端到各所述AP的初始距离、以及各所述AP的坐标位置，获取所述移动终端的初始坐标位置。
在本发明一个优选实施例中，所述从第k-1时刻到第k时刻的状态方程如下：
Xk＝Φk,k-1Xk-1+Wk-1 (2)
Figure BDA0002531623970000031
其中，Xk为第k时刻的状态变量，Xk-1为第k-1时刻的状态变量，Wk-1为第k-1时刻的状态噪声矩阵，Wk-1的方差为Qk-1，Qk-1预先设置，Ts为第k-1时刻到第k时刻的时间间隔。
在本发明一个优选实施例中，所述滤波算法为卡尔曼滤波、无迹卡尔曼滤波和粒子滤波算法中的任何一种。
在本发明一个优选实施例中，当所述滤波算法为卡尔曼滤波算法时，所述根据所述第k时刻的第一观测变量、第k-1时刻状态变量和所述状态方程，估计得到第k时刻的初始状态变量的步骤，包括：
建立如下第一观测方程：
Figure BDA0002531623970000032
Figure BDA0002531623970000041
在式(5)中，
Figure BDA0002531623970000042
为第一观测变量，
Figure BDA0002531623970000043
为第一观测变量的噪声矩阵，
Figure BDA0002531623970000044
的协方差阵为
Figure BDA0002531623970000045
预先设置，
Figure BDA0002531623970000046
为第一观测矩阵，由于第一观测矩阵
Figure BDA0002531623970000047
是非线性的，则根据式(7)计算
Figure BDA0002531623970000048
的雅可比矩阵
Figure BDA0002531623970000049
Figure BDA00025316239700000410
在式(6)和(7)中，x,y,z为第k时刻移动终端的三维坐标位置，xi,yi,zi为第k时刻第i个AP的三维坐标位置，ε为移动终端的时钟误差；
根据如下式(8)所示的状态预测方程，预测第k时刻的预测状态变量
Figure BDA00025316239700000411
Figure BDA00025316239700000412
根据如下式(9)所示的方差预测方程进行方差预测，得到第k时刻的预测方差Pk,k-1：
Figure BDA00025316239700000413
其中，Pk-1为第k-1时刻的方差，第0时刻的方差P0预先设置；
根据如下增益矩阵方程(10)和观测更新方程(11)对预测状态变量
Figure BDA00025316239700000414
进行修正，得到第k时刻的初始状态变量
Figure BDA00025316239700000415
Figure BDA00025316239700000416
Figure BDA00025316239700000417
根据如下方差更新公式(12)更新方差，得到Pk (1)：
Figure BDA0002531623970000051
其中，I为单位矩阵。
在本发明一个优选实施例中，所述根据所述第k时刻的第二观测变量，对所述第k时刻的初始状态变量进行修正，得到第k时刻的目标状态变量的步骤包括：
建立如下第二观测方程：
Figure BDA0002531623970000052
Figure BDA0002531623970000053
其中，
Figure BDA0002531623970000054
为第二观测变量，
Figure BDA0002531623970000055
为第二观测变量的噪声矩阵，
Figure BDA0002531623970000056
的协方差阵为
Figure BDA0002531623970000057
预先设置；
根据如下增益矩阵方程(16)和观测更新方程(17)对第k时刻的初始状态变量
Figure BDA0002531623970000058
进行修正，得到第k时刻的目标状态变量
Figure BDA0002531623970000059
Figure BDA00025316239700000510
Figure BDA00025316239700000511
根据如下方差更新公式(18)更新方差：
Pk＝[I-Kk (2)Hk (2)]Pk (1) (18)
其中，I为单位矩阵列。
在本发明一个优选实施例中，所述根据所述第k时刻的第三观测变量，对所述第k时刻的初始状态变量进行修正，得到第k时刻的目标状态变量的步骤，包括：
建立如下第三观测方程：
Figure BDA0002531623970000061
Figure BDA0002531623970000062
其中，
Figure BDA0002531623970000063
为第三观测变量，
Figure BDA0002531623970000064
为第三观测变量的噪声矩阵，
Figure BDA0002531623970000065
的协方差阵为
Figure BDA0002531623970000066
预先设置；
根据如下增益矩阵方程(25)和观测更新方程(26)对第k时刻的初始状态变量
Figure BDA0002531623970000067
进行修正，得到第k时刻的目标状态变量
Figure BDA0002531623970000068
Figure BDA0002531623970000069
Figure BDA00025316239700000610
根据如下方差更新公式(27)更新方差：
Pk＝[I-Kk (3)Hk (3)]Pk (1) (27)
其中，I为单位矩阵列。
为了实现上述目的，本发明还提供一种基于Wifi和传感器的融合定位系统，包括：
初始位置获取模块，用于获取移动终端的初始坐标位置；
状态变量构建模块，用于构建包含坐标位置和速度的状态变量，其中所述坐标位置的初值赋为所述初始坐标位置，所述速度的初值赋为0；
状态方程建立模块，用于建立从第k-1时刻到第k时刻的状态方程，其中k>1；
第一观测变量获取模块，用于根据Wifi RTT定位算法，获取第k时刻所述移动终端与预设的多个AP之间的距离并将所述距离作为第k时刻的第一观测变量；
初始状态变量获取模块，用于根据所述第k时刻的第一观测变量、第k-1时刻的状态变量和所述状态方程，估计得到第k时刻的初始状态变量；
运动模式识别模块，用于根据所述移动终端内的传感器采集的传感器数据，识别第k时刻所述移动终端所处的运动模式；
第二观测变量获取模块，用于在所述运动模式为静止模式时，将零速度作为第k时刻的第二观测变量；
第一目标状态变量获取模块，用于根据所述第k时刻的第二观测变量，对所述第k时刻的初始状态变量进行修正，得到第k时刻的目标状态变量；
第三观测变量获取模块，用于在所述运动模式为步行模式时，基于第k-1时刻所述移动终端的坐标位置、以及所述传感器数据进行PDR计算，得到第k时刻所述移动终端的PDR位置并将所述PDR位置作为第k时刻的第三观测变量；
第二目标状态变量获取模块，用于根据所述第k时刻的第三观测变量，对所述第k时刻的初始状态变量进行修正，得到第k时刻的目标状态变量。
在本发明一个优选实施例中，所述状态变量还包含所述移动终端的时钟误差。
在本发明一个优选实施例中，所述初始位置获取模块具体用于：
采集所述移动终端到各所述AP的往返时间RTT值、以及各所述AP的坐标位置；
根据所述移动终端到各所述AP的往返时间RTT值，获取所述移动终端到各所述AP的初始距离；
根据所述移动终端到各所述AP的初始距离、以及各所述AP的坐标位置，获取所述移动终端的初始坐标位置。
在本发明一个优选实施例中，所述状态方程建立模块建立的从第k-1时刻到第k时刻的状态方程如下：
Xk＝Φk,k-1Xk-1+Wk-1 (2)
Figure BDA0002531623970000071
其中，Xk为第k时刻的状态变量，Xk-1为第k-1时刻的状态变量，Wk-1为第k-1时刻的状态噪声矩阵，Wk-1的方差为Qk-1，Qk-1预先设置，Ts为第k-1时刻到第k时刻的时间间隔。
在本发明一个优选实施例中，所述滤波算法为卡尔曼滤波、无迹卡尔曼滤波和粒子滤波算法中的任何一种。
在本发明一个优选实施例中，当所述滤波算法为卡尔曼滤波算法时，所述初始状态变量获取模块包括：
第一观测方程建立单元，用于建立如下第一观测方程：
Figure BDA0002531623970000081
Figure BDA0002531623970000082
在式(5)中，
Figure BDA0002531623970000083
为第一观测变量，
Figure BDA0002531623970000084
为第一观测变量的噪声矩阵，
Figure BDA0002531623970000085
的协方差阵为
Figure BDA0002531623970000086
预先设置，
Figure BDA0002531623970000087
为第一观测矩阵，由于第一观测矩阵
Figure BDA0002531623970000088
是非线性的，则根据式(7)计算
Figure BDA0002531623970000089
的雅可比矩阵
Figure BDA00025316239700000810
Figure BDA00025316239700000811
在式(6)和(7)中，x,y,z为第k时刻移动终端的三维坐标位置，xi,yi,zi为第k时刻第i个AP的三维坐标位置，ε为移动终端的时钟误差；
状态预测单元，用于根据如下式(8)所示的状态预测方程，预测第k时刻的预测状态变量
Figure BDA00025316239700000812
Figure BDA00025316239700000813
方差预测单元，用于根据如下式(9)所示的方差预测方程进行方差预测，得到第k时刻的预测方差Pk,k-1：
Figure BDA0002531623970000091
其中，Pk-1为第k时刻的方差，第0时刻的方差P0预先设置；
第一修正单元，用于根据如下增益矩阵方程(10)和观测更新方程(11)对预测状态变量
Figure BDA0002531623970000092
进行修正，得到第k时刻的初始状态变量
Figure BDA0002531623970000093
Figure BDA0002531623970000094
Figure BDA0002531623970000095
第一方差更新单元，用于根据如下方差更新公式(12)更新方差，得到Pk (1)：
Figure BDA0002531623970000096
其中，I为单位矩阵。
在本发明一个优选实施例中，所述第一目标状态变量获取模块包括：
第二观测方程建立单元，用于建立如下第二观测方程：
Figure BDA0002531623970000097
Figure BDA0002531623970000098
其中，
Figure BDA0002531623970000099
为第二观测变量，
Figure BDA00025316239700000910
为第二观测变量的噪声矩阵，
Figure BDA00025316239700000911
的协方差阵为
Figure BDA00025316239700000912
预先设置；
第二修正单元，用于根据如下增益矩阵方程(16)和观测更新方程(17)对第k时刻的初始状态变量
Figure BDA00025316239700000913
进行修正，得到第k时刻的目标状态变量
Figure BDA00025316239700000914
Figure BDA00025316239700000915
Figure BDA0002531623970000101
第二方差更新单元，用于根据如下方差更新公式(18)更新方差：
Pk＝[I-Kk (2)Hk (2)]Pk (1) (18)
其中，I为单位矩阵列。
在本发明一个优选实施例中，所述第二目标状态变量获取模块包括：
第三观测方程建立单元，用于建立如下第三观测方程：
Figure BDA0002531623970000102
Figure BDA0002531623970000103
其中，
Figure BDA0002531623970000104
为第三观测变量，
Figure BDA0002531623970000105
为第三观测变量的噪声矩阵，
Figure BDA0002531623970000106
的协方差阵为
Figure BDA0002531623970000107
预先设置；
第三修正单元，用于根据如下增益矩阵方程(25)和观测更新方程(26)对第k时刻的初始状态变量
Figure BDA0002531623970000108
进行修正，得到第k时刻的目标状态变量
Figure BDA0002531623970000109
Figure BDA00025316239700001010
Figure BDA00025316239700001011
第三方差更新单元，用于根据如下方差更新公式(27)更新方差：
Pk＝[I-Kk (3)Hk (3)]Pk (1) (27)
其中，I为单位矩阵列。
为了实现上述目的，本发明还提供一种电子设备，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现前述方法的步骤。
为了实现上述目的，本发明还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现前述方法的步骤。
通过采用上述技术方案，本发明具有如下有益效果：
本发明首先获取移动终端的初始坐标位置，并构建包含坐标位置和速度的状态变量，而后利用Wifi RTT定位算法测得的第一观测变量估计第k时刻的初始状态变量，同时利用传感器数据对移动终端所处的运动模式进行识别，并在移动终端处于静止模式时，将零速度作为第二观测变量对k时刻的初始状态变量进行修正，在移动终端处于步行模式时，获取移动终端的PDR位置作为第三观测变量对k时刻的初始状态变量进行修正，从而得到第k时刻的目标状态变量，由于目标状态变量中包含移动终端的位置，所以可以准确定位出移动终端在k时刻的位置。与现有技术相比，本发明充分利用了传感器数据，可识别出移动终端的运动模式，并根据移动终端的不同运动模式获取不同观测变量与wifi RTT估计结果进行融合，从而可以在Wifi有遮挡、多路径的情况下也能提供连续稳定的融合定位准确结果。
附图说明
图1为本发明实施例1中基于Wifi和传感器的融合定位方法的流程图；
图2为本发明实施例2中基于Wifi和传感器的融合定位系统的结构框图；
图3为本发明实施例2中初始状态变量获取模块的结构框图；
图4为本发明实施例2中第一目标状态变量获取模块的结构框图；
图5为本发明实施例2中第二目标状态变量获取模块的结构框图；
图6为本发明实施例3中电子设备的硬件架构图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
在本发明使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本公开。在本公开和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。
实施例1
本实施例提供一种基于Wifi和传感器的融合定位方法，如图1所示，包括以下步骤：
S1，获取第0时刻移动终端的初始坐标位置，具体过程如下：
首先，采集移动终端MT到N个AP的往返时间RTT值(包含T1i,T2i,T3i,T4i)、以及各AP的三维坐标位置(xi,yi,zi)，其中i＝1,2,…,N，N>＝4，T1i表示MT向第i个AP发送请求信息的时间戳，T2i表示第i个AP收到MT发送的请求信息的时间戳，T3i表示第i个AP向MT发送反馈信息的时间戳，T4i表示MT收到第i个AP发送的反馈信息的时间戳。
而后，根据移动终端到各AP的往返时间RTT值，获取移动终端到各AP的初始距离di＝[(T4i-T3i)+(T2i–T1i)]/2*C，其中，C为光速，即C＝3*108m/s，i＝1,2,…,N。
最后，根据第0时刻移动终端到各AP的初始距离、以及各AP的坐标位置，获取第0时刻移动终端的初始坐标位置。应该理解，移动终端到第i个AP的距离di与移动终端的三维坐标位置(x,y,z)以及第i个AP的三维坐标位置(xi,yi,zi)之间具有如下关系式：
Figure BDA0002531623970000121
因而，可通过最小二乘法(也可采用牛顿迭代等方法)解出上述方程组，得到MT在第0时刻的初始坐标位置[x0 y0 Z0]。
S2，构建包含坐标位置、速度和时钟误差的状态变量Xk＝[x y z Vx Vy Vz ε]T，其中，x、y、z为MT在三维直角参考坐标系(可以选东北天坐标系)下的坐标位置，Vx、Vy、Vz为MT在前述参考坐标系下的速度，ε为MT的时钟误差。在本实施例中，将第0时刻状态变量中[x yz]的初值赋为MT的初始坐标位置[x0 y0 Z0]，状态变量中的Vx、Vy、Vz、ε的初值均赋为0。
S3，建立从第k-1时刻到第k时刻的状态方程，其中k>1。在本实施例中，建立的状态方程如下：
Xk＝Φk,k-1Xk-1+Wk-1 (2)
Figure BDA0002531623970000131
其中，Xk为第k时刻的状态变量，Xk-1为第k-1时刻的状态变量，Wk-1为第k-1时刻的状态噪声矩阵，其方差为Qk-1，Qk-1根据经验设置，可以是定值，也可以是变化的，根据实际情况调整；Ts为第k-1时刻到第k时刻的时间间隔。
S4，根据Wifi RTT定位算法，获取第k时刻所述移动终端与预设的各AP之间的距离di并将所述距离di作为第k时刻的第一观测变量
Figure BDA0002531623970000132
即：
Figure BDA0002531623970000133
其中，本步骤获取di的过程与步骤S1类似，在此不再赘述。
S5，根据所述第k时刻的第一观测变量
Figure BDA0002531623970000134
第k-1时刻的状态变量Xk-1和所述状态方程，采用KF(kalman filter,卡尔曼滤波)算法估计得到第k时刻的初始状态变量，具体通过如下步骤实现：
S51，建立如下第一观测方程
Figure BDA0002531623970000135
Figure BDA0002531623970000136
Figure BDA0002531623970000137
其中，
Figure BDA0002531623970000141
为第一观测变量的噪声矩阵，
Figure BDA0002531623970000142
的协方差阵为
Figure BDA0002531623970000143
可根据经验设置，
Figure BDA0002531623970000144
为第一观测矩阵，由于第一观测矩阵
Figure BDA0002531623970000145
是非线性的，则需根据式(7)计算
Figure BDA0002531623970000146
的雅可比矩阵
Figure BDA0002531623970000147
Figure BDA0002531623970000148
在式(6)和(7)中，x,y,z为第k时刻移动终端的三维坐标位置，xi,yi,zi为第k时刻第i个无线局域网接入点的三维坐标位置。
S52，根据如下式(8)所示的状态预测方程，预测第k时刻的预测状态变量
Figure BDA0002531623970000149
Figure BDA00025316239700001410
其中，Φk,k-1根据公式(3)得到。
具体地，本步骤可以采用运动模型(如匀速运动模型、匀加速运动模型等)对状态变量中的坐标位置和速度进行预测，采用固定时钟误差模型、一阶马尔可夫模型等对状态变量中的时钟误差进行预测，本步骤并不对采用的具体模型作任何限制。
S53，根据如下式(9)所示的方差预测方程进行方差预测，得到第k时刻的预测方差Pk,k-1：
Figure BDA00025316239700001411
其中，Φk,k-1根据公式(3)得到，Qk-1根据经验设置，Pk为第k-1时刻的方差，第0时刻的方差P0根据经验设置。
S54，根据如下增益矩阵方程(10)和观测更新方程(11)对预测状态变量
Figure BDA00025316239700001412
进行修正，得到第k时刻的初始状态变量
Figure BDA00025316239700001413
Figure BDA00025316239700001414
Figure BDA0002531623970000151
其中，
Figure BDA0002531623970000152
根据公式(4)得到，
Figure BDA0002531623970000153
由公式(7)得到。
S55，根据如下方差更新公式(12)更新方差：
Figure BDA0002531623970000154
S6，根据所述移动终端内的传感器采集的传感器数据，识别第k时刻所述移动终端所处的运动模式。
具体地，传感器可以包括惯性传感器和气压计，惯性传感器可以包括加速度计(Accelerometer)、陀螺仪(Gyroscope)和磁力计(Magnetometer)，其中加速度计用于测量MT在载体坐标系三个坐标轴方向的加速度，陀螺仪又称角速度传感器，通过测量MT旋转时产生的科氏加速度来获得横滚、俯仰、航向的转动角速度，磁力计用于测量MT三轴的磁场感应情况，进而确定MT与东西南北四个方向的夹角，其与陀螺仪结合使用能够确定MT准确的立体运动方向，气压计用于测量气压。
在本实施例中，可以采用预设的识别规则或者机器学习(如神经网络)等方式对第k时刻所述移动终端所处的运动模式进行识别。根据传感器数据进行运动模式识别在行业内为较成熟的方案，在此仅举例说明一种简单的基于规则识别的运动模式识别方法：
选取合适的滑动时间窗口(比如1秒)，在时间窗口内按照预定频率(如50HZ)采集加速度计和陀螺仪的三轴数据，并计算加速度计三轴数据模值的标准差accNormStd以及陀螺仪三轴数据模值的标准差gyroNormStd。如果在时间窗口内满足以下条件：accNormStd<＝ACC_STD_TH1&&gyroNormStd<＝GYRO_STD_TH1，则判定MT为静态模式，其中ACC_STD_TH1、GYRO_STD_TH1根据经验设置。
选取M(M)个连续的时间窗口，判断M个连续时间窗口内采集的加速度计三轴数据是否满足以下条件，若满足则判定MT为步行模式：
(1)在每个时间窗口内，ACC_STD_TH1<accNormStd<ACC_STD_TH2；
(2)计算每个时间窗口内加速度计三轴数据的模值的最大值和最小值，并确定最大值和最小值出现的时间，比如：计算第1个时间窗口内加速度计三轴数据的模值accNorm的最大值accNormMax1和最小值accNormMin1，两者对应的时间点分别为tMax1和tMin1；计算第2个时间窗口内加速度计三轴数据的模值的最大值accNormMax2和最小值accNormMin2，两者对应的时间点分别为tMax2和tMin2，其中ACC_TH3<＝accNormMax i<＝ACC_TH4，ACC_TH5<＝accNormMin i<＝ACC_TH6(i＝1,2,…,M)，并且M个连续时间窗口内accNorm和最大值和最小值是交替出现的，比如可以是tMax1<tMin1<tMax2<tMin2，也可以是tMin1<tMax1<tMin2<tMax2，同时相邻最大值和最小值出现的时间间隔满足：ACC_DT_TH1<＝|tMin1-tMax1|<＝ACC_DT_TH2，ACC_DT_TH1<＝|tMax2-tMin1|<＝ACC_DT_TH2(对于tMax1<tMin1<tMax2<tMin2这种情况而言)，ACC_DT_TH1<＝|tMax2-tMin2|<＝ACC_DT_TH2。
需说明的是，上述阈值均可根据经验设置。当判定传感器数据即不满足静止模式的条件，也不满足步行模式的条件时，判定MT为其它模式，则第k时刻的计算流程结束，根据初始状态变量
Figure BDA0002531623970000161
获取MT的第k时刻定位结果。
S7，当所述运动模式为静止模式时，将零速度作为第k时刻的第二观测变量
Figure BDA0002531623970000162
即，令：
Figure BDA0002531623970000163
其中，零速度表示MT的三维速度为[0 0 0]。
S8，根据所述第k时刻的第二观测变量，采用KF算法对所述第k时刻的初始状态变量进行修正，得到第k时刻的目标状态变量，具体过程如下：
S81，建立如下第二观测方程：
Figure BDA0002531623970000164
Figure BDA0002531623970000165
其中，
Figure BDA0002531623970000171
为第二观测变量的噪声矩阵，其协方差阵为
Figure BDA0002531623970000172
可根据经验设置。
S82，根据如下增益矩阵方程(16)和观测更新方程(17)对第k时刻的初始状态变量
Figure BDA0002531623970000173
进行修正，得到第k时刻的目标状态变量
Figure BDA0002531623970000174
Figure BDA0002531623970000175
Figure BDA0002531623970000176
从
Figure BDA0002531623970000177
中即可直接读取移动终端第k时刻的坐标位置、速度和时钟误差，同时将
Figure BDA0002531623970000178
作为Xk，以用于第k+1时刻的计算过程。
S83，根据如下方差更新公式(18)更新方差，得到第k时刻的方差Pk，以用于第k+1时刻的计算过程：
Pk＝[I-Kk (2)Hk (2)]Pk (1) (18)
S9，当所述运动模式为步行模式时，基于第k-1时刻所述移动终端的坐标位置、以及所述传感器数据进行PDR计算，得到第k时刻所述移动终端的PDR位置并将所述PDR位置作为第k时刻的第三观测变量。
在本实施例中，可以通过如下公式(19)～(21)进行PDR计算：
xpdr,k＝xpdr,k-1+stepLen*sinα (19)
ypdr,k＝ypdr,k-1+stepLen*cosα (20)
zpdr,k＝zpdr,k-1+Δh (21)
其中，stepLen是通过加速度计数据计算得到的步长，α是通过加速度计/陀螺仪/气压计计算得到的航向，Δh是通过气压计计算得到的高度变化(关于stepLen、α、Δh的计算方法为本领域已有技术，在此不做详细说明)。本步骤以步骤S1得到的MT初始位置[x0 y0Z0]为初始位置进行PDR递推计算，从而得到第k时刻的PDR位置[xpdr,k ypdr,k Zpdr,k]作为第k时刻的第三观测变量
Figure BDA0002531623970000179
即有：
Figure BDA0002531623970000181
S10，根据所述第k时刻的第三观测变量，采用KF算法对所述第k时刻的初始状态变量进行修正，得到第k时刻的目标状态变量，具体过程如下：
S101，建立如下第三观测方程
Figure BDA0002531623970000182
Figure BDA0002531623970000183
Figure BDA0002531623970000184
其中，
Figure BDA0002531623970000185
为第三观测变量的噪声矩阵，其协方差阵为
Figure BDA0002531623970000186
可根据经验设置。
S102，根据如下增益矩阵方程(25)和观测更新方程(26)对第k时刻的初始状态变量
Figure BDA0002531623970000187
进行修正，得到第k时刻的目标状态变量
Figure BDA0002531623970000188
Figure BDA0002531623970000189
Figure BDA00025316239700001810
从
Figure BDA00025316239700001811
中即可直接读取移动终端第k时刻的坐标位置、速度和时钟误差，同时将
Figure BDA00025316239700001812
作为Xk，以用于第k+1时刻的计算过程。
S103，根据如下方差更新公式(27)更新方差，得到第k时刻的最终方差Pk，以用于第k+1时刻的计算过程：
Pk＝[I-Kk (3)Hk (3)]Pk (1) (27)
可见，本实施例首先获取移动终端的初始坐标位置，并构建包含坐标位置、速度和时钟误差的状态变量，而后利用Wifi RTT定位算法测得的第一观测变量估计第k时刻的初始状态变量，同时利用传感器数据对移动终端所处的运动模式进行识别，当移动终端处于静止模式时，将零速度作为第二观测变量对k时刻的初始状态变量进行修正，当移动终端处于步行模式时，获取移动终端的PDR位置作为第三观测变量对k时刻的初始状态变量进行修正，从而得到k时刻的目标状态变量，由于目标状态变量中包含移动终端的位置，所以可以准确定位出移动终端在k时刻的位置。与现有技术相比，本发明充分利用了传感器数据，可识别出移动终端的运动模式，并根据移动终端的不同运动模式获取不同观测变量与wifiRTT估计结果进行融合，从而可以在Wifi有遮挡、多路径的情况下也能提供连续稳定的融合定位准确结果。
此外，由于本实施例的状态变量包含时钟误差，因而通过对状态变量的修正，还可以实现时钟误差的在线校准。
需要说明的是，本实施例中采用的KF(kalman filter,卡尔曼滤波)算法也可采用其他滤波算法如UKF(无迹卡尔曼滤波)、PF(粒子滤波)等进行替换。
另外，也可以将前述三维坐标位置、速度降为二维，比如只考虑水平位置速度而不考虑高度方向的位置速度，在此情况下，前述N的取值范围为N>＝3，且气压计可以省略。
对于本实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本实施例并不受所描述的动作顺序的限制，因为依据本实施例，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作并不一定是本实施例所必须的。
实施例2
本实施例提供一种基于Wifi和传感器的融合定位系统10，如图2所示，包括初始位置获取模块101、状态变量构建模块102、状态方程建立模块103、第一观测变量获取模块104、初始状态变量获取模块105、运动模式识别模块106、第二观测变量获取模块107、第一目标状态变量获取模块108、第三观测变量获取模块109、第二目标状态变量获取模块110，下面分别对第个模块进行详细介绍。
初始位置获取模块101用于获取第0时刻移动终端的初始坐标位置，具体过程如下：
首先，采集移动终端MT到N个AP的往返时间RTT值(包含T1i,T2i,T3i,T4i)、以及各AP的三维坐标位置(xi,yi,zi)，其中i＝1,2,…,N，N>＝4，T1i表示MT向第i个AP发送请求信息的时间戳，T2i表示第i个AP收到MT发送的请求信息的时间戳，T3i表示第i个AP向MT发送反馈信息的时间戳，T4i表示MT收到第i个AP发送的反馈信息的时间戳。
而后，根据移动终端到各AP的往返时间RTT值，获取移动终端到各AP的初始距离di＝[(T4i-T3i)+(T2i–T1i)]/2*C，其中，C为光速，即C＝3*108m/s，i＝1,2,…,N。
最后，根据第0时刻移动终端到各AP的初始距离、以及各AP的坐标位置，获取第0时刻移动终端的初始坐标位置。应该理解，移动终端到第i个AP的距离di与移动终端的三维坐标位置(x,y,z)以及第i个AP的三维坐标位置(xi,yi,zi)之间具有如下关系式：
Figure BDA0002531623970000201
因而，可通过最小二乘法(也可采用牛顿迭代等方法)解出上述方程组，得到MT在第0时刻的初始坐标位置[x0 y0 Z0]。
状态变量构建模块102用于构建包含坐标位置、速度和时钟误差的状态变量Xk＝[x y z Vx Vy Vz ε]T，其中，x、y、z为MT在三维直角参考坐标系(可以选东北天坐标系)下的坐标位置，Vx、Vy、Vz为MT在前述参考坐标系下的速度，ε为MT的时钟误差。在本实施例中，将第0时刻状态变量中[x y z]的初值赋为MT的初始坐标位置[x0 y0 Z0]，状态变量中的Vx、Vy、Vz、ε的初值均赋为0。
状态方程建立模块103用于建立从第k-1时刻到第k时刻的状态方程，其中k>1。在本实施例中，建立的状态方程如下：
Xk＝Φk,k-1Xk-1+Wk-1 (2)
Figure BDA0002531623970000202
其中，Xk为第k时刻的状态变量，Xk-1为第k-1时刻的状态变量，Wk-1为第k-1时刻的状态噪声矩阵，其方差为Qk-1，Qk-1根据经验设置，可以是定值，也可以是变化的，根据实际情况调整；Ts为第k-1时刻到第k时刻的时间间隔。
第一观测变量获取模块104用于根据Wifi RTT定位算法，获取第k时刻所述移动终端与预设的各AP之间的距离di并将所述距离di作为第k时刻的第一观测变量
Figure BDA0002531623970000211
即：
Figure BDA0002531623970000212
其中，本模块获取di的过程与初始位置获取模块101中的过程类似，在此不再赘述。
初始状态变量获取模块105用于根据所述第k时刻的第一观测变量
Figure BDA0002531623970000213
所述第k-1时刻的状态变量和所述状态方程，采用KF(kalman filter,卡尔曼滤波)算法估计得到第k时刻的初始状态变量，如图3所示，具体包括：
第一观测方程建立单元1051，用于建立如下第一观测方程
Figure BDA0002531623970000214
Figure BDA0002531623970000215
Figure BDA0002531623970000216
其中，
Figure BDA0002531623970000217
为第一观测矩阵，由于第一观测矩阵
Figure BDA0002531623970000218
是非线性的，需要计算
Figure BDA0002531623970000219
的偏导数得到其雅可比矩阵
Figure BDA00025316239700002110
为第一观测变量的噪声矩阵，
Figure BDA00025316239700002111
的协方差阵为
Figure BDA00025316239700002112
可根据经验设置，其中，
Figure BDA00025316239700002113
Figure BDA0002531623970000221
在式(6)和(7)中，x,y,z为第k时刻移动终端的三维坐标位置，xi,yi,zi为第k时刻第i个无线局域网接入点的三维坐标位置。
预测单元1052，用于根据如下式(8)所示的状态预测方程，预测第k时刻的预测状态变量
Figure BDA0002531623970000222
Figure BDA0002531623970000223
其中，Φk,k-1根据公式(3)得到。
具体地，本模块可以采用运动模型(如匀速运动模型、匀加速运动模型等)对状态变量中的坐标位置和速度进行预测，采用固定时钟误差模型、一阶马尔可夫模型等对状态变量中的时钟误差进行预测，本模块并不对采用的具体模型作任何限制。
方差预测单元1053，用于根据如下式(9)所示的方差预测方程进行方差预测，得到第k时刻的预测方差Pk,k-1：
Figure BDA0002531623970000224
其中，Φk,k-1根据公式(3)得到，Qk-1根据经验设置，Pk为第k时刻的方差，第0时刻的方差P0根据经验设置。
第一修正单元1054，用于根据如下增益矩阵方程(10)和观测更新方程(11)对预测状态变量
Figure BDA0002531623970000225
进行修正，得到第k时刻的初始状态变量
Figure BDA0002531623970000226
Figure BDA0002531623970000227
Figure BDA0002531623970000228
其中，Zk (1)根据公式(4)得到，
Figure BDA0002531623970000229
由公式(7)得到。
第一方差更新单元1055，用于根据如下方差更新公式(12)更新方差：
Figure BDA00025316239700002210
运动模式识别模块106用于根据所述移动终端内的传感器采集的传感器数据，识别第k时刻所述移动终端所处的运动模式。
具体地，传感器可以包括惯性传感器和气压计，惯性传感器可以包括加速度计(Accelerometer)、陀螺仪(Gyroscope)和磁力计(Magnetometer)，其中加速度计用于测量MT在载体坐标系三个坐标轴方向的加速度，陀螺仪又称角速度传感器，通过测量MT旋转时产生的科氏加速度来获得横滚、俯仰、航向的转动角速度，磁力计用于测量MT三轴的磁场感应情况，进而确定MT与东西南北四个方向的夹角，其与陀螺仪结合使用能够确定MT准确的立体运动方向，气压计用于测量气压。
在本实施例中，可以采用预设的识别规则或者机器学习(如神经网络)等方式对第k时刻所述移动终端所处的运动模式进行识别。根据传感器数据进行运动模式识别在行业内为较成熟的方案，在此仅举例说明一种简单的基于规则识别的运动模式识别方法：
选取合适的滑动时间窗口(比如1秒)，在时间窗口内按照预定频率(如50HZ)采集加速度计和陀螺仪的三轴数据，并计算加速度计三轴数据模值的标准差accNormStd以及陀螺仪三轴数据模值的标准差gyroNormStd。如果在时间窗口内满足以下条件：accNormStd<＝ACC_STD_TH1&&gyroNormStd<＝GYRO_STD_TH1，则判定MT为静态模式，其中ACC_STD_TH1、GYRO_STD_TH1根据经验设置。
选取M(M)个连续的时间窗口，判断M个连续时间窗口内采集的加速度计三轴数据是否满足以下条件，若满足则判定MT为步行模式：
(1)在每个时间窗口内，ACC_STD_TH1<accNormStd<ACC_STD_TH2；
(2)计算每个时间窗口内加速度计三轴数据的模值的最大值和最小值，并确定最大值和最小值出现的时间，比如：计算第1个时间窗口内加速度计三轴数据的模值accNorm的最大值accNormMax1和最小值accNormMin1，两者对应的时间点分别为tMax1和tMin1；计算第2个时间窗口内加速度计三轴数据的模值的最大值accNormMax2和最小值accNormMin2，两者对应的时间点分别为tMax2和tMin2，其中ACC_TH3<＝accNormMax i<＝ACC_TH4，ACC_TH5<＝accNormMin i<＝ACC_TH6(i＝1,2,…,M)，并且M个连续时间窗口内accNorm和最大值和最小值是交替出现的，比如可以是tMax1<tMin1<tMax2<tMin2，也可以是tMin1<tMax1<tMin2<tMax2，同时相邻最大值和最小值出现的时间间隔满足：ACC_DT_TH1<＝|tMin1-tMax1|<＝ACC_DT_TH2，ACC_DT_TH1<＝|tMax2-tMin1|<＝ACC_DT_TH2(对于tMax1<tMin1<tMax2<tMin2这种情况而言)，ACC_DT_TH1<＝|tMax2-tMin2|<＝ACC_DT_TH2。
需说明的是，上述阈值均可根据经验设置。当判定传感器数据即不满足静止模式的条件，也不满足步行模式的条件时，判定MT为其它模式，则第k时刻的计算流程结束，根据初始状态变量
Figure BDA0002531623970000241
获取MT的第k时刻定位结果。
第二观测变量获取模块107用于在所述运动模式为静止模式时，将零速度作为第k时刻的第二观测变量
Figure BDA0002531623970000242
即，令：
Figure BDA0002531623970000243
其中，零速度表示MT的三维速度为[0 0 0]。
第一目标状态变量获取模块108用于根据所述第k时刻的第二观测变量，采用KF算法对所述第k时刻的初始状态变量进行修正，得到第k时刻的目标状态变量，如图4所示，具体包括：
第二观测方程建立单元1081，用于建立如下第二观测方程：
Figure BDA0002531623970000244
Figure BDA0002531623970000245
其中，
Figure BDA0002531623970000246
为第二观测变量的噪声矩阵，其协方差阵为
Figure BDA0002531623970000247
可根据经验设置。
第二修正单元1082，用于根据如下增益矩阵方程(16)和观测更新方程(17)对第k时刻的初始状态变量
Figure BDA0002531623970000248
进行修正，得到第k时刻的目标状态变量
Figure BDA0002531623970000251
Figure BDA0002531623970000252
Figure BDA0002531623970000253
从
Figure BDA0002531623970000254
中即可直接读取移动终端第k时刻的坐标位置、速度和时钟误差，同时将
Figure BDA0002531623970000255
作为Xk，以用于第k+1时刻的计算过程。
第二修正单元1083，用于根据如下方差更新公式(18)更新方差，得到第k时刻的方差Pk，以用于第k+1时刻的计算过程：
Pk＝[I-Kk (2)Hk (2)]Pk (1) (18)
第三观测变量获取模块109用于在所述运动模式为步行模式时，基于第k-1时刻所述移动终端的坐标位置、以及所述传感器数据进行PDR计算，得到第k时刻所述移动终端的PDR位置并将所述PDR位置作为第k时刻的第三观测变量。
在本实施例中，可以通过如下公式(19)～(21)进行PDR计算：
xpdr,k＝xpdr,k-1+stepLen*sinα (19)
ypdr,k＝ypdr,k-1+stepLen*cosα (20)
zpdr,k＝zpdr,k-1+Δh (21)
其中，stepLen是通过加速度计数据计算得到的步长，α是通过加速度计/陀螺仪/气压计计算得到的航向，Δh是通过气压计计算得到的高度变化(关于stepLen、α、Δh的计算方法为本领域已有技术，在此不做详细说明)。本模块以初始位置获取模块101得到的MT初始位置[x0 y0 Z0]为初始位置进行PDR递推计算，从而得到第k时刻的PDR位置[xpdr,kypdr,k Zpdr,k]作为第k时刻的第三观测变量
Figure BDA0002531623970000256
即有：
Figure BDA0002531623970000257
第二目标状态变量获取模块110用于在根据所述第k时刻的第三观测变量，采用KF算法对所述第k时刻的初始状态变量进行修正，得到第k时刻的目标状态变量，如图5所示，具体包括：
第三观测方程建立单元1101，用于建立如下第三观测方程
Figure BDA0002531623970000261
Figure BDA0002531623970000262
Figure BDA0002531623970000263
其中，
Figure BDA0002531623970000264
为第三观测变量的噪声矩阵，其协方差阵为
Figure BDA0002531623970000265
可根据经验设置。
第三修正单元1102，用于根据如下增益矩阵方程(25)和观测更新方程(26)对第k时刻的初始状态变量
Figure BDA0002531623970000266
进行修正，得到第k时刻的目标状态变量
Figure BDA0002531623970000267
Figure BDA0002531623970000268
Figure BDA0002531623970000269
从
Figure BDA00025316239700002610
中即可直接读取移动终端第k时刻的坐标位置、速度和时钟误差，同时将
Figure BDA00025316239700002611
作为Xk，以用于第k+1时刻的计算过程。
第三方差更新单元1103，用于根据如下方差更新公式(27)更新方差，得到第k时刻的最终方差Pk，以用于第k+1时刻的计算过程：
Pk＝[I-Kk (3)Hk (3)]Pk (1) (27)
可见，本实施例首先获取移动终端的初始坐标位置，并构建包含坐标位置、速度和时钟误差的状态变量，而后利用Wifi RTT定位算法测得的第一观测变量估计第k时刻的初始状态变量，同时利用传感器数据对移动终端所处的运动模式进行识别，当移动终端处于静止模式时，将零速度作为第二观测变量对k时刻的初始状态变量进行修正，当移动终端处于步行模式时，获取移动终端的PDR位置作为第三观测变量对k时刻的初始状态变量进行修正，从而得到k时刻的目标状态变量，由于目标状态变量中包含移动终端的位置，所以可以准确定位出移动终端在k时刻的位置。与现有技术相比，本发明充分利用了传感器数据，可识别出移动终端的运动模式，并根据移动终端的不同运动模式获取不同观测变量与wifiRTT估计结果进行融合，从而可以在Wifi有遮挡、多路径的情况下也能提供连续稳定的融合定位准确结果。
此外，由于本实施例的状态变量包含时钟误差，因而通过对k时刻的第一观测变量进行修正，还可以实现时钟误差的在线校准。
需要说明的是，本实施例中采用的KF(kalman filter,卡尔曼滤波)算法也可采用其他滤波算法如UKF(无迹卡尔曼滤波)、PF(粒子滤波)等进行替换。
另外，也可以将前述三维坐标位置、速度降为二维，比如只考虑水平位置速度而不考虑高度方向的位置速度，在此情况下，前述N的取值范围为N>＝3，且气压计可以省略。
实施例3
本实施例提供一种电子设备，电子设备可以通过计算设备的形式表现(例如可以为服务器设备)，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其中处理器执行计算机程序时可以实现实施例1提供的基于Wifi和传感器的融合定位方法。
图6示出了本实施例的硬件结构示意图，如图6所示，电子设备9具体包括：
至少一个处理器91、至少一个存储器92以及用于连接不同系统组件(包括处理器91和存储器92)的总线93，其中：
总线93包括数据总线、地址总线和控制总线。
存储器92包括易失性存储器，例如随机存取存储器(RAM)921和/或高速缓存存储器922，还可以进一步包括只读存储器(ROM)923。
存储器92还包括具有一组(至少一个)程序模块924的程序/实用工具925，这样的程序模块924包括但不限于：操作系统、一个或者多个应用程序、其它程序模块以及程序数据，这些示例中的每一个或某种组合中可能包括网络环境的实现。
处理器91通过运行存储在存储器92中的计算机程序，从而执行各种功能应用以及数据处理，例如本发明实施例1所提供的基于Wifi和传感器的融合定位方法。
电子设备9进一步可以与一个或多个外部设备94(例如键盘、指向设备等)通信。这种通信可以通过输入/输出(I/O)接口95进行。并且，电子设备9还可以通过网络适配器96与一个或者多个网络(例如局域网(LAN)，广域网(WAN)和/或公共网络，例如因特网)通信。网络适配器96通过总线93与电子设备9的其它模块通信。应当明白，尽管图中未示出，可以结合电子设备9使用其它硬件和/或软件模块，包括但不限于：微代码、设备驱动器、冗余处理器、外部磁盘驱动阵列、RAID(磁盘阵列)系统、磁带驱动器以及数据备份存储系统等。
应当注意，尽管在上文详细描述中提及了电子设备的若干单元/模块或子单元/模块，但是这种划分仅仅是示例性的并非强制性的。实际上，根据本申请的实施方式，上文描述的两个或更多单元/模块的特征和功能可以在一个单元/模块中具体化。反之，上文描述的一个单元/模块的特征和功能可以进一步划分为由多个单元/模块来具体化。
实施例4
本实施例提供了一种计算机可读存储介质，其上存储有计算机程序，所述程序被处理器执行时实现实施例1所提供的基于Wifi和传感器的融合定位方法的步骤。
其中，可读存储介质可以采用的更具体可以包括但不限于：便携式盘、硬盘、随机存取存储器、只读存储器、可擦拭可编程只读存储器、光存储器件、磁存储器件或上述的任意合适的组合。
在可能的实施方式中，本发明还可以实现为一种程序产品的形式，其包括程序代码，当所述程序产品在终端设备上运行时，所述程序代码用于使所述终端设备执行实现实施例1所述的基于Wifi和传感器的融合定位方法的步骤。
其中，可以以一种或多种程序设计语言的任意组合来编写用于执行本发明的程序代码，所述程序代码可以完全地在用户设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户设备上部分在远程设备上执行或完全在远程设备上执行。
虽然以上描述了本发明的具体实施方式，但是本领域的技术人员应当理解，这仅是举例说明，本发明的保护范围是由所附权利要求书限定的。本领域的技术人员在不背离本发明的原理和实质的前提下，可以对这些实施方式做出多种变更或修改，但这些变更和修改均落入本发明的保护范围。

Claims (18)

1.一种基于Wifi和传感器的融合定位方法，其特征在于，包括：
获取移动终端的初始坐标位置；
构建包含坐标位置和速度的状态变量，并给所述状态变量赋初值；
建立从第k-1时刻到第k时刻的状态方程，其中k>1；
根据Wifi RTT定位算法，获取第k时刻所述移动终端与预设的多个AP之间的距离并将所述距离作为第k时刻的第一观测变量；
根据所述第k时刻的第一观测变量、第k-1时刻的状态变量和所述状态方程，采用滤波算法估计得到第k时刻的初始状态变量；
根据所述移动终端内的传感器采集的传感器数据，识别第k时刻所述移动终端所处的运动模式；
当所述运动模式为静止模式时，将零速度作为第k时刻的第二观测变量；
根据所述第k时刻的第二观测变量，采用滤波算法对所述第k时刻的初始状态变量进行修正，得到第k时刻的目标状态变量；
当所述运动模式为步行模式时，基于第k-1时刻所述移动终端的坐标位置、以及所述传感器数据进行PDR计算，得到第k时刻所述移动终端的PDR位置并将所述PDR位置作为第k时刻的第三观测变量；
根据所述第k时刻的第三观测变量，采用滤波算法对所述第k时刻的初始状态变量进行修正，得到第k时刻的目标状态变量。
2.根据权利要求1所述的基于Wifi和传感器的融合定位方法，其特征在于，所述状态变量还包含所述移动终端的时钟误差。
3.根据权利要求1所述的基于Wifi和传感器的融合定位方法，其特征在于，所述获取移动终端的初始坐标位置的步骤包括：
采集所述移动终端到各所述AP的往返时间RTT值、以及各所述AP的坐标位置；
根据所述移动终端到各所述AP的往返时间RTT值，获取所述移动终端到各所述AP的初始距离；
根据所述移动终端到各所述AP的初始距离、以及各所述AP的坐标位置，获取所述移动终端的初始坐标位置。
4.根据权利要求1所述的基于Wifi和传感器的融合定位方法，其特征在于，所述从第k-1时刻到第k时刻的状态方程如下：
Xk＝Φk，k-1Xk-1+Wk-1 (2)
Figure FDA0002531623960000021
其中，Xk为第k时刻的状态变量，Xk-1为第k-1时刻的状态变量，Wk-1为第k-1时刻的状态噪声矩阵，Wk-1的方差为Qk-1，Qk-1预先设置，Ts为第k-1时刻到第k时刻的时间间隔。
5.根据权利要求4所述的基于Wifi和传感器的融合定位方法，其特征在于，所述滤波算法为卡尔曼滤波、无迹卡尔曼滤波和粒子滤波算法中的任何一种。
6.根据权利要求5所述的基于Wifi和传感器的融合定位方法，其特征在于，当所述滤波算法为卡尔曼滤波算法时，所述根据所述第k时刻的第一观测变量、第k-1时刻状态变量和所述状态方程，估计得到第k时刻的初始状态变量的步骤，包括：
建立如下第一观测方程：
Figure FDA0002531623960000022
Figure FDA0002531623960000023
在式(5)中，
Figure FDA0002531623960000024
为所述第一观测变量
Figure FDA0002531623960000025
为第一观测矩阵，
Figure FDA0002531623960000026
为第一观测变量的噪声矩阵，
Figure FDA0002531623960000027
的协方差阵为
Figure FDA0002531623960000028
Figure FDA0002531623960000029
预先设置，由于第一观测矩阵
Figure FDA00025316239600000210
是非线性的，则根据式(7)计算
Figure FDA00025316239600000211
的雅可比矩阵
Figure FDA00025316239600000212
Figure FDA0002531623960000031
在式(6)和(7)中，x，y，z为第k时刻移动终端的三维坐标位置，xi，yi，zi为第k时刻第i个AP的三维坐标位置，ε为移动终端的时钟误差；
根据如下式(8)所示的状态预测方程，预测第k时刻的预测状态变量
Figure FDA0002531623960000039
Figure FDA0002531623960000032
根据如下式(9)所示的方差预测方程进行方差预测，得到第k时刻的预测方差Pk，k-1：
Figure FDA0002531623960000033
其中，Pk-1为第k-1时刻的方差，第0时刻的方差P0预先设置；
根据如下增益矩阵方程(10)和观测更新方程(11)对预测状态变量
Figure FDA0002531623960000034
进行修正，得到第k时刻的初始状态变量
Figure FDA0002531623960000035
Figure FDA0002531623960000036
Figure FDA0002531623960000037
根据如下方差更新公式(12)更新方差，得到Pk (1)：
Figure FDA0002531623960000038
其中，I为单位矩阵。
7.根据权利要求6所述的基于Wifi和传感器的融合定位方法，其特征在于，所述根据所述第k时刻的第二观测变量，对所述第k时刻的初始状态变量进行修正，得到第k时刻的目标状态变量的步骤包括：
建立如下第二观测方程：
Figure FDA0002531623960000041
Figure FDA0002531623960000042
其中，
Figure FDA0002531623960000043
为所述第二观测变量，
Figure FDA0002531623960000044
为第二观测变量的噪声矩阵，
Figure FDA0002531623960000045
的协方差阵为
Figure FDA0002531623960000046
Figure FDA0002531623960000047
预先设置；
根据如下增益矩阵方程(16)和观测更新方程(17)对第k时刻的初始状态变量
Figure FDA0002531623960000048
进行修正，得到第k时刻的目标状态变量
Figure FDA0002531623960000049
Figure FDA00025316239600000410
Figure FDA00025316239600000411
根据如下方差更新公式(18)更新方差：
Pk＝[I-Kk (2)Hk (2)]Pk (1) (18)
其中，I为单位矩阵列。
8.根据权利要求6所述的基于Wifi和传感器的融合定位方法，其特征在于，所述根据所述第k时刻的第三观测变量，对所述第k时刻的初始状态变量进行修正，得到第k时刻的目标状态变量的步骤，包括：
建立如下第三观测方程：
Figure FDA00025316239600000412
Figure FDA00025316239600000413
其中，
Figure FDA00025316239600000414
为所述第三观测变量，
Figure FDA00025316239600000415
为第三观测变量的噪声矩阵，
Figure FDA00025316239600000416
的协方差阵为
Figure FDA00025316239600000417
Figure FDA00025316239600000418
预先设置；
根据如下增益矩阵方程(25)和观测更新方程(26)对第k时刻的初始状态变量
Figure FDA0002531623960000051
进行修正，得到第k时刻的目标状态变量
Figure FDA0002531623960000052
Figure FDA0002531623960000053
Figure FDA0002531623960000054
根据如下方差更新公式(27)更新方差：
Pk＝[I-Kk (3)Hk (3)]Pk (1) (27)
其中，I为单位矩阵列。
9.一种基于Wifi和传感器的融合定位系统，其特征在于，包括：
初始位置获取模块，用于获取移动终端的初始坐标位置；
状态变量构建模块，用于构建包含坐标位置和速度的状态变量，并给所述状态变量赋初值；
状态方程建立模块，用于建立从第k-1时刻到第k时刻的状态方程，其中k＞1；
第一观测变量获取模块，用于根据Wifi RTT定位算法，获取第k时刻所述移动终端与预设的多个AP之间的距离并将所述距离作为第k时刻的第一观测变量；
初始状态变量获取模块，用于根据所述第k时刻的第一观测变量、第k-1时刻的状态变量和所述状态方程，采用滤波算法估计得到第k时刻的初始状态变量；
运动模式识别模块，用于根据所述移动终端内的传感器采集的传感器数据，识别第k时刻所述移动终端所处的运动模式；
第二观测变量获取模块，用于在所述运动模式为静止模式时，将零速度作为第k时刻的第二观测变量；
第一目标状态变量获取模块，用于根据所述第k时刻的第二观测变量，采用滤波算法对所述第k时刻的初始状态变量进行修正，得到第k时刻的目标状态变量；
第三观测变量获取模块，用于在所述运动模式为步行模式时，基于第k-1时刻所述移动终端的坐标位置、以及所述传感器数据进行PDR计算，得到第k时刻所述移动终端的PDR位置并将所述PDR位置作为第k时刻的第三观测变量；
第二目标状态变量获取模块，用于根据所述第k时刻的第三观测变量，采用滤波算法对所述第k时刻的初始状态变量进行修正，得到第k时刻的目标状态变量。
10.根据权利要求9所述的基于Wifi和传感器的融合定位系统，其特征在于，所述状态变量还包含所述移动终端的时钟误差。
11.根据权利要求9所述的基于Wifi和传感器的融合定位系统，其特征在于，所述初始位置获取模块具体用于：
采集所述移动终端到各所述AP的往返时间RTT值、以及各所述AP的坐标位置；
根据所述移动终端到各所述AP的往返时间RTT值，获取所述移动终端到各所述AP的初始距离；
根据所述移动终端到各所述AP的初始距离、以及各所述AP的坐标位置，获取所述移动终端的初始坐标位置。
12.根据权利要求9所述的基于Wifi和传感器的融合定位系统，其特征在于，所述状态方程建立模块建立的从第k-1时刻到第k时刻的状态方程如下：
Xk＝Φk，k-1Xk-1+Wk-1 (2)
Figure FDA0002531623960000061
其中，Xk为第k时刻的状态变量，Xk-1为第k-1时刻的状态变量，Wk-1为第k-1时刻的状态噪声矩阵，Wk-1的方差为Qk-1，Qk-1预先设置，Ts为第k-1时刻到第k时刻的时间间隔。
13.根据权利要求12所述的基于Wifi和传感器的融合定位系统，其特征在于，所述滤波算法为卡尔曼滤波、无迹卡尔曼滤波和粒子滤波算法中的任何一种。
14.根据权利要求13所述的基于Wifi和传感器的融合定位系统，其特征在于，当所述滤波算法为卡尔曼滤波算法时，所述初始状态变量获取模块包括：
第一观测方程建立单元，用于建立如下第一观测方程：
Figure FDA0002531623960000071
Figure FDA0002531623960000072
在式(5)中，
Figure FDA0002531623960000073
为第一观测变量，
Figure FDA0002531623960000074
为第一观测矩阵，
Figure FDA0002531623960000075
为第一观测变量的噪声矩阵，
Figure FDA0002531623960000076
的协方差阵为
Figure FDA0002531623960000077
Figure FDA0002531623960000078
预先设置，由于第一观测矩阵
Figure FDA0002531623960000079
是非线性的，则根据式(7)计算
Figure FDA00025316239600000710
的雅可比矩阵
Figure FDA00025316239600000711
Figure FDA00025316239600000712
在式(6)和(7)中，x，y，z为第k时刻移动终端的三维坐标位置，xi，yi，zi为第k时刻第i个AP的三维坐标位置，ε为移动终端的时钟误差；
状态预测单元，用于根据如下式(8)所示的状态预测方程，预测第k时刻的预测状态变量
Figure FDA00025316239600000713
Figure FDA00025316239600000714
方差预测单元，用于根据如下式(9)所示的方差预测方程进行方差预测，得到第k时刻的预测方差Pk，k-1：
Figure FDA00025316239600000715
其中，Pk-1为第k-1时刻的方差，第0时刻的方差P0预先设置；
第一修正单元，用于根据如下增益矩阵方程(10)和观测更新方程(11)对预测状态变量
Figure FDA0002531623960000081
进行修正，得到第k时刻的初始状态变量
Figure FDA0002531623960000082
Figure FDA0002531623960000083
Figure FDA0002531623960000084
第一方差更新单元，用于根据如下方差更新公式(12)更新方差，得到Pk(1)：
Figure FDA0002531623960000085
其中，I为单位矩阵。
15.根据权利要求13所述的基于Wifi和传感器的融合定位系统，其特征在于，所述第一目标状态变量获取模块包括：
第二观测方程建立单元，用于建立如下第二观测方程：
Figure FDA0002531623960000086
Figure FDA0002531623960000087
其中，
Figure FDA0002531623960000088
为第二观测变量，
Figure FDA0002531623960000089
为第二观测变量的噪声矩阵，
Figure FDA00025316239600000810
的协方差阵为
Figure FDA00025316239600000811
Figure FDA00025316239600000812
预先设置；
第二修正单元，用于根据如下增益矩阵方程(16)和观测更新方程(17)对第k时刻的初始状态变量
Figure FDA00025316239600000813
进行修正，得到第k时刻的目标状态变量
Figure FDA00025316239600000814
Figure FDA00025316239600000815
Figure FDA00025316239600000816
第二方差更新单元，用于根据如下方差更新公式(18)更新方差：
Pk＝[I-Kk (2)Hk (2)]Pk (1) (18)
其中，I为单位矩阵列。
16.根据权利要求13所述的基于Wifi和传感器的融合定位系统，其特征在于，所述第二目标状态变量获取模块包括：
第三观测方程建立单元，用于建立如下第三观测方程：
Figure FDA0002531623960000091
Figure FDA0002531623960000092
其中，
Figure FDA0002531623960000093
为第三观测变量，
Figure FDA0002531623960000094
为第三观测变量的噪声矩阵，
Figure FDA0002531623960000095
的协方差阵为
Figure FDA0002531623960000096
Figure FDA0002531623960000097
预先设置；
第三修正单元，用于根据如下增益矩阵方程(25)和观测更新方程(26)对第k时刻的初始状态变量
Figure FDA0002531623960000098
进行修正，得到第k时刻的目标状态变量
Figure FDA0002531623960000099
Figure FDA00025316239600000910
Figure FDA00025316239600000911
第三方差更新单元，用于根据如下方差更新公式(27)更新方差：
Pk＝[I-Kk (3)Hk (3)]Pk (1) (27)
其中，I为单位矩阵列。
17.一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至8中任一项所述方法的步骤。
18.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至8中任一项所述方法的步骤。
CN202010526503.8A 2020-06-09 2020-06-09 基于Wifi和传感器的融合定位方法、系统、电子设备及存储介质 Active CN111683337B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010526503.8A CN111683337B (zh) 2020-06-09 2020-06-09 基于Wifi和传感器的融合定位方法、系统、电子设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010526503.8A CN111683337B (zh) 2020-06-09 2020-06-09 基于Wifi和传感器的融合定位方法、系统、电子设备及存储介质

Publications (2)

Publication Number Publication Date
CN111683337A CN111683337A (zh) 2020-09-18
CN111683337B true CN111683337B (zh) 2021-12-10

Family

ID=72435354

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010526503.8A Active CN111683337B (zh) 2020-06-09 2020-06-09 基于Wifi和传感器的融合定位方法、系统、电子设备及存储介质

Country Status (1)

Country Link
CN (1) CN111683337B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113365217B (zh) * 2021-04-20 2022-09-30 中国科学院空天信息创新研究院 一种基于wifi-rtt测距的监听定位系统及方法
CN113613327B (zh) * 2021-08-16 2024-04-12 中国科学院空天信息创新研究院 一种基于反射投影模型增强的WiFi-RTT定位处理系统及方法
CN113890667B (zh) * 2021-12-06 2022-03-01 天津七一二通信广播股份有限公司 反向积分滤波往返校时方法与系统
CN115066012B (zh) * 2022-02-25 2024-03-19 西安电子科技大学 一种基于uwb的多用户无锚点定位方法
CN116939500A (zh) * 2022-03-31 2023-10-24 中兴通讯股份有限公司 一种追踪定位方法、装置、电子设备及存储介质

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105093178A (zh) * 2015-07-20 2015-11-25 小米科技有限责任公司 终端定位方法、装置及系统
CN106105274A (zh) * 2014-03-14 2016-11-09 高通股份有限公司 用于确定无线通信装置的位置的系统和方法
CN109154665A (zh) * 2016-03-24 2019-01-04 高通股份有限公司 确定用户设备的时间校准值

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8046169B2 (en) * 2008-01-03 2011-10-25 Andrew, Llc System and method for determining the geographic location of a device
US20140253389A1 (en) * 2013-03-08 2014-09-11 Qualcomm Incorporated Ranging using wi-fi and ultrasound measurements communication
US20140269400A1 (en) * 2013-03-14 2014-09-18 Qualcomm Incorporated Broadcasting short interframe space information for location purposes

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106105274A (zh) * 2014-03-14 2016-11-09 高通股份有限公司 用于确定无线通信装置的位置的系统和方法
CN105093178A (zh) * 2015-07-20 2015-11-25 小米科技有限责任公司 终端定位方法、装置及系统
CN109154665A (zh) * 2016-03-24 2019-01-04 高通股份有限公司 确定用户设备的时间校准值

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Android移动智能终端高精度导航定位技术研究;邢伟坡，于臻，徐斌，王玉林;《航海技术》;20190630;全文 *
Indoor Smartphone Localization: A Hybrid WiFi RTT-RSS Ranging Approach;Guangyi Guo;Ruizhi Chen;Feng Ye;Xuesheng Peng;Zuoya Liu;Yuanjin;《IEEE Access》;20191205;全文 *

Also Published As

Publication number Publication date
CN111683337A (zh) 2020-09-18

Similar Documents

Publication Publication Date Title
CN111683337B (zh) 基于Wifi和传感器的融合定位方法、系统、电子设备及存储介质
Nurminen et al. Particle filter and smoother for indoor localization
CN110398245B (zh) 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
CN109413578B (zh) 一种基于wifi与pdr融合的室内定位方法
CN109883423B (zh) 基于卡尔曼滤波的定位方法、系统、设备及存储介质
US11035915B2 (en) Method and system for magnetic fingerprinting
CN112815939B (zh) 移动机器人的位姿估计方法及计算机可读存储介质
Ibrahim et al. Inertial measurement unit based indoor localization for construction applications
Khan et al. Investigating effective methods for integration of building's map with low cost inertial sensors and wifi-based positioning
Manos et al. Walking direction estimation using smartphone sensors: A deep network-based framework
Yu et al. Neural networks-based Wi-Fi/PDR indoor navigation fusion methods
CN115031723A (zh) 一种uwb定位精度提升办法
CN110160530B (zh) 一种基于四元数的航天器姿态滤波方法
CN109769206B (zh) 一种室内定位融合方法、装置、存储介质及终端设备
JP2022537361A (ja) ドリフト補正を用いるモーションセンサを使用した相対位置追跡
CN110595464A (zh) 一种imu与视觉传感器融合定位方法和装置
CN113483753B (zh) 一种基于环境约束的惯性航向误差消除方法
Liu et al. Pedestrian indoor navigation using foot-mounted IMU with multi-sensor data fusion
Kong et al. Hybrid indoor positioning method of BLE and monocular VINS based smartphone
Zhou et al. Indoor route and location inference using smartphone IMU sensors
CN114705223A (zh) 多移动智能体在目标跟踪中的惯导误差补偿方法及系统
Lin et al. A Cnn-Speed-Based Gnss/Pdr Integrated System For Smartwatch
CN113484857B (zh) 多源异构点迹融合的跟踪方法、装置、设备及存储介质
Qian et al. An indoor fusion positioning algorithm of Bluetooth and PDR based on particle filter with dynamic adjustment of weights calculation strategy.
Raitoharju et al. A linear state model for PDR+ WLAN positioning

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant