CN110602647B - 基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法 - Google Patents

基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法 Download PDF

Info

Publication number
CN110602647B
CN110602647B CN201910856194.8A CN201910856194A CN110602647B CN 110602647 B CN110602647 B CN 110602647B CN 201910856194 A CN201910856194 A CN 201910856194A CN 110602647 B CN110602647 B CN 110602647B
Authority
CN
China
Prior art keywords
representing
particle
state
time
ith particle
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
CN201910856194.8A
Other languages
English (en)
Other versions
CN110602647A (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.)
Jiangnan University
Original Assignee
Jiangnan University
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 Jiangnan University filed Critical Jiangnan University
Priority to CN201910856194.8A priority Critical patent/CN110602647B/zh
Publication of CN110602647A publication Critical patent/CN110602647A/zh
Application granted granted Critical
Publication of CN110602647B publication Critical patent/CN110602647B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S11/00—Systems for determining distance or velocity not using reflection or reradiation
    • G01S11/02—Systems for determining distance or velocity not using reflection or reradiation using radio waves
    • G01S11/06—Systems for determining distance or velocity not using reflection or reradiation using radio waves using intensity measurements
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0257—Hybrid positioning
    • 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/029—Location-based management or tracking services
    • 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/33—Services specially adapted for particular environments, situations or purposes for indoor environments, e.g. buildings
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)

Abstract

基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法，属于无线传感器网络技术领域。步骤如下：(1)首先采集WiFi的信号强度并记录位置坐标来构建指纹数据库，再采用加权k近邻法进行定位；(2)采集MEMS惯性传感器数据，结合加速度和基于差分加速度有限状态机的计步算法进行计步，融合多种传感器读数进行航向估计，结合卡尔曼滤波和非线性步长模型来估计步长；(3)使用扩展卡尔曼滤波融合WiFi指纹法和行人航位推算的定位结果；(4)结合粒子滤波和室内地图信息校正估计位置。本发明通过融合，解决了WiFi指纹法定位精度易受信号波动影响的问题以及行人航位推算方法定位误差随时间增加而累积的问题，能够显著地提高定位精度。

Description

基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法
技术领域
本发明涉及一种基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法，属于无线传感器网络技术领域。
背景技术
伴随着无线通信技术的发展和移动智能终端的普及，室内定位已成为当前信息产业的一个热门应用，并呈现出广阔的市场前景。WiFi指纹法和行人航位推算法是两种最为常用且无需部署昂贵硬件基础设施的定位方法，行人航位推算方法在短时间内定位精度较高，但定位误差随时间积累逐渐增大；而利用WiFi 定位时，定位精度易受信号波动的影响，它的特点是单点定位误差大但没有误差积累。
发明内容
针对单一的传统室内定位技术上的不足，本发明提供了一种基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法，通过使用扩展卡尔曼滤波对WiFi指纹法和行人航位推算法的定位结果进行融合，有效地弥补了它们各自的缺陷。
本发明的技术方案：
基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法，步骤如下：
(1)利用WiFi的信号强度信息进行指纹法定位，它由离线阶段和在线阶段组成，具体的步骤如下：
a)用户手持智能手机获取环境中的各个采样点上各个AP(Wireless AccessPoint，无线访问接入点)对应的WiFi信号强度、MAC地址和已知的位置坐标；
b)把所有采样点上采集到的上述信息合并在一起，构建出指纹数据库；
c)分别计算待定位点处测量得到的WiFi信号强度和采样点上WiFi信号强度之间的欧式距离，对所有采样点对应的欧式距离按顺序排序，并选择k个距离最近的采样点作为邻居；
d)把距离倒数的占比作为采样点坐标对应的权重，将这k个距离最近的采样点的坐标的加权和作为估计坐标；
(2)在已知当前位置的情况下，通过估计行人的步长和运动方向，推算下一步位置，行人航位推算由步态检测、步长估计以及方向检测三个部分组成，具体的步骤如下：
a)用户手持智能手机终端实时采集MEMS(Microelectro Mechanical Systems，微机电系统)惯性传感器数据，将原始加速度取平方和，通过卡尔曼滤波去除噪声干扰，使用加速度差分有限状态机实现计步检测；
b)把前一步步长作为后一步步长的先验估计值，把非线性步长估计模型的输出结果作为测量值，结合先验估计值和测量值进行卡尔曼滤波，把滤波得到的最优估计值作为步长估计结果。
c)当陀螺仪z轴的角速度大于0.5rad/s时，仅使用方向传感器测得的航向，当相邻时刻的方向传感器读数大于60°时，仅使用陀螺仪测得的航向；在其他情况下，使用这两种传感器数据加权融合得到的航向。
(3)利用扩展卡尔曼滤波算法将WiFi定位结果与行人航位推算定位结果进行融合，具体的步骤如下：
a)根据加速度来做步态检测，判断行人是否行走一步；
b)当检测到行走一步时，就进行WiFi扫描和行人航位推算，初始位置由WiFi 指纹法的定位结果确定，根据行人的状态信息通过非线性公式进行系统模型建模；
c)根据WiFi扫描到的信息进行指纹法定位，根据指纹法定位结果进行测量模型建模；
d)结合系统模型和测量模型进行测量更新和状态更新，将扩展卡尔曼滤波的最优估计值作为融合定位结果；
(4)结合粒子滤波和室内地图信息进行位置校正，具体的步骤如下：
a)在定位区域的范围内，初始化每个粒子的状态和权重；
b)根据系统的状态转移过程预测粒子当前时刻的状态，设置预测规则为：粒子根据各自的速度和方向进行运动，当粒子超出边界时，就随机改变方向再次预测；
c)将基于扩展卡尔曼滤波的融合定位结果作为观测位置，假设粒子的预测位置与观测位置之间的欧式距离服从高斯分布，根据其概率密度更新粒子的权重，并进行权重归一化；
d)当粒子数评价标准小于粒子总数的一半时，进行粒子重采样，在重采样过程中，重新生成权重较大的粒子、去除权重较小的粒子。
e)返回所有粒子的加权平均值作为最终的估计状态。
步骤(1)中用于WiFi指纹法定位的公式如下：
用于计算待定位点和采样点处WiFi信号强度之间的欧式距离：
Figure BDA0002198316970000031
其中，rsst,j表示待定位点处第j个AP的信号强度，rsss,j表示采样点处第j 个AP的信号强度，dt,s表示待定位点处、采样点处信号强度之间的欧式距离，n 表示AP的个数。
用于计算采样点坐标对应的权重：
Figure BDA0002198316970000041
ws表示采样点对应的权重，dt,s表示待定位点处、采样点处信号强度之间的欧式距离，k表示采样点的总数。
计算待定位点的估计坐标：
Figure BDA0002198316970000042
其中，Pt表示待定位点的位置坐标，ws表示采样点对应的权重，Ps表示采样点的位置坐标，k表示采样点的总数。
步骤(2)中MEMS惯性传感器提供的数据包括加速度数据、角速度数据和磁场强度数据。
用于行人航位推算的公式如下：
Figure BDA0002198316970000043
xn表示行人在第n步的横坐标，yn表示行人在第n步的纵坐标，dn表示行人在第n步的步长，θn表示行人在第n步的航向。
步长估计的具体步骤如下：
对状态做一步预测：
Figure BDA0002198316970000044
A表示状态转移矩阵，
Figure BDA0002198316970000045
表示第k-1步的步长估计值，
Figure BDA0002198316970000046
表示第k步的步长预测。
计算一步预测协方差矩阵：
Pk′＝APk-1AT+Qk
Pk-1表示第k-1步的估计协方差，Qk表示过程噪声的协方差。
计算卡尔曼增益：
Kk＝Pk′HT(HPk′HT+Rk)-1
Pk′表示第k步的协方差预测，H表示观测矩阵，Rk表示测量噪声的协方差。
根据Weinberg模型得到状态的观测值：
Figure BDA0002198316970000051
amax表示一步内z轴上的最大加速度，amin表示一步内z轴上的最小加速度， K为单位转换的系数。
状态更新：
Figure BDA0002198316970000052
Figure BDA0002198316970000053
表示第k步的步长预测，Kk表示卡尔曼增益，zk表示Weinberg模型得到的步长，H表示观测矩阵。
协方差矩阵更新：
Pk＝(I-KkH)Pk′
Kk表示卡尔曼增益，H表示观测矩阵，I表示一维单位阵，Pk′表示第k步的协方差预测。
加权融合方向传感器和陀螺仪数据的航向估计公式如下：
Orifuse,t＝p·Orio,t+(1-p)·Orig,t
Orio,t表示t时刻方向传感器的航向，Orig,t表示t时刻陀螺仪的航向，p表示加权系数。
步骤(3)中基于扩展卡尔曼滤波的融合定位步骤如下：
根据系统模型获得状态的先验信息：
Figure BDA0002198316970000061
xk-1表示行人在第k-1步的横坐标,sk表示第k步的步长，θk-1表示第k-1 步的航向，yk-1表示行人在第k-1步的纵坐标，Δθk表示第k步的航向变化。
计算观测向量的预测值：
Zk|k-1＝HkXk|k-1
Hk表示观测矩阵，Xk|k-1表示状态的预测。
求解状态方程的雅克比矩阵：
Figure BDA0002198316970000062
求解预测误差协方差：
Figure BDA0002198316970000063
Figure BDA0002198316970000064
表示状态转移矩阵，Pk-1表示第k-1步的估计协方差，Qk表示过程噪声的协方差。
计算卡尔曼增益：
Figure BDA0002198316970000065
Pk|k-1表示预测误差协方差，Hk表示观测矩阵。
将WiFi指纹法的定位结果作为状态向量的观测值。
状态更新：
Xk＝Xk|k-1+Kk(Zk-Zk|k-1)
Xk|k-1表示状态的预测，Kk表示卡尔曼增益，Zk表示状态向量的观测值， Zk|k-1表示观测向量的预测。
协方差矩阵更新：
Pk＝(I-KkHk)Pk|k-1
Kk表示卡尔曼增益，Hk表示观测矩阵，I表示单位阵，Pk|k-1表示预测误差协方差。
步骤(4)中基于粒子滤波的位置校正步骤如下：
根据系统的状态转移方程预测粒子当前时刻的状态：
Figure BDA0002198316970000071
xi,k-1表示k-1时刻第i个粒子的横坐标，θi,k表示k时刻第i个粒子的方向， vi,k表示k时刻第i个粒子的速度，yi,k-1表示k-1时刻第i个粒子的纵坐标，θstd表示粒子方向的标准差。
计算预测位置与观测位置之间的欧式距离：
Figure BDA0002198316970000072
xi,k表示k时刻第i个粒子的预测位置的横坐标，yi,k表示k时刻第i个粒子的预测位置的纵坐标，x′i,k表示k时刻第i个粒子的观测位置的横坐标，y′i,k表示 k时刻第i个粒子的观测位置的纵坐标。
计算上述距离的概率密度：
Figure BDA0002198316970000073
dstd表示粒子速度的标准差，di,k表示k时刻第i个粒子预测位置与观测位置之间的欧式距离。
更新粒子的权重：
Figure BDA0002198316970000081
wi,k-1表示k-1时刻第i个粒子的初始权重。
权重归一化：
Figure BDA0002198316970000082
Figure BDA0002198316970000083
表示k时刻第i个粒子更新后的权重，M表示粒子总数。
粒子数评价标准的计算公式如下：
Figure BDA0002198316970000084
返回所有粒子的加权平均值作为最终的估计状态：
Figure BDA0002198316970000085
Xi,k表示k时刻第i个粒子的状态，wi,k表示k时刻第i个粒子的归一化权重，M表示粒子总数。
本发明的有益效果：本发明解决了WiFi指纹法定位精度易受信号波动影响的问题以及行人航位推算方法定位误差随时间增加而累积的问题，跟传统单一的定位技术相比，能够显著地提高定位精度。
附图说明
图1是本发明室内定位系统的框架图。
图2是行人航位推算原理图。
图3是改进前和改进后的步长估计结果对比图。
图4是改进前和改进后的步长估计错误率对比图。
图5是滤波前和滤波后的航向角对比图。
图6是融合前和融合后的航向角对比图。
图7是实验中各个定位算法的定位误差效果图。
图8是实验中各个定位算法的定位误差的累积概率对比图。
具体实施方式
根据权利要求所包含的内容举例说明
实施例1：
本发明在面积为100多平方米的实验室内进行了数据的采集与分析，该区域包括了WiFi无线信号的覆盖。使用华为P8(Android 4.0平台)型号的智能手机进行数据的采集。
利用WiFi的信号强度信息进行指纹法定位，具体的步骤如下：
a)采集wifi信号强度和已知位置的坐标，构建出指纹数据库；
b)用k近邻法进行指纹法定位；
实施例2：
用户行走时沿着顺时针方向，并用智能手机采集加速度、磁场强度、角速度。
通过估计行人的步长和运动方向，推算下一步位置，具体公式如下：
Figure BDA0002198316970000091
行人航位推算具体的步骤如下：
a)使用加速度差分有限状态机实现计步检测；
b)用基于卡尔曼滤波的方法进行步长估计；
c)融合方向传感器、陀螺仪数据得到航向。用于加权融合的公式如下：
Orifuse,t＝p·Orio,t+(1-p)·Orig,t
实验中，分别用改进前和改进后的非线性步长估计模型进行步长估计。其中，非线性模型的参数K取0.35。分别获得方向传感器、陀螺仪得到的航向角，并进行FIR低通滤波处理，再代入加权融合算法中，其中，将FIR低通滤波器的临界频率设为0.005，权重系数p＝0.1(如果陀螺仪测量结果更接近真实方向，设置p接近0；如果方向传感器测量结果更接近真实方向，设置p接近1)。
通过卡尔曼滤波，改进后的非线性模型估计得到的步长更加稳定一些，在准确性方面也优于改进前的模型。FIR低通滤波可以剔除传感器读数中存在的高频噪声，通过调节权重参数p，加权融合算法可以有效补偿两种传感器的缺陷，从而灵活地校正航向。
实施例3：
利用扩展卡尔曼滤波算法将WiFi定位结果与行人航位推算定位结果进行融合，具体的步骤如下：
a)根据系统模型获得状态的先验信息：
Figure BDA0002198316970000101
b)将WiFi指纹法的定位结果作为状态向量的观测值；
c)结合系统模型和测量模型进行状态更新：
Xk＝Xk|k-1+Kk(Zk-Zk|k-1)
其中，扩展卡尔曼滤波的估计误差协方差初始值设为
Figure BDA0002198316970000111
过程噪声的协方差设为
Figure BDA0002198316970000112
测量噪声的协方差设为
Figure BDA0002198316970000113
实施例4：
结合粒子滤波和室内地图信息进行位置校正，具体的步骤如下：
a)根据系统的状态转移过程预测粒子当前时刻的状态，预测公式为：
Figure BDA0002198316970000114
b)更新粒子的权重：
Figure BDA0002198316970000115
c)粒子重采样。
d)返回所有粒子的加权平均值作为最终的估计状态：
Figure BDA0002198316970000116
粒子运动速度的标准差vstd＝0.01，粒子运动方向的标准差θstd＝0.01。

Claims (8)

1.基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法，其特征在于，步骤如下：
(1)利用WiFi的信号强度信息进行指纹法定位，它由离线阶段和在线阶段组成，具体的步骤如下：
a)用户手持智能手机获取环境中的各个采样点上各个AP对应的WiFi信号强度、MAC地址和已知的位置坐标；
b)把所有采样点上采集到的上述信息合并在一起，构建出指纹数据库；
c)分别计算待定位点处测量得到的WiFi信号强度和采样点上WiFi信号强度之间的欧式距离，对所有采样点对应的欧式距离按顺序排序，并选择k个距离最近的采样点作为邻居；
d)把距离倒数的占比作为采样点坐标对应的权重，将这k个距离最近的采样点的坐标的加权和作为估计坐标；
(2)在已知当前位置的情况下，通过估计行人的步长和运动方向，推算下一步位置，行人航位推算由步态检测、步长估计以及方向检测三个部分组成，具体的步骤如下：
a)用户手持智能手机终端实时采集MEMS惯性传感器数据，将原始加速度取平方和，通过卡尔曼滤波去除噪声干扰，使用加速度差分有限状态机实现计步检测；
b)把前一步步长作为后一步步长的先验估计值，把非线性步长估计模型的输出结果作为测量值，结合先验估计值和测量值进行卡尔曼滤波，把滤波得到的最优估计值作为步长估计结果；
c)当陀螺仪z轴的角速度大于0.5rad/s时，仅使用方向传感器测得的航向，当相邻时刻的方向传感器读数大于60°时，仅使用陀螺仪测得的航向；在其他情况下，使用这两种传感器数据加权融合得到的航向；
(3)利用扩展卡尔曼滤波算法将WiFi定位结果与行人航位推算定位结果进行融合，具体的步骤如下：
a)根据加速度来做步态检测，判断行人是否行走一步；
b)当检测到行走一步时，就进行WiFi扫描和行人航位推算，初始位置由WiFi指纹法的定位结果确定，根据行人的状态信息通过非线性公式进行系统模型建模；
c)根据WiFi扫描到的信息进行指纹法定位，根据指纹法定位结果进行测量模型建模；
d)结合系统模型和测量模型进行测量更新和状态更新，将扩展卡尔曼滤波的最优估计值作为融合定位结果；
(4)结合粒子滤波和室内地图信息进行位置校正，具体的步骤如下：
a)在定位区域的范围内，初始化每个粒子的状态和权重；
b)根据系统的状态转移过程预测粒子当前时刻的状态，设置预测规则为：粒子根据各自的速度和方向进行运动，当粒子超出边界时，就随机改变方向再次预测；
c)将基于扩展卡尔曼滤波的融合定位结果作为观测位置，设粒子的预测位置与观测位置之间的欧式距离服从高斯分布，根据其概率密度更新粒子的权重，并进行权重归一化；
d)当粒子数评价标准小于粒子总数的一半时，进行粒子重采样，在重采样过程中，重新生成权重较大的粒子、去除权重较小的粒子；
e)返回所有粒子的加权平均值作为最终的估计状态。
2.根据权利要求1所述的基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法，其特征在于，步骤(1)中用于WiFi指纹法定位的公式如下：
用于计算待定位点和采样点处WiFi信号强度之间的欧式距离：
Figure FDA0002618130150000031
其中，rsst,j表示待定位点处第j个AP的信号强度，rsss,j表示采样点处第j个AP的信号强度，dt,s表示待定位点处、采样点处信号强度之间的欧式距离，n表示AP的个数；
用于计算采样点坐标对应的权重：
Figure FDA0002618130150000032
ws表示采样点对应的权重，dt,s表示待定位点处、采样点处信号强度之间的欧式距离，k表示采样点的总数；
计算待定位点的估计坐标：
Figure FDA0002618130150000033
其中，Pt表示待定位点的位置坐标，ws表示采样点对应的权重，Ps表示采样点的位置坐标，k表示采样点的总数。
3.根据权利要求1或2所述的基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法，其特征在于，步骤(2)中MEMS惯性传感器提供的数据包括加速度数据、角速度数据和磁场强度数据；
用于行人航位推算的公式如下：
Figure FDA0002618130150000034
xn表示行人在第n步的横坐标，yn表示行人在第n步的纵坐标，dn表示行人在第n步的步长，θn表示行人在第n步的航向；
步长估计的具体步骤如下：
对状态做一步预测：
Figure FDA0002618130150000041
A表示状态转移矩阵，
Figure FDA0002618130150000042
表示第k-1步的步长估计值，
Figure FDA0002618130150000043
表示第k步的步长预测；
计算一步预测协方差矩阵：
Pk′＝APk-1AT+Qk
Pk-1表示第k-1步的估计协方差，Qk表示过程噪声的协方差；
计算卡尔曼增益：
Kk＝Pk′HT(HPk′HT+Rk)-1
Pk′表示第k步的协方差预测，H表示观测矩阵，Rk表示测量噪声的协方差；根据Weinberg模型得到的步长：
Figure FDA0002618130150000044
amax表示一步内z轴上的最大加速度，amin表示一步内z轴上的最小加速度，K为单位转换的系数；
状态更新：
Figure FDA0002618130150000045
Figure FDA0002618130150000046
表示第k步的步长预测，Kk表示卡尔曼增益，zk表示Weinberg模型得到的步长，H表示观测矩阵；
协方差矩阵更新：
Pk＝(I-KkH)Pk′
Kk表示卡尔曼增益，H表示观测矩阵，I表示一维单位阵，Pk′表示第k步的协方差预测；
加权融合方向传感器和陀螺仪数据的航向估计公式如下：
Orifuse,t＝p·Orio,t+(1-p)·Orig,t
Orio,t表示t时刻方向传感器的航向，Orig,t表示t时刻陀螺仪的航向，p表示加权系数。
4.根据权利要求1或2所述的基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法，其特征在于，步骤(3)中基于扩展卡尔曼滤波的融合定位步骤如下：
根据系统模型获得状态的先验信息：
Figure FDA0002618130150000051
xk-1表示行人在第k-1步的横坐标,sk表示第k步的步长，θk-1表示第k-1步的航向，yk-1表示行人在第k-1步的纵坐标，Δθk表示第k步的航向变化；
计算观测向量的预测值：
Zk|k-1＝HkXk|k-1
Hk表示观测矩阵，Xk|k-1表示状态的预测；
求解状态方程的雅克比矩阵：
Figure FDA0002618130150000052
求解预测误差协方差：
Figure FDA0002618130150000053
Figure FDA0002618130150000061
表示状态转移矩阵，Pk-1表示第k-1步的估计协方差，Qk表示过程噪声的协方差；
计算卡尔曼增益：
Figure FDA0002618130150000062
Pk|k-1表示预测误差协方差，Hk表示观测矩阵；
将WiFi指纹法的定位结果作为状态向量的观测值；
状态更新：
Xk＝Xk|k-1+Kk(Zk-Zk|k-1)
Xk|k-1表示状态的预测，Kk表示卡尔曼增益，Zk表示状态向量的观测值，Zk|k-1表示观测向量的预测；
协方差矩阵更新：
Pk＝(I-KkHk)Pk|k-1
Kk表示卡尔曼增益，Hk表示观测矩阵，I表示单位阵，Pk|k-1表示预测误差协方差。
5.根据权利要求3所述的基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法，其特征在于，步骤(3)中基于扩展卡尔曼滤波的融合定位步骤如下：
根据系统模型获得状态的先验信息：
Figure FDA0002618130150000063
xk-1表示行人在第k-1步的横坐标,sk表示第k步的步长，θk-1表示第k-1步的航向，yk-1表示行人在第k-1步的纵坐标，Δθk表示第k步的航向变化；
计算观测向量的预测值：
Zk|k-1＝HkXk|k-1
Hk表示观测矩阵，Xk|k-1表示状态的预测；
求解状态方程的雅克比矩阵：
Figure FDA0002618130150000071
求解预测误差协方差：
Figure FDA0002618130150000072
Figure FDA0002618130150000073
表示状态转移矩阵，Pk-1表示第k-1步的估计协方差，Qk表示过程噪声的协方差；
计算卡尔曼增益：
Figure FDA0002618130150000074
Pk|k-1表示预测误差协方差，Hk表示观测矩阵；
将WiFi指纹法的定位结果作为状态向量的观测值；
状态更新：
Xk＝Xk|k-1+Kk(Zk-Zk|k-1)
Xk|k-1表示状态的预测，Kk表示卡尔曼增益，Zk表示状态向量的观测值，Zk|k-1表示观测向量的预测；
协方差矩阵更新：
Pk＝(I-KkHk)Pk|k-1
Kk表示卡尔曼增益，Hk表示观测矩阵，I表示单位阵，Pk|k-1表示预测误差协方差。
6.根据权利要求1、2或5所述的基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法，其特征在于，步骤(4)中基于粒子滤波的位置校正步骤如下：
根据系统的状态转移方程预测粒子当前时刻的状态：
Figure FDA0002618130150000081
xi,k-1表示k-1时刻第i个粒子的横坐标，θi,k表示k时刻第i个粒子的方向，vi,k表示k时刻第i个粒子的速度，yi,k-1表示k-1时刻第i个粒子的纵坐标，θstd表示粒子方向的标准差；
计算预测位置与观测位置之间的欧式距离：
Figure FDA0002618130150000082
xi,k表示k时刻第i个粒子的预测位置的横坐标，yi,k表示k时刻第i个粒子的预测位置的纵坐标，x′i,k表示k时刻第i个粒子的观测位置的横坐标，y′i,k表示k时刻第i个粒子的观测位置的纵坐标；
计算上述距离的概率密度：
Figure FDA0002618130150000083
dstd表示粒子速度的标准差，di,k表示k时刻第i个粒子预测位置与观测位置之间的欧式距离；
更新粒子的权重：
Figure FDA0002618130150000084
wi,k-1表示k-1时刻第i个粒子的初始权重；
权重归一化：
Figure FDA0002618130150000085
Figure FDA0002618130150000091
表示k时刻第i个粒子更新后的权重，M表示粒子总数；
粒子数评价标准的计算公式如下：
Figure FDA0002618130150000092
返回所有粒子的加权平均值作为最终的估计状态：
Figure FDA0002618130150000093
Xi,k表示k时刻第i个粒子的状态，wi,k表示k时刻第i个粒子的归一化权重，M表示粒子总数。
7.根据权利要求3所述的基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法，其特征在于，步骤(4)中基于粒子滤波的位置校正步骤如下：
根据系统的状态转移方程预测粒子当前时刻的状态：
Figure FDA0002618130150000094
xi,k-1表示k-1时刻第i个粒子的横坐标，θi,k表示k时刻第i个粒子的方向，vi,k表示k时刻第i个粒子的速度，yi,k-1表示k-1时刻第i个粒子的纵坐标，θstd表示粒子方向的标准差；
计算预测位置与观测位置之间的欧式距离：
Figure FDA0002618130150000095
xi,k表示k时刻第i个粒子的预测位置的横坐标，yi,k表示k时刻第i个粒子的预测位置的纵坐标，x′i,k表示k时刻第i个粒子的观测位置的横坐标，y′i,k表示k时刻第i个粒子的观测位置的纵坐标；
计算上述距离的概率密度：
Figure FDA0002618130150000101
dstd表示粒子速度的标准差，di,k表示k时刻第i个粒子预测位置与观测位置之间的欧式距离；
更新粒子的权重：
Figure FDA0002618130150000102
wi,k-1表示k-1时刻第i个粒子的初始权重；
权重归一化：
Figure FDA0002618130150000103
Figure FDA0002618130150000104
表示k时刻第i个粒子更新后的权重，M表示粒子总数；
粒子数评价标准的计算公式如下：
Figure FDA0002618130150000105
返回所有粒子的加权平均值作为最终的估计状态：
Figure FDA0002618130150000106
Xi,k表示k时刻第i个粒子的状态，wi,k表示k时刻第i个粒子的归一化权重，M表示粒子总数。
8.根据权利要求4所述的基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法，其特征在于，步骤(4)中基于粒子滤波的位置校正步骤如下：
根据系统的状态转移方程预测粒子当前时刻的状态：
Figure FDA0002618130150000111
xi,k-1表示k-1时刻第i个粒子的横坐标，θi,k表示k时刻第i个粒子的方向，vi,k表示k时刻第i个粒子的速度，yi,k-1表示k-1时刻第i个粒子的纵坐标，θstd表示粒子方向的标准差；
计算预测位置与观测位置之间的欧式距离：
Figure FDA0002618130150000112
xi,k表示k时刻第i个粒子的预测位置的横坐标，yi,k表示k时刻第i个粒子的预测位置的纵坐标，x′i,k表示k时刻第i个粒子的观测位置的横坐标，y′i,k表示k时刻第i个粒子的观测位置的纵坐标；
计算上述距离的概率密度：
Figure FDA0002618130150000113
dstd表示粒子速度的标准差，di,k表示k时刻第i个粒子预测位置与观测位置之间的欧式距离；
更新粒子的权重：
Figure FDA0002618130150000114
wi,k-1表示k-1时刻第i个粒子的初始权重；
权重归一化：
Figure FDA0002618130150000115
Figure FDA0002618130150000116
表示k时刻第i个粒子更新后的权重，M表示粒子总数；
粒子数评价标准的计算公式如下：
Figure FDA0002618130150000121
返回所有粒子的加权平均值作为最终的估计状态：
Figure FDA0002618130150000122
Xi,k表示k时刻第i个粒子的状态，wi,k表示k时刻第i个粒子的归一化权重，M表示粒子总数。
CN201910856194.8A 2019-09-11 2019-09-11 基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法 Active CN110602647B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910856194.8A CN110602647B (zh) 2019-09-11 2019-09-11 基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910856194.8A CN110602647B (zh) 2019-09-11 2019-09-11 基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法

Publications (2)

Publication Number Publication Date
CN110602647A CN110602647A (zh) 2019-12-20
CN110602647B true CN110602647B (zh) 2020-11-24

Family

ID=68858837

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910856194.8A Active CN110602647B (zh) 2019-09-11 2019-09-11 基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法

Country Status (1)

Country Link
CN (1) CN110602647B (zh)

Families Citing this family (27)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111044050B (zh) * 2019-12-30 2022-06-21 中电海康集团有限公司 一种基于粒子滤波和卡尔曼滤波的蓝牙定位方法
CN111256727B (zh) * 2020-02-19 2022-03-08 广州蓝胖子机器人有限公司 一种基于Augmented EKF的提高里程计精度的方法
CN111536967B (zh) * 2020-04-09 2022-12-16 江苏大学 一种基于ekf的多传感器融合温室巡检机器人跟踪方法
CN111623765B (zh) * 2020-05-18 2022-07-01 清华大学 基于多模态数据的室内定位方法及系统
CN112333818B (zh) * 2020-10-27 2021-11-02 中南民族大学 基于自适应周期性粒子滤波的多源融合室内定位系统及方法
CN112423387A (zh) * 2020-11-23 2021-02-26 刘秀萍 多种传感器融合的终端设备室内定位方法
CN112539747B (zh) * 2020-11-23 2023-04-28 华中科技大学 一种基于惯性传感器和雷达的行人航位推算方法和系统
CN112461237B (zh) * 2020-11-26 2023-03-14 浙江同善人工智能技术有限公司 一种应用于动态变化场景下的多传感器融合定位方法
CN112511972A (zh) * 2020-11-27 2021-03-16 国网天津市电力公司 基于5g的变电站巡检机器人定位方法及装置
CN112881979B (zh) * 2021-01-19 2022-10-21 上海工程技术大学 一种基于ekf滤波的初始状态自适应融合定位方法
CN112797985B (zh) * 2021-02-08 2024-01-23 上海第二工业大学 基于加权扩展卡尔曼滤波的室内定位方法及室内定位系统
CN113077055A (zh) * 2021-03-26 2021-07-06 北京邮电大学 一种基于FBPF-EIKF-FastSLAM的分布式多源融合定位方法
CN113091748A (zh) * 2021-04-12 2021-07-09 北京航空航天大学 一种室内自校准导航定位方法
CN113050142B (zh) * 2021-05-31 2021-08-10 腾讯科技（深圳）有限公司 终端设备的定位方法、装置、电子设备及可读存储介质
CN113256866B (zh) * 2021-06-15 2021-09-24 南京高美吉交通科技有限公司 城市轨道交通无障碍通行系统及其实施方法
CN113566820B (zh) * 2021-06-17 2023-05-16 电子科技大学 一种基于位置指纹和pdr算法的融合人行定位方法
CN113422593B (zh) * 2021-07-05 2024-04-26 北京信息科技大学 滤波方法、滤波器、计算机可读存储介质、处理器和fpga
CN113932809B (zh) * 2021-11-26 2024-03-12 昆山九毫米电子科技有限公司 基于智能粒子滤波的室内无人靶车定位方法
CN114302359B (zh) * 2021-12-02 2023-08-01 中国船舶重工集团公司第七0九研究所 一种基于WiFi-PDR融合的高精度室内定位方法
CN114554389B (zh) * 2021-12-29 2023-06-20 重庆邮电大学 一种行人导航定位系统融合方法
CN114485639B (zh) * 2022-01-19 2023-10-20 浙江大学 一种用于室内导航的uwb定位漂移校正方法
CN114440881B (zh) * 2022-01-29 2023-05-30 海南大学 一种融合多源传感器信息的无人车定位方法
CN115112121B (zh) * 2022-03-28 2023-03-03 浙江德清知路导航科技有限公司 基于数据与模型结合的多源融合定位方法、系统及终端
CN114739397B (zh) * 2022-03-28 2024-05-07 中国矿业大学 矿井环境运动惯性估计自适应卡尔曼滤波融合定位方法
CN114710744B (zh) * 2022-04-12 2023-03-21 电子科技大学 一种深度融合WiFi测距与PDR推算的室内定位方法
CN114894196B (zh) * 2022-05-20 2023-09-19 上海航天测控通信研究所 一种航迹推算辅助的离开角测向定位方法
CN115103299B (zh) * 2022-06-15 2024-04-09 河南工程学院 一种基于rfid的多传感器融合定位方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101895867A (zh) * 2010-06-25 2010-11-24 哈尔滨工业大学 基于滑动时间窗的wlan室内wknn跟踪方法
CN102932742A (zh) * 2012-10-12 2013-02-13 上海交通大学 基于惯性传感器与无线信号特征的室内定位方法及系统

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104869541B (zh) * 2015-06-11 2018-02-27 吉林大学 一种室内定位追踪方法
CN105372628A (zh) * 2015-11-19 2016-03-02 上海雅丰信息科技有限公司 一种基于Wi-Fi的室内定位导航方法
US9906914B2 (en) * 2016-05-11 2018-02-27 Mapsted Corp. Scalable indoor navigation and positioning systems and methods
CN107389063B (zh) * 2017-07-26 2020-12-22 重庆邮电大学 基于gsm/mems融合的高精度室内融合定位方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101895867A (zh) * 2010-06-25 2010-11-24 哈尔滨工业大学 基于滑动时间窗的wlan室内wknn跟踪方法
CN102932742A (zh) * 2012-10-12 2013-02-13 上海交通大学 基于惯性传感器与无线信号特征的室内定位方法及系统

Also Published As

Publication number Publication date
CN110602647A (zh) 2019-12-20

Similar Documents

Publication Publication Date Title
CN110602647B (zh) 基于扩展卡尔曼滤波和粒子滤波的室内融合定位方法
CN109413578B (zh) 一种基于wifi与pdr融合的室内定位方法
CN110958575B (zh) 一种基于WiFi融合预测的定位方法及系统
CN106714110B (zh) 一种Wi-Fi位置指纹地图自动构建方法及系统
CN108632761B (zh) 一种基于粒子滤波算法的室内定位方法
CN110388926B (zh) 一种基于手机地磁和场景图像的室内定位方法
CN105716604A (zh) 基于地磁序列的移动机器人室内定位方法及系统
CN108521627B (zh) 基于HMM的wifi和地磁融合的室内定位系统及方法
CN103471589A (zh) 一种室内行人行走模式识别和轨迹追踪的方法
CN110320495A (zh) 一种基于Wi-Fi、蓝牙和PDR融合定位的室内定位方法
CN104180805A (zh) 基于智能手机的室内行人定位与跟踪方法
JP2008126401A (ja) パーティクルフィルター基盤の移動ロボットの姿勢推定方法、装置及び媒体
CN110487286B (zh) 基于点特征投影与激光点云融合的机器人位姿判断方法
CN112731371B (zh) 一种激光雷达与视觉融合的集成化目标跟踪系统及方法
CN111901749A (zh) 一种基于多源融合的高精度三维室内定位方法
CN112881979B (zh) 一种基于ekf滤波的初始状态自适应融合定位方法
CN111970633A (zh) 基于WiFi、蓝牙和步行者行航位推算融合的室内定位方法
CN110426037A (zh) 一种封闭环境下的行人运动轨迹实时获取方法
CN113566820A (zh) 一种基于位置指纹和pdr算法的融合人行定位方法
CN115017578A (zh) 一种建筑的智能实测实量方法、装置、ugv及存储介质
Antsfeld et al. Deep smartphone sensors-WiFi fusion for indoor positioning and tracking
Wei et al. MM-Loc: Cross-sensor indoor smartphone location tracking using multimodal deep neural networks
CN111578938B (zh) 目标物的定位方法及装置
CN108462939A (zh) 一种地磁时序分析的室内定位方法
CN114302359B (zh) 一种基于WiFi-PDR融合的高精度室内定位方法

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