一种融合地图信息的改进卡尔曼滤波器室内定位跟踪方法与流程

本发明属于室内跟踪定位技术领域，尤其涉及一种融合地图信息的改进卡尔曼滤波器室内定位跟踪方法，可用于覆盖wifi信号的室内环境，通过融合地图信息的改进卡尔曼滤波器室内定位跟踪方法，来优化wi-fi定位跟踪结果。

背景技术：

当前，随着无线网络的发展和无线局域网的广泛部署，基于wifi的室内定位技术受到广泛重视。在覆盖wifi网络的室内环境下，通过测量来自接入点aps的接收信号强度rss，结合采集好的信号强度数据库求解，确定移动用户的位置。实时定位时，由于信号波动、多径干扰、阴影效应及人员的走动等多种因素影响，定位结果常常发生抖动，导致定位效果不佳。为了稳定定位结果，通常会对定位结果进行滤波，使得输出的定位结果更加平滑。

卡尔曼滤波是对随机信号做滤波的算法之一，应用十分广泛，目前在通信、导航、遥感、石油勘探等各个领域都有重要应用。相比于维纳滤波、最小二乘等其他滤波算法，卡尔曼滤波器有如下特点：

(1)在求解时不需要贮存大量的观测数据，并且当得得到新的观测数据时，可随时算得新的参数滤波值，便于实时地处理观测成果。

(2)卡尔曼滤波器是以最小均方误差为原则的最佳线性滤波器，根据前一个估计值和最近一个观察数据来估计信号的当前值，需要已知系统的状态方程和量测方程，它的解是以估计值的形式给出的。

(3)卡尔曼滤波采用实用的递推算法，成功地将状态变量法引入到滤波理论中来，在时域内设计滤波器，且不要求保存过去的测量数据。

(4)被估计量可以是平稳的，也可以是非平稳的，即卡尔曼滤波也适用于非平稳过程。

利用离散卡尔曼滤波器对定位跟踪进行处理是一种优化结果的方法，但是由于方法在长时间平稳移动后会更偏向预测值，一旦发生大幅度偏离原运动方向的情况，就会需要一段时间的纠正过程，不利于跟踪与定位结果。针对此方面，还有不少的优化空间。

综上所述，现有技术存在的问题是：

利用离散卡尔曼滤波器对定位跟踪进行处理是一种优化结果的方法，但是由于方法在长时间平稳移动后会更偏向预测值，一旦发生大幅度偏离原运动方向的情况，就会需要一段时间的纠正过程，不利于跟踪与定位结果。针对此方面，还有不少的优化空间。

技术实现要素：

针对上述问题，本发明的目的在于提供一种融合地图信息的改进卡尔曼滤波器室内定位跟踪方法。

实现本发明目的的技术方案是：融合地图信息信息，遇到拐角时，如果移动终端发生转弯，调整卡尔曼滤波器的参数，以改善其跟踪精度。包括两个阶段：

1)确定基本离散卡尔曼滤波器方程和对应的变量。

1.1)确定状态向量：卡尔曼滤波器的状态向量为：

x＝[sx,vx,sy,vy]^t

其中sx和vx分别表示x方向的位置和速度，sy和vy分别表示y方向的位置和速度；

1.2)确定量测阵：由于用户在定位时只有定位结果，而没有输出速度信息，因此观测量只有sx和sy，从而量测阵为：

其中h为卡尔曼滤波器的量测阵；

1.3)确定状态方程：由于发明定位跟踪系统设定为各个直线段匀速运动，因此状态方程为：

其中sxk，syk，vxk和vyk分别表示k时刻定位终端的位置和速度，sx(k-1)，sy(k-1)，vx(k-1)和vy(k-1)分别表示k-1时刻定位终端的位置和速度；

1.4)确定状态转移矩阵：

其中a表示卡尔曼滤波器的状态转移矩阵，δt表示两次定位之间的时间间隔；

1.5)最小均方误差阵p的初始值的选取没有特别要求，在卡尔曼滤波过程中p矩阵会趋于稳定；

1.6)在卡尔曼滤波器的实际应用时，测量噪声的协方差r一般可以直接观测得到，是滤波器的已知条件。但是激励噪声协方差阵q无法直接得到，可以根据实际情况选取合适的q。

2)结合地图对卡尔曼滤波器跟踪算法进行优化。

2.1)计算第k个时刻卡尔曼滤波器的时间更新方程：

其中为k时刻的预测状态向量，为k-1时刻卡尔曼滤波器输出的状态向量，a为状态转移矩阵，b是控制输入矩阵，uk-1为k-1时刻的驱动输入向量，t表示矩阵的转置；

2.2)计算第k个时刻卡尔曼滤波器的测量更新方程：

其中gk表示第k个时刻的卡尔曼滤波增益，zk为第k个时刻的观测向量，i为单位矩阵；

2.3)如果定位跟踪结束，则退出跟踪算法，否则，执行步骤2.4；

2.4)判断定位终端是否到达拐角附近，如果到达拐角附近，执行步骤2.5，否则，令k的值增加1后回到步骤2.1；

2.5)判断定位终端是否发生转弯，如果发生转弯，执行步骤2.6，否则，令k的值增加1后回到步骤2.1；

2.6)调整卡尔曼滤波器的参数：根据实际情况对卡尔曼滤波器的误差协方差阵p的值进行调整，从而提高定位终端发生转弯时的定位跟踪精度。

2.7)令k的值增加1后回到步骤2.1，继续执行融合地图信息的改进卡尔曼滤波算法，直到定位跟踪结束。

本发明与现有技术相比具有以下优点：

由于本发明结合了地图信息对定位终端是否到达转角处进行判断，并利用速度的角度变化判断出定位终端是否发生转弯，如果发生转弯则自动对卡尔曼滤波器的参数进行调整，从而改进了室内定位跟踪效果，特别是在发生转弯时，有明显的改进作用，达到了提升定位跟踪精度的目的。

附图说明

图1是本发明提供的融合地图信息的改进卡尔曼滤波器室内定位跟踪方法流程图；

图2是误差协方差p和卡尔曼增益gk随着时间的变化示意图；

图3是融合地图信息的卡尔曼滤波算法的跟踪轨迹；

图4是融合地图信息的卡尔曼滤波算法与其他算法结果比较图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

下面将结合附图，对本发明的优选实例进行详细的描述。

在室内走廊的环境下，由于卫星信号无法穿透建筑，因此卫星定位技术目前还不能满足人们对于室内定位的需求。目前基于wifi的室内定位一般是先在不同的坐标点对wifi信号的名称与强度进行检测与记录，形成数据库，然后在定位阶段，用户利用收到的wifi信息对应到具体坐标，从而得到定位结果。利用卡尔曼滤波器可以在连续运动中对定位结果进行修正，对人员进行跟踪。但是由于卡尔曼滤波器的方法在拐角处会产生较大误差，为此，本发明专门提出一种融合地图信息的改进卡尔曼滤波器定位跟踪方法，对定位与跟踪结果进行修正。

实现本发明的技术思路是：融合地图信息信息，遇到拐角时，如果手机发生转弯，调整卡尔曼滤波器的参数，以改善其跟踪精度。其实现方案如下：

1)确定基本离散卡尔曼滤波器方程和对应的变量。

1.1)卡尔曼滤波器的状态向量为：

x＝[sx,vx,sy,vy]^t

其中sx和vx分别表示手机的运动的x方向的位置和速度，sy和vy分别表示移动终端设备的运动的y方向的位置和速度。

1.2)由于用户手持移动终端设备在定位时只有定位结果，而没有输出速度信息，因此观测量只有sx和sy，从而量测阵为：

1.3)由于本发明定位跟踪系统设定为各个直线段匀速运动，因此状态方程为：

1.4)在定位时，前后两次定位间隔为1秒的情况下，δt＝1，此时状态转移矩阵为：

1.5)最小均方误差阵p的初始值的选取没有特别要求，在卡尔曼滤波过程中p矩阵会趋于稳定，在这里取最小均方误差阵p为：

1.6)在卡尔曼滤波器的实际应用时，测量噪声的协方差r一般可以直接观测得到，是滤波器的已知条件。但是激励噪声协方差阵q无法直接得到，因为无法直接观测到过程信号xk。在这里通过q给过程信号“注入”足够的不确定性，建立一个简单的过程模型，从而产生一个可以接受的结果。在q和r都是常量的情况下，过程估计误差协方差阵r和卡尔曼滤波增益kk都会快速收敛，并且保持为常量。在这里，我们取q和r分别为：

2)结合地图对卡尔曼滤波器跟踪算法进行优化。

2.1)计算第k个时刻卡尔曼滤波器的时间更新方程：

其中为k时刻的预测状态向量，为k-1时刻卡尔曼滤波器输出的状态向量，a为状态转移矩阵，b是控制输入矩阵，uk-1为k-1时刻的驱动输入向量，t表示矩阵的转置；

2.2)计算第k个时刻卡尔曼滤波器的测量更新方程：

其中gk表示第k个时刻的卡尔曼滤波增益，zk为第k个时刻的观测向量，i为单位矩阵；

2.3)如果定位跟踪结束，则退出跟踪算法，否则，执行步骤2.4；

2.4)判断定位终端是否到达拐角附近：卡尔曼滤波器每输出一个位置估计都会计算与各个拐角的距离dk，如果dk小于某一个阈值，则认为移动终端到达拐角附近，存在转弯的可能，在本实施例中阈值dk取0.5米。如果到达拐角附近，执行步骤2.5，否则，令k的值增加1后回到步骤2.1；

2.5)判断定位终端是否发生转弯，如果发生转弯，执行步骤2.6，否则，令k的值增加1后回到步骤2.1；判断是否发生转弯的具体步骤如下：

2.5.a)通过卡尔曼滤波器估计得到的x方向和y方向的速度计算出k时刻定位终端的速度方向dirk；

2.5.b)对速度方向进行算数平均滤波；

其中dirk表示k时刻未经过算数平均滤波之前的速度方向，表示算数平均滤波输出的k时刻的速度方向，n表示算数平均滤波器的阶数，阶数越大，滤波得到的速度方向越平滑，但是会导致判别转弯越不敏感，本实施例中取算数平均滤波器的阶数n为2；

2.5.c)判断速度角度的变化：

其中表示卡尔曼滤波器估计得到的速度角度的变化，如果大于某个角度阈值，那么认为定位终端发生了转弯，否则，认为定位终端没有发生转弯。在本实施例中角度阈值取5度。

2.6)调整卡尔曼滤波器的参数：根据实际情况对卡尔曼滤波器的误差协方差阵p的值进行调整，从而提高定位终端发生转弯时的定位跟踪精度。在本实施例中，如果判别到移动设备发生转弯时，令误差协方差矩阵p恢复为初始值：

2.7)令k的值增加1后回到步骤2.1，继续执行融合地图信息的改进卡尔曼滤波算法，直到定位跟踪结束。

用本实施例与现有的标准卡尔曼滤波算法进行定位跟踪的效果对比。

为了验证本发明所提融合地图信息的改进卡尔曼滤波器定位跟踪方法的性能，对所提方法进行了仿真实验，在50x50米的区域内，假设用户手持定位终端以匀速走过带90度转角的轨迹，轨迹的起点坐标为(1.5,0.25)，三个转角的坐标分别为(1.5,48.75)，(48.5,48.75)和(48.5,1.25)，在各个直线段做匀速直线运动。

本发明中误差协方差p和卡尔曼增益gk随着时间的变化如图2所示。从图中可以看出，本发明的方法可以准确的判别出四次转弯，并在每次转弯时通过增大误差协方差p的值来增大卡尔曼增益gk的值，从而使拐角处的卡尔曼滤波输出更加接近于观测向量。

本发明所提融合地图信息的改进卡尔曼滤波器定位跟踪方法轨迹如图3所示。

从图4中可以看出，本发明所提算法的滤波轨迹在拐角处的误差明显减小，跟踪效果明显提升。对各算法的定位跟踪误差进行统计，得到误差的cdf曲线如图所示。经过滤波后的定位跟踪误差大大减小，误差在1米以内的概率达到90％。

本发明所提的融合地图信息的改进卡尔曼滤波器定位跟踪方法虽然整体的定位跟踪精度提高较为有限，但是在拐角处对跟踪效果的改善非常明显，达到了所提定位跟踪方法的目的。

综上，本发明融合地图信息的改进卡尔曼滤波器定位跟踪方法在拐角处有明显的改进效果，提升了定位与跟踪的精度。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。