基于抗差EKF的Wi-Fi/MEMS融合室内定位算法

张千坤，钟志刚，陈任翔，张校伟（.中讯邮电咨询设计院有限公司郑州分公司，河南郑州 450007；.中讯邮电咨询设计院有限公司，北京 00048）

1 概述

随着基于位置的服务（LBS）［1-2］被越来越多的移动用户所使用，给无线网络定位技术带来了巨大的机遇。在室外环境下，全球导航卫星系统（GNSS）基本可以满足移动用户的定位需求，但是在市区或者室内环境下会受到建筑物的影响，性能变差无法达到定位的要求［3］。

为了弥补GNSS定位方面的缺陷，基于无线网［4-5］、蓝牙［6］、超宽带（UWB）［7］等的定位技术开始普及，其中基于无线网的定位技术由于造价低、分布广，应用最为广泛，主要包括基于指纹的定位技术［8］、基于测距的定位技术［9］、基于测角的定位技术［10］。但是，由于环境因素、定位精度等问题限制了无线网单一定位技术的发展，其中指纹定位技术由于受多径干扰的影响，信号接收强度（RSSI）会发生抖动，影响定位的效果，测角与测距的定位技术需要在有直射径的环境下才能准确定位，在非视距的环境下会产生很大的定位误差。近年来，数据融合广泛用于室内定位中，基于此，本文提出一种新的定位方法，首先对线阵AOA 估计技术进行改进，提出一种基于L阵的空间平滑AOA 估计技术，可以准确地估计出信号的方位角与俯仰角，提高角度估计的准确性；其次，联合多个Wi-Fi 接入点（AP）测量的角度利用最小二乘拟合得到移动终端的位置；最后设计抗差卡尔曼滤波器（EKF），将AOA 定位结果与MEMS 传感器解算的信息进行融合，抑制MEMS 传感器产生的累计误差，并解决AOA 定位在NLOS环境下无法定位的问题。

2 定位算法描述

本文提出的基于抗差EKF的Wi-Fi/MEMS 融合室内定位系统框图如图1 所示。主要包括4 个部分，分别是基于Wi-Fi 的AOA 定位模块，速度与航向角计算模块，抗差EKF 滤波模块以及位置结算模块。首先利用从Wi-Fi 设备中提取的CSI 信息通过一种空间平滑技术虚拟大规模阵列天线，利用2 维L 阵MUSIC 算法对俯仰角和方位角进行估计，并利用最小二乘算法结合多个AP的角度得到定位结果；其次根据MEMS传感器输出的参数对目标进行步态检测和四元素计算，得到目标的速度信息；最后将Wi-Fi 的定位结果作为抗差EKF 滤波器的输入信息，对目标进行二维定位，并结合气压计的输出信息，得到目标的高度进行三维定位。

图1 系统框图

2.1 基于Wi-Fi的AOA定位技术

由文献［11］可知，传统的MUSIC 算法只能解算出小于阵列天线数量的信号个数，利用子载波CSI 信息可以重新构造阵列天线的方向矩阵，扩展信号子空间特征向量，从而虚拟大规模的阵列天线，解决在室内环境下由于多径信号过多，造成MUSIC 算法无法区分出信号子空间与噪声子空间的问题［12］。但是这种算法只能估计到达信号的方位角，如果终端与AP 之间的高度落差较大，则角度估计会出现偏差。本文在此基础上，提出一种L阵空间平滑AOA 估计算法，可以同时对俯仰角和方位角进行估计，克服了高度对方位角估计的影响。

由于Wi-Fi 信号是OFDM 信号，信号到达天线引起的相位差包括2 个部分，分别是由信号到达时间引起的不同子载波之间的相位差以及信号到达不同天线由角度引起的相位差。

信号到达时间引起的相位差可以表示为Ωτk=e-j2πΔfτk，L阵天线之间的相位差包含2 个部分，分别为，当 有2×M-1 根天线（x轴M根，y轴M-1 根），N个子载波时，接收到信号的方向矩阵可以表示为：

式中：

Φx,θk,φk——x轴2根天线之间的相位差

Φy,θk,φk——y轴2根天线之间的相位差

由无线网卡采集到L阵3个通道的90个子载波可以表示为：

其中csim,n(m=1,2,3;n=1,2,…,30)表示的是第m根天线，第n个子载波上测量得到的CSI。

图2 空间平滑示意图

在每根天线上选择15 个子载波为一组进行前项空间平滑［13］，平滑后的结果如图3所示。

图3 空间平滑结果

从图3中可以看出，经过空间平滑以后，可以由原先的三根天线虚拟出现在的45根天线，远大于室内多径信号的数量，结合文献［11］利用MUSIC 算法可以准确地估计出信号的方位角。同时结合多台AP 的AOA信息，利用最小二乘算法对目标的位置信息进行解算。

式中：

R——AP的个数

——终端在位置p时到第i台接收机的角度

θi——第i台接收机估计出的角度

2.2 速度与航向角估计

本系统选择手机自带的惯性传感器进行定位，定位框图如图4所示。

图4 MEMS定位框图

2.2.1 速度估计

行人在行走的时候加速度以正弦波的形式变化［14］，因此可以通过三轴加速度计测量出的值来检测行人的步态，三轴加速度计值建立的步态检测模型可以表示为：

式中：

ax，ay，az——三轴加速度数据

为了消除传感器自带的噪声干扰，本文选择低通滤波器对三轴加速度计的输出值进行滤波处理。因此公式（4）可以写成：

式中：

axfilter，ayfilter，azfilter——滤波后的三轴加速度计输出值

本文通过检测Atotal是否为局部最大值以及是否大于给定的阈值A0来判定此时目标是否跨步。假设MEMS传感器的输出频率为fs，利用步态检测可以检测出每秒由于目标走动而出现的峰值点的个数，从而得到相邻峰值点之间的采样个数ΔN，则可以得到目标走到第k步需要的时间为，进而得到速度为vk=，其中vk表示的是第k步时的速度，Pk表示第k步时的步长。

2.2.2 航向角估计

本文采用四元素法对终端的航向角进行计算，四元素法的表示形式为q=w+xi+yj+zk，

载体坐标系与参考坐标系之间的转换为：

其中(q)为姿态旋转矩阵，根据姿态旋转矩阵与四元素的关系［15］可以得到：

在已知四元素后，通过哥氏定理变换［16］可以得出四元素的微分方程：

式中：

ωx，ωy，ωz——传感器在载体坐标系下的三轴角速度的数据

本文设置四元素为滤波器的状态变量，对式（8）进行离散化处理，得到：

式中：

Ts——采样时间间隔

I——单位矩阵

——初始四元素

k——采样时刻

对四元素进行归一化结合公式（9）即可更新姿态旋转矩阵。

由于陀螺仪的不理想容易造成四元素计算时出现误差，本文使用磁力计和加速度计来建立新的状态方程和观测方程：

式中：

Wk——过程激励噪声

Vk+1——观测噪声

Yk+1——观测变量，计算公式如下：

通过扩展卡尔曼模型更新四元素，利用四元素与欧拉角的关系［15］得到航向角为：

3 Wi-Fi/MEMS融合定位

本文采用EKF 算法将Wi-Fi 的定位结果与MEMS传感器解算的速度与航向进行组合，建立如下状态方程：

式中：

Et-1和Nt-1——t-1时刻的地理东位置和北位置

νt-1和φt-1——t-1时刻终端的速度和航向角

Wt-1——t-1时刻的状态噪声

p(Wt-1)～N(0,Qt)，Qt——状态噪声矩阵的协方差矩阵

则观测方程可以表示为：

式中：

Vt——t时刻的观测噪声

p(vt)～N(0,Rt)，Rt——观测噪声协方差矩阵

抗差M估计［17］又称M-LS 抗差估计，是一种基于等价权模型的抗差估计，通过构造等价权，使未知参数估计尽可能避免受到粗误差的影响，为了解决实际环境中的粗误差以及模型误差，本文将抗差M估计与EKF 算法相结合，得到抗差EKF 滤波器，此时卡尔曼增益可以写为：

式中：

(t)——卡尔曼增益

P(t,t-1)——t时刻通过估计得到的误差协方差矩阵

H(t)——t时刻的系统测量矩阵

(t)——观测噪声协方差矩阵

根据文献［18］，IGG3函数表示为

为满足在室内环境下跨楼层定位需求，本文通过使用气压计来解算终端的高度，根据压高公式：

式中：

Ps——气压值。

4 实验结果

本文采用Intel 5300 网卡来采集CSI 信息，所使用的智能手机终端包含有三轴加速度计、磁力计、陀螺仪以及气压计等，满足Wi-Fi与MEMS融合的条件。

为了测试高度对线阵角度估计的影响，本文首先选择室外环境，分别将AP和发射机之间的相对高度设置为0 cm、100 cm、200 cm，得到如表1所示结果。

从表1 中可以看出高度对线阵的角度估计有影响，随着高度的增加误差逐渐增大，继续用L阵进行测试得到的结果如表2所示。

表1 不同高度线阵角度估计误差统计

从表2 中可以看出，增加高度对L阵的角度估计影响不大，在有相对高度时，角度估计的结果要优于线阵。

表2 不同高度L阵角度估计误差统计

随后，本文选择典型的室内环境进行测试，长和宽分别为12.5 和7.5 m，接收机放置高度为2.4 m，角度测试结果如图5所示。

从图5 中可以看出，在相同高度下基于L 阵的角度估计优于线阵，说明高度对线阵方位角估计有影响，本文提出的基于L 阵的超分辨角度估计算法可以很好地解决这个问题。

图5 角度估计结果

本文选择4台AP 在室内环境下进行定位测试，具体的行走轨迹图如图6 所示，定位环境包含LOS 环境以及NLOS 环境，图6 中S1-S2 为LOS 环境（AP 与终端之间存在直射径），S2-S3为NLOS环境，得到的定位结果如图7所示。

图6 室内行走轨迹图

图7 室内定位结果

从图7中定位结果可以看出，在NLOS环境下由于接收机接收不到直射径，造成角度估计精度降低，使得定位结果比LOS 环境下定位结果差，但是当AOA 与MEMS传感器融合以后，克服了NLOS环境下定位精度较低的问题，且较单AOA 定位系统，定位精度明显提升，中值误差在1 m 以内，可以满足室内场景的定位需求。

4 结束语

本文提出一种基于抗差EKF的Wi-Fi/MEMS 融合室内算法，研究了包括改善AOA 估计技术，提出了基于Wi-Fi 的超分辨L 阵AOA 估计算法，提高了角度估计的精度，并联合多个AP 确定移动终端的位置，在此基础上，将Wi-Fi 定位结果与MEMS 信息通过抗差EKF 滤波器进行融合，解决NLOS 环境下无法定位的问题，提高了定位精度。该算法利用CSI 信息进行定位，很容易移植到4G LTE系统以及5G NR系统中。