一种基于两层扩展卡尔曼滤波的WiFi/MEMS组合室内定位
方法
【技术领域】
本发明属于室内定位技术领域,具体涉及一种基于两层扩展卡尔曼滤波的WiFi/MEMS组合室内定位方法。
【背景技术】
近些年来,位置服务(Location-Based Services,LBS)已经受到了愈发广泛的重视,并被应用于移动商务、货物追踪、寻找附近的商店或餐厅和社交等领域。相应地,对室内定位的需求在一大批应用中迅速增长。尽管全球导航卫星系统(Global NavigationSatellite Systems,GNSS)在室外环境下已经达到了理想的定位精度,但由于室内环境下GNSS信号不可用或严重衰减,室内导航定位仍是公认的难关。
目前,已存在诸如WiFi、射频识别、超声波、超宽带技术、蓝牙、紫蜂、红外线、伪卫星等多种室内定位技术。其中,由于各大场合无线网络的日益普及,WiFi定位技术受到了越来越多的关注与青睐。按照定位原理的不同,WiFi定位技术大致可分为两类:三角测量法和指纹匹配法。随着微机电系统(Micro-Electro-Mechanical Systems,MEMS)的迅速发展,MEMS惯性传感器具有了小型轻质、功耗低、制造成本低等特征优势,使其成为室内导航定位技术的一种新兴手段。航迹推算(Dead-Reckoning,DR)是应用惯性传感器定位的主要算法,包括惯性导航系统(Inertial Navigation System,INS)机械编排和行人航迹推算(Pedestrian Dead-Reckoning,PDR)两种。关于WiFi和MEMS惯性传感器的融合,不同的融合结构会导致不同的结果。文献“WiFi/MEMS组合室内定位方法研究[J].测绘通报.2016,S2:262-266”提出了一套基于WiFi指纹匹配和MEMS传感器PDR算法组合的组合室内定位方法,但是该方法的精度有待进一步提高。
【发明内容】
本发明的目的是提供一种基于两层扩展卡尔曼滤波(Extended Kalman Filter,EKF)的WiFi/MEMS组合室内定位方法,即把EKF引入以实现WiFi和MEMS惯性传感器的融合。与传统的WiFi/MEMS组合室内定位方法相比,该方法显著提高了定位精度。此外,该方法在稀疏的WiFi热点(Access Points,AP)部署下仍具有较高精度且具有一定的稳定性。
本发明一种基于两层扩展卡尔曼滤波的WiFi/MEMS组合室内定位方法,它包含以下五个步骤:
步骤一:建立系统方程
对于定姿卡尔曼滤波算法,系统的状态方程和量测方程可表示为
式中,ψk表示航向角,θk表示俯仰角,γk表示横滚角,ωxk,ωyk,ωzk表示载体坐标系下观测到的三轴角速度,wψk,wθk,wγk表示系统噪声,Δt表示采样时间间隔,Ak表示系统的量测向量,Tk表示由式(3)计算得到的方向余弦矩阵,vk表示量测噪声向量,[·]T表示转置。
系统噪声向量和量测噪声向量的协方差矩阵分别为Q1k和R1k。
对于定位卡尔曼滤波算法,系统的状态方程和量测方程可表示为
Ck=Bk+uk (5)
式中,xk,yk表示位置坐标,Sk表示步长,ψk表示由定姿卡尔曼滤波算法计算的航向角,wxk,wyk,wSk,wψk表示系统噪声,Ck表示系统的量测向量,Bk表示系统的状态向量,uk表示量测噪声向量。
系统噪声向量和量测噪声向量的协方差矩阵分别为Q2k和R2k。
步骤二:滤波初始化
对于定姿卡尔曼滤波算法,初始化如下
式中,D0表示系统的状态向量,E(·)为数学期望。
对于定位卡尔曼滤波算法,初始化如下
式中,B0表示系统的状态向量,E(·)为数学期望。
步骤三:时间更新
对于定姿卡尔曼滤波算法,系统的状态方程可表示为
Dk=fk-1(Dk-1,uk-1,w1,k-1) (10)
式中,fk-1(·)表示非线性向量函数,uk-1表示已知的输入,w1,k-1表示系统噪声向量。
循环k=1,2,…,完成以下步骤
计算以下偏微分矩阵
式中,D表示系统的状态向量,表示D在第k-1步的后验估计。
状态估计和估计误差协方差的时间更新如下
式中,表示第k步的先验估计误差协方差,表示第k-1步的后验估计误差协方差。
式中,表示D在第k步的先验估计。
对于定位卡尔曼滤波算法,系统的状态方程可表示为
Bk=gk-1(Bk-1,w2,k-1) (14)
式中,gk-1(·)表示非线性向量函数,w2,k-1表示系统噪声向量。
循环k=1,2,…,完成以下步骤
计算以下偏微分矩阵
式中,B表示系统的状态向量,表示B在第k-1步的后验估计。
状态估计和估计误差协方差的时间更新如下
式中,表示第k步的先验估计误差协方差,表示第k-1步的后验估计误差协方差。
式中,表示B在第k步的先验估计。
步骤四:量测更新
对于定姿卡尔曼滤波算法,循环k=1,2,…,完成以下步骤
计算以下偏微分矩阵
状态估计的量测更新和估计误差协方差的更新如下
式中,K1k表示卡尔曼滤波增益,表示D在第k步的后验估计,表示第k步的后验估计误差协方差。
对于定位卡尔曼滤波算法,循环k=1,2,…,完成以下步骤
状态估计的量测更新和估计误差协方差的更新如下
式中,K2k表示卡尔曼滤波增益,表示B在第k步的后验估计,表示第k步的后验估计误差协方差。
步骤五:迭代计算
重复步骤三、步骤四,直至滤波过程结束。
本发明一种基于两层扩展卡尔曼滤波的WiFi/MEMS组合室内定位方法的有益效果是:通过引入定姿和定位两层扩展卡尔曼滤波,将来自WiFi和MEMS惯性传感器的信息进行融合,通过对信息的充分挖掘和利用,显著地提高了定位精度。经过实验验证,本发明作为一种室内定位方法在稀疏的WiFi热点部署下仍具有较高精度且具有一定的稳定性。
【附图说明】
图1为本发明方法流程示意图。
图2为WiFi航向角计算法示意图。
图3为转弯时WiFi航向角计算法失效示意图。
图4为陀螺累计值示意图。
图5为行走过程中转弯示意图。
【具体实施方式】
下面结合附图对本发明作详细说明。
本发明提出了一种基于两层扩展卡尔曼滤波的WiFi/MEMS组合室内定位方法,其流程图如图1所示,它包括以下五个步骤:
步骤一:建立系统方程
在本发明中,行人方向量测是利用WiFi定位结果的信息得到的。如图2所示,运用最小二乘法,由最近的4个WiFi定位点可以拟合出一条直线。如果第k个定位点的坐标大于第(k-3)个定位点的坐标,说明行人在向前运动;如果第k个定位点的坐标小于第(k-3)个定位点的坐标,说明行人在向后运动。
由于行人在大部分时间里作直线运动,以上方法在大多数场合均适用。而当行人转弯时,其不再适用,如图3所示。因此,在运用该方法前有必要进行转弯检测,以判断行人是直线运动还是转弯运动。具体方法是:在加速度计数据检测识别步态后,根据时间戳累计加速度波峰-波谷对应时间之间的陀螺仪数据,步态间z轴陀螺累计值如图4所示,图中出现3个奇异点,结合图5,说明该奇异点代表行人在行走过程中的3次转弯过程。因此,可以根据经验设定一个阈值,一旦陀螺累计值超过了该阈值,就代表检测到了一次转弯过程。利用上述方法判断前进方向变化的前提条件是尽可能的保持设备水平稳定。
在第一层扩展卡尔曼滤波中,将来自惯性传感器(包括陀螺仪和加速度计)的信息融合以计算欧拉角(包括航向角、俯仰角和横滚角)。
系统的状态向量可表示为
Dk=[ψk θk γk]T (25)
式中,ψk表示航向角,θk表示俯仰角,γk表示横滚角。
系统的状态方程可表示为
式中,ωxk,ωyk,ωzk表示载体坐标系下观测到的三轴角速度,wψk,wθk,wγk表示系统噪声,Δt表示采样时间间隔。
系统的量测向量可表示为
Ak=[axk ayk azk]T (27)
式中,axk,ayk,azk表示载体坐标系下观测到的三轴加速度。
系统的量测方程可表示为
式中,Tk表示由式(3)计算得到的方向余弦矩阵,vk表示量测噪声向量。
在第二层扩展卡尔曼滤波中,将来自PDR和WiFi的信息(包含WiFi航向角)融合以计算行人位置。
系统的状态向量可表示为
Bk=[xk yk Sk ψk]T (29)
式中,xk,yk表示位置坐标,Sk表示步长,ψk表示由定姿卡尔曼滤波算法计算的航向角。
系统的状态方程可表示为
式中,wxk,wyk,wSk,wψk表示系统噪声。
系统的量测向量可表示为
Ck=[xWiFik yWiFik Sacck ψWiFik]T (31)
式中,xWiFik,yWiFik表示由WiFi计算得到的位置坐标,Sacck表示由式(32)计算得到的步长,ψWiFik表示由WiFi计算得到的航向角。
式中,azmax,azmin分别表示最大和最小垂向加速度,K为一校准后的常数(通过训练得到)。
系统的量测方程可表示为
Ck=Bk+uk (33)
式中,uk表示量测噪声向量。
对于定姿卡尔曼滤波算法,系统的状态方程和量测方程可表示为
式中,ψk表示航向角,θk表示俯仰角,γk表示横滚角,ωxk,ωyk,ωzk表示载体坐标系下观测到的三轴角速度,wψk,wθk,wγk表示系统噪声,Δt表示采样时间间隔,Ak表示系统的量测向量,Tk表示由式(3)计算得到的方向余弦矩阵,vk表示量测噪声向量。系统噪声向量和量测噪声向量的协方差矩阵分别为Q1k和R1k。
对于定位卡尔曼滤波算法,系统的状态方程和量测方程可表示为
Ck=Bk+uk (37)
式中,xk,yk表示位置坐标,Sk表示步长,ψk表示由定姿卡尔曼滤波算法计算的航向角,wxk,wyk,wSk,wψk表示系统噪声,Ck表示系统的量测向量,Bk表示系统的状态向量,uk表示量测噪声向量。系统噪声向量和量测噪声向量的协方差矩阵分别为Q2k和R2k。
步骤二:滤波初始化
对于定姿卡尔曼滤波算法,初始化如下
式中,D0表示系统的状态向量,E(·)为数学期望。
对于定位卡尔曼滤波算法,初始化如下
步骤三:时间更新
对于定姿卡尔曼滤波算法,系统的状态方程可表示为
Dk=fk-1(Dk-1,uk-1,w1,k-1) (42)
式中,fk-1(·)表示非线性向量函数,uk-1表示已知的输入,w1,k-1表示系统噪声向量。
循环k=1,2,…,完成以下步骤
计算以下偏微分矩阵
状态估计和估计误差协方差的时间更新如下
对于定位卡尔曼滤波算法,系统的状态方程可表示为
Bk=gk-1(Bk-1,w2,k-1) (46)
式中,gk-1(·)表示非线性向量函数,w2,k-1表示系统噪声向量。
循环k=1,2,…,完成以下步骤
计算以下偏微分矩阵
状态估计和估计误差协方差的时间更新如下
步骤四:量测更新
对于定姿卡尔曼滤波算法,循环k=1,2,…,完成以下步骤
计算以下偏微分矩阵
状态估计的量测更新和估计误差协方差的更新如下
对于定位卡尔曼滤波算法,循环k=1,2,…,完成以下步骤
状态估计的量测更新和估计误差协方差的更新如下
步骤五:迭代计算
重复步骤三、步骤四,直至滤波过程结束。