CN109883423A - 基于卡尔曼滤波的定位方法、系统、设备及存储介质 - Google Patents

Abstract

本发明公开了一种基于卡尔曼滤波的定位方法、装置、设备及计算机可读存储介质，该方法包括：获取智能终端的当前时刻航向角与上一时刻航向角，并根据当前时刻航向角与上一时刻航向角的航向角差值，通过预测更新方程预测当前时刻的状态向量并更新误差协方差矩阵；当未接收到定位模块的定位输出时，根据状态向量，获取智能终端的位置；当接收到定位模块的定位输出时，获取定位模块的定位输出作为测量值，通过测量更新方程更新状态向量和误差协方差矩阵；根据更新后的当前时刻状态向量，获取智能终端的位置，本发明利用差分方向卡尔曼滤波器融合差分方向和定位结果，充分利用方向变化趋势，避免由于方向的估计偏差导致的定位性能损失，提高定位精度。

Description

基于卡尔曼滤波的定位方法、系统、设备及存储介质

技术领域

本发明涉及定位技术领域，尤其涉及一种基于卡尔曼滤波的定位方法、系 统、设备及存储介质。

背景技术

随着手机、平板、智能手表等等具有计算能力的移动终端的快速更新换代 及普及，用户对位置信息以及导航服务的需求越来越强烈。常用的室外定位系 统为GPS，北斗卫星导航系统等全球卫星定位系统。常见的室内定位解决方案 有基于各种无线射频信号定位方案，如蓝牙室内定位、WiFi室内定位、超宽带 室内定位技术、ZigBee室内定位和RFID标签室内定位等等，也有基于声波、 基于LED灯的室内定位方案，以及以上各自技术的结合方案。

目前，室内外定位技术往往和行人航位推算算法(Pedestrian Dead Reckoning，PDR)融合，弥补定位系统可能存在的定位盲区或者定位点跳动问题，以获得 更精确稳定的定位。PDR算法通加速度传感器、陀螺仪、磁力计检测行人步伐， 计算行人的步长和方向，推测出行人运动的轨迹形状，但是手机等消费级智能 终端内置的磁力计，加速度计和陀螺仪的精度有限，且室内环境存在诸多丰富 的地磁干扰，对用户行进方向的判定存在较大误差，导致行人航位推算轨迹失 真，影响定位精度。

发明内容

基于此，本发明提供了一种基于卡尔曼滤波的定位方法、系统、设备及存 储介质，其能有效地避免方向的估计偏差导致的定位性能损失，提高定位精度。

第一方面，本发明实施例提供了一种基于卡尔曼滤波的定位方法，包括以 下步骤：

获取智能终端的当前时刻航向角与上一时刻航向角，并计算所述当前时刻 航向角与所述上一时刻航向角的航向角差值；

根据所述航向角差值，通过预测更新方程预测当前时刻的状态向量并更新 误差协方差矩阵；

当未接收到定位模块的定位输出时，根据所述状态向量，获取智能终端的 位置；

当接收到所述定位模块的定位输出时，获取所述定位模块的定位输出，作 为测量值；

根据所述测量值，通过测量更新方程更新所述状态向量和误差协方差矩阵；

根据更新后的当前时刻的状态向量，获取所述智能终端的位置。

优选地，所述方法还包括：

根据接收到的所述定位模块输出的初始定位坐标、预设的初始速度以及预 设的初始航向角对所述差分方向卡尔曼滤波器进行初始化处理。

优选地，所述根据所述航向角差值，通过预测更新方程预测当前时刻的状 态向量并更新误差协方差矩阵包括：

通过预设的匀速运动模型，更新所述差分方向卡尔曼滤波器的状态转移矩 阵；

根据所述航向角差值，计算所述差分方向卡尔曼滤波器的激励矩阵以及激 励输入值；

通过预设的准静态白噪声模型，更新所述差分方向卡尔曼滤波器的噪声矩 阵；

根据所述状态转移矩阵、所述激励矩阵、所述激励输入值以及所述噪声矩 阵，通过预测更新方程预测当前时刻的状态向量并更新误差协方差矩阵。

优选地，所述预设的匀速运动模型为：

其中，x_k和y_k分别表示智能终端在时刻k时水平坐标系x轴和y轴的坐标， vx_k和vy_k分别表示智能终端在时刻k时水平坐标系x和y轴的速度，dt_k为状态 更新的时间间隔；

所述差分方向卡尔曼滤波器的状态转移矩阵为：

优选地，所述根据所述初始航向角差值，计算所述差分方向卡尔曼滤波器 的激励矩阵以及激励输入值，具体包括：

根据所述初始航向角差值，确定所述差分方向卡尔曼滤波器的差分方向旋 转矩阵；

通过所述差分方向旋转矩阵旋转所述状态向量中的速度矢量，并根据旋转 后的速度矢量以及所述状态向量中的速度矢量，计算所述差分方向卡尔曼滤波 器的激励输入值：

其中，v_k＝[vx_k vy_k]^T，表示状态向量中的速度矢量；表示 旋转后的速度矢量；

根据所述激励输入值，确定匀变速运动下所述激励输入值对所述智能终端 速度和位置的改变量：

根据所述激励输入值对所述智能终端速度和位置的改变量，确定所述差分 方向卡尔曼滤波器的激励矩阵：

优选地，所述预测更新方程为：

其中，和为对k时刻的状态向量和误差协方差矩阵的预测；

所述测量更新方程为：

其中，H表示测量矩阵，R_k表示测量误差协方差矩阵。

优选地，所述方法还包括：

获取加速度计采集到的各轴加速度序列，并对所述各轴加速度序列进行步 伐检测，获得步伐检测结果；

根据所述步伐检测结果，通过定时器对用于获取当前时刻航向角与所述上 一时刻航向角的航向角差值的差分方向模块和用于对所述智能终端进行定位的 定位模块进行时间更新。

第二方面，本发明实施例提供了一种基于卡尔曼滤波的定位系统，包括：

差分方向模块，用于获取智能终端的当前时刻航向角与上一时刻航向角， 并计算所述当前时刻航向角与所述上一时刻航向角的航向角差值；

差分方向卡尔曼滤波器，用于根据所述航向角差值，通过预测更新方程预 测当前时刻的状态向量并更新误差协方差矩阵；

所述差分方向卡尔曼滤波器，用于当未接收到定位模块的定位输出时，根 据所述状态向量，获取智能终端的位置；

所述差分方向卡尔曼滤波器，用于当接收到所述定位模块的定位输出时， 获取所述定位模块的定位输出，作为测量值；

所述差分方向卡尔曼滤波器，用于根据所述测量值，通过测量更新方程更 新所述状态向量和误差协方差矩阵；

所述差分方向卡尔曼滤波器，用于根据更新后的当前时刻的状态向量，获 取所述智能终端的位置。

第三方面，本发明实施例提供了一种室内定位设备，包括处理器、存储器 以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处 理器执行所述计算机程序时实现第一方面所述的基于卡尔曼滤波的定位方法。

第四方面，本发明实施例提供了一种计算机可读存储介质，所述计算机可 读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述 计算机可读存储介质所在设备执行第一方面所述的基于卡尔曼滤波的定位方法。

相对于现有技术，本发明实施例具有如下有益效果：

通过获取智能终端的当前时刻航向角与上一时刻航向角，并计算所述当前 时刻航向角与所述上一时刻航向角的航向角差值；根据所述航向角差值，通过 预测更新方程预测当前时刻的状态向量并更新误差协方差矩阵；当未接收到定 位模块的定位输出时，根据所述状态向量，获取智能终端的位置；当接收到所 述定位模块的定位输出时，获取所述定位模块的定位输出，作为测量值，根据 所述测量值，通过测量更新方程更新所述状态向量和误差协方差矩阵；根据更 新后的当前时刻的状态向量，获取所述智能终端的位置，利用差分方向卡尔曼 滤波器融合差分方向和定位结果，充分利用方向变化趋势，进行迭代预测更新， 避免由于方向的估计偏差导致的定位性能损失，抗干扰能力更强，提高定位精 度。

附图说明

为了更清楚地说明本发明的技术方案，下面将对实施方式中所需要使用的 附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施 方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以 根据这些附图获得其他的附图。

图1是本发明第一实施例提供的基于卡尔曼滤波的定位方法的流程示意图；

图2本发明实施例提供的差分方向模块结构示意图；

图3是本发明第二实施例提供的基于卡尔曼滤波的定位系统的示意框图；

图4是本发明第三实施例提供的基于卡尔曼滤波的定位设备的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清 楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是 全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造 性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参照图1，本发明第一实施例提供了一种基于卡尔曼滤波的定位方法，其 可由室内定位设备来执行，并包括以下步骤：

S11：获取智能终端的当前时刻航向角与上一时刻航向角，并计算所述当前 时刻航向角与所述上一时刻航向角的航向角差值；

在本发明实施例中，所述基于卡尔曼滤波的定位设备可为手机、平板电脑、 笔记本电脑等智能终端，所述室内定位方法可作为其中一个功能模块集成与所 述基于卡尔曼滤波的定位设备上，由所述基于卡尔曼滤波的定位设备来执行。

上述步骤S11通过差分方向模块执行，具体地，如图2所示，所述差分方 向模块包括航向角获取模块、延时器、加法器。通过所述航向角获取模块获取 智能终端的当前时刻航向角与上一时刻航向角，通过延时器和加法器计算当前 时刻航向角与所述上一时刻航向角的航向角差值。需要说明的是，本发明实施 例对偏航角的获取方式不做任何限定。例如所述航向角获取模块可通过以下方 法计算航向角：

当智能终端设有电子罗盘时，所述航向角获取模块直接读取电子罗盘的航 向角。

或者，通过世界坐标系和设备坐标系的重力和磁场北方向的变化关系求解 偏航角、俯仰角和翻滚角。获取磁力计输出的设备坐标系下各轴磁场强度 M_l＝(m_x m_y m_z)，获取加速度计解算得到的重力分量g_l＝(a_x a_y a_z)，其 中，重力可通过对加速度低通滤波得到。世界坐标系下各轴磁场强度为 M_g＝(0 m 0)，重力加速度为g_g＝(0 0 g)，根据所述各轴磁场强度以及 所述重力分量，计算出设备的偏航角，俯仰角和翻滚角分别为yaw、pitch和roll， 通过磁力计、加速度计，实现电子罗盘的功能。

具体计算过程如下：

又或者，通过磁力计、陀螺仪和加速度计计算航向角。具体，通过陀螺仪 和加速度计联合解算重力加速度，然后上述计算过程得到偏航角。

本发明实施例，通过磁力计、陀螺仪、加速度计，计算智能终端的偏航角 差值，即差分方向，并将该差分方向融合差分方向卡尔曼滤波器算法中，用加 速度估计持智能终端的用户的行进方向，不依赖智能终端的方向，降低了对用 户行进方向的估计难度，避免由于长时间估计轨迹导致的累积误差以及对方向 的估计偏差将导致行人航位推算轨迹失真。

S12：根据所述航向角差值，通过预测更新方程预测当前时刻的状态向量并 更新误差协方差矩阵；

常用的融合算法为卡尔曼滤波器(Kalman filtering)，卡尔曼滤波器是一种高效率的自回归滤波器，利用系统状态方程，通过输入的观测数据，对系统状态进 行最优估计的算法，具有低存储，实时响应快，易于实现等优点，在导航、信 号处理、机器人规划和控制等工业界各领域得到广泛应用。通过当前时刻和上 一时刻的航向角差值预测当前时刻的状态并更新误差协方差矩阵。

S13：当未接收到定位模块的定位输出时，根据所述状态向量，获取智能终 端的位置；

S14：当接收到所述定位模块的定位输出时，获取所述定位模块的定位输出， 作为测量值；

在本发明实施例中，通过定位模块对智能终端进行定位。需要说明的是， 本发明实施例对所述智能终端的定位方式不做任何限定。例如，所述定位模块 可为GPS定位模块、北斗等卫星定位导航系统或者蓝牙定位模块、WiFi定位模 块等，通过获取卫星信号，或者应用场景中的蓝牙、WiFi信息进行目标位置定 位。本发明实施例中卫星定位可为飞行时间技术(Time of Flight,TOF)定位算 法；蓝牙定位和WiFi定位方法包括但不限于：邻近节点算法，三边法，指纹场 匹配算法，TOA测距法、TDOA测距法等等。

S15：根据所述测量值，通过测量更新方程更新所述状态向量和误差协方差 矩阵；

所述状态向量是包括坐标以及对应速度的状态向量，例如 s_k＝[x_k,vx_k,y_k,vy_k]^T，其中，(·)^T表示矩阵转置，k表示时刻，x_k和y_k分别为 在k时刻时水平坐标系x轴和y轴的坐标，vx_k和vy_k分别为在k时刻时水平坐 标系x和y轴的速度；通过对定位得到的实际位置坐标对当前时刻的预测状态 和状态误差协方差矩阵进行更新。

S16：根据更新后当前时刻的状态向量，获取所述智能终端的位置。

当获得当前时刻状态向量当前时刻的状态向量后，进一步判断是否接收到 所述智能终端在所述当前时刻的定位输出；若否，则返回步骤S11，并预测到的 当前时刻的状态向量中的位置坐标作为所述智能终端的最终位置；若是，则执 行步骤S14-16，采用所述当前时刻定位得到的实际位置对所述状态向量和状态 的误差协方差矩阵进行更新，并以更新后的当前时刻的状态向量中的位置坐标 为所述智能终端的最终位置。在本发明实施例中，在迭代更新的过程中，不是 每次迭代都要包含测量更新，当没有测量值(即智能终端在当前时刻的位置) 时，可以跳过测量更新，进入下一个迭代。

本发明实施例利用差分方向卡尔曼滤波器融合差分方向和定位结果，利用 差分方向和实际的定位结果对卡尔曼滤波器的预测更新和测量更新，通过不断 迭代更新，充分利用方向变化趋势，避免由于方向的估计偏差导致的定位性能 损失，抗干扰能力更强，提高定位精度。

在一种可选的实施例中，所述方法还包括：

根据接收到的所述定位模块输出的初始定位坐标、预设的初始速度以及预 设的初始航向角对所述差分方向卡尔曼滤波器进行初始化处理。

本发明实施例中，差分方向卡尔曼滤波器的初始化包括状态向量、误差协 方差矩阵、状态转移矩阵、噪声矩阵、误差协方差矩阵等变量的初始化。例如： 初始状态向量的x₀和y₀位置坐标可为定位模块输出的初始定位坐标。需要说明的 是，本发明实施例对初始速度的设定方式不做任何限定。例如，初始速度的初 始化可以通过下述方法设定：将vx₀，vy₀设为0；或者，初始速度大小设定为行 人常见的走速度，如1.3m/s，然后根据初始航向角计算vx₀，vy₀；又或者，根据 行人的身高和加速度数据估计一个初始速度大小，如2m/s，然后根据初始航向 角计算vx₀，vy₀。

其中，状态向量的误差协方差矩阵初始设定为一个对角原始取值较大的矩 阵，让其随着差分方向卡尔曼滤波器的迭代过程收敛，例如，初始误差协方差 矩阵为：

在本实施例中，测量误差协方差矩阵R_k可以由设备自身给出，也可以依据 所述智能终端的位置，通过误差统计得到经验值，以及通过调参设定一个合适 的取值，例如，将测量误差协方差矩阵R设置为：

采用初始定位坐标、预设的初始速度以及预设的初始航向角等参数对状态 转移矩阵、激励矩阵、激励输入值、噪声矩阵、误差协方差矩阵等变量、预测 更新方程以及测量更新方程进行初始化。之后根据上一时刻的状态，用预测更 新方程估计当前时刻的状态，以及在后续过程中利用差分方向和实际的定位结 果对卡尔曼滤波器进行预测更新和测量更新。

在一种可选的实施例中，所述根据所述航向角差值，通过预测更新方程预 测当前时刻的状态向量并更新误差协方差矩阵包括：

通过预设的匀速运动模型，更新所述差分方向卡尔曼滤波器的状态转移矩 阵；

根据所述航向角差值，计算所述差分方向卡尔曼滤波器的激励矩阵以及激 励输入值；

通过预设的准静态白噪声模型，更新所述差分方向卡尔曼滤波器的噪声矩 阵；

根据所述状态转移矩阵、所述激励矩阵、所述激励输入值以及所述噪声矩 阵，通过预测更新方程预测当前时刻的状态向量并更新误差协方差矩阵。

在一种可选的实施例中，所述预设的匀速运动模型为：

其中，x_k和y_k分别表示智能终端在时刻k时水平坐标系x轴和y轴的坐标， vx_k和vy_k分别表示智能终端在时刻k时水平坐标系x和y轴的速度，dt_k为状态 更新的时间间隔；

所述差分方向卡尔曼滤波器的状态转移矩阵为：

其中，H表示测量矩阵，R_k表示测量误差协方差矩阵；

在本实施例中，测量向量为定位模块计算得到的定位坐标，卡尔曼滤波器 初始化时，以初始定位坐标作为测量向量。其中，测量向量 测量矩阵

当有多种定位信息源，得到多种定位结果时，可以相应的扩展测量向量、 测量矩阵和测量误差协方差矩阵，融合多种定位信息结果，根据测量值对状态 和测量误差协方差矩阵进行更新，得到滤波后的定位结果。

在一种可选的实施例中，所述根据所述航向角差值，计算所述差分方向卡 尔曼滤波器的激励矩阵以及激励输入值，具体包括：

根据所述航向角差值，确定所述差分方向卡尔曼滤波器的差分方向旋转矩 阵；

其中，航向角差值，即差分方向为当前时刻方位角θ_k和上一时刻方位角θ_k-1的航向角差值dθ_k＝θ_k-θ_k-1，当方位角为顺时针方向夹角时，所述差分方向 旋转矩阵为：

当方位角为逆时针夹角时，，所述差分方向旋转矩阵为：

通过所述差分方向旋转矩阵旋转所述状态向量中的速度矢量，并根据旋转 后的速度矢量以及所述状态向量中的速度矢量，计算所述差分方向卡尔曼滤波 器的激励输入值：

其中，v_k＝[vx_k vy_k]^T表示状态向量中的速度矢量；表示旋 转后的速度矢量；

根据所述激励输入值，确定匀变速运动下所述激励输入值对所述智能终端 速度和位置的改变量：

根据所述激励输入值对所述智能终端的速度和位置的改变量，确定所述差 分方向卡尔曼滤波器的激励矩阵：

在本实施例中，状态激励项将状态向量中的速度矢量v_k改变为旋转差分角 度的速度同时设定各轴速度变化时加速度不变，此时可以得到公式(4)所 示对速度和位置的改变量，进一步得到公式(5)所示的激励矩阵。

在一种可选的实施例中，所述差分方向卡尔曼滤波器的噪声矩阵为：

其中，为速度白噪声方差。

在本实施例中，采用准静态白噪声模型，即设定在一个时间片段内，噪声 的大小是不变的，同时x和y轴互相独立，则可以得到公式(Ⅳ)所示的噪声矩 阵。需要说明的是，本发明实施例对速度噪声方差的初始化不做任何限定，例 如，速度噪声方差可设定一个固定值，例如，σ＝0.25；或者，将速度噪声方差 初始化为一个较大值，例如，σ₀＝1。速度噪声方差随着差分方向卡尔曼滤波器 的更新和迭代逐步收敛逼近到一个最终值，例如，σ_e＝0.25。收敛的方式可以 为差分方向卡尔曼滤波器每迭代5次，更新一次速度噪声方差，其中， σ_k＝(σ_k-1+σ_e)/2。

在一种可选的实施例中，所述差分方向卡尔曼滤波器的预测更新方程为：

其中，和为对k时刻的状态向量和误差协方差矩阵的预测。

所述分方向卡尔曼滤波器的测量更新方程：

其中，H表示测量矩阵，R_k表示测量误差协方差矩阵。

在一种可选的实施例中，所述方法还包括：

获取加速度计采集到的各轴加速度序列，并对所述各轴加速度序列进行步 伐检测，获得步伐检测结果；

根据所述步伐检测结果，通过定时器对用于获取当前时刻航向角与所述上 一时刻航向角的航向角差值的差分方向模块和用于对所述智能终端进行定位的 定位模块进行时间更新。

在本实施例中，提取用户在行走时加速度计采集到的各轴加速度序列的周 期性特征；对所述周期性特征进行步伐检测，并将步伐检测结果输入定时器。 需要说明的是，本发明实施例对步伐检测方式不做任何限定，例如，可通过过 零点检测，阈值检测等进行步伐检测。

定时器用于同步差分方向模块中的航向角获取模块和定位模块，以驱动差 分方向卡尔曼滤波器更新定位。需要说明的是，本发明实施例对定时器的时间 更新方法不做任何限定，例如，可当接收到步伐检测结果时进行时间更新；或 者，每隔预设时间间隔进行时间更新，例如1秒、2秒；又或者，根据接收到步 伐检测结果，每隔预设时间间隔检测是否存在计步变化，若存在计步变化，则 更新时间；若不存在计步变化，则不更新时间。

通过对持智能终端的用户进行步伐检测，避免定位漂动的现象，进一步提 高定位精度。同时通过定时器同步差分方向模块中的航向角获取模块和定位模 块，可以保证卡尔曼滤波器的每次迭代更新的过程中都包括智能终端的差分方 向和定位结果，进一步提高预测定位的准确性，提高定位精度。

所述卡尔曼滤波器的工作原理如下：

初始化卡尔曼滤波器：根据定位模块的输出和航向角，以及预设的卡尔曼 滤波器的初始参数对卡尔曼滤波器的变量和更新方程进行初始化；

卡尔曼滤波器的迭代，每个迭代获取当前状态s_k(含位置信息，最终输出 的信息)，和状态的误差协方差P_k。每个迭代周期包含预测更新和测量更新。

预测更新：根据上一时刻状态s_k-1，以及状态转移矩阵F_k，以及根据当前 时刻航向角和上一时刻的航向角的差值dθ_k以及上一时刻状态s_k-1得到的激励输 入u_k，用预测更新方程估计当前时刻的状态以及根据上一时刻的状态误差 协方差P_k-1预测当前时刻的状态误差协方差

测量更新：定位模块获得智能终端当前时刻的位置的测量值z_k。根据测量更 新方程更新状态和状态误差协方差没有测量值时，该步骤 可缺省，直接作为s_k，直接作为P_k。

通过不断迭代预测更新和测量更新，当前时刻随着时间的流逝，变成上一 时刻，重新去估计新的当前时刻。

相对于现有技术，本发明实施例具有如下有益效果：

通过获取智能终端的当前时刻航向角与上一时刻航向角，并计算所述当前 时刻航向角与所述上一时刻航向角的航向角差值；根据所述航向角差值，通过 预测更新方程预测当前时刻的状态向量并更新误差协方差矩阵；当未接收到定 位模块的定位输出时，根据所述状态向量，获取智能终端的位置；当接收到所 述定位模块的定位输出时，获取所述定位模块的定位输出，作为测量值，根据 所述测量值，通过测量更新方程更新所述状态向量和误差协方差矩阵；根据更 新后当前时刻的状态向量，获取所述智能终端的位置，利用差分方向卡尔曼滤 波器融合差分方向和定位结果，充分利用方向变化趋势，避免由于方向的估计 偏差和累积误差导致的定位性能损失，提高定位精度，抗干扰能力更强，提升 定位系统的稳定性。

请参阅图3，本发明第二实施例提供了一种基于卡尔曼滤波的定位系统，包 括：

差分方向模块1，用于获取智能终端的当前时刻航向角与上一时刻航向角， 并计算所述当前时刻航向角与所述上一时刻航向角的航向角差值；

差分方向卡尔曼滤波器2，用于根据所述航向角差值，通过预测更新方程预 测当前时刻的状态向量并更新误差协方差矩阵；

所述差分方向卡尔曼滤波器2，用于当未接收到定位模块的定位输出时，根 据所述状态向量，获取智能终端的位置；

所述差分方向卡尔曼滤波器2，用于当接收到所述定位模块的定位输出时， 获取所述定位模块的定位输出，作为测量值；

所述差分方向卡尔曼滤波器2，用于根据所述测量值，通过测量更新方程更 新所述状态向量和误差协方差矩阵；

所述差分方向卡尔曼滤波器2，用于根据更新后当前时刻的状态向量，获取 所述智能终端的位置。

在一种可选的实施例中，

所述差分方向卡尔曼滤波器2，用于根据接收到的所述定位模块输出的初始 定位坐标、预设的初始速度以及预设的初始航向角对所述差分方向卡尔曼滤波 器进行初始化处理。

在一种可选的实施例中，

所述差分方向卡尔曼滤波器2，用于通过预设的匀速运动模型，更新所述差 分方向卡尔曼滤波器的状态转移矩阵；

所述差分方向卡尔曼滤波器2，用于根据所述航向角差值，计算所述差分方 向卡尔曼滤波器的激励矩阵以及激励输入值；

所述差分方向卡尔曼滤波器2，用于通过预设的准静态白噪声模型，更新所 述差分方向卡尔曼滤波器的噪声矩阵；

所述差分方向卡尔曼滤波器2，用于根据所述状态转移矩阵、所述激励矩阵、 所述激励输入值以及所述噪声矩阵，通过预测更新方程预测当前时刻的状态向 量并更新误差协方差矩阵。

在一种可选的实施例中，所述预设的匀速运动模型为：

其中，x_k和y_k分别表示智能终端在时刻k时水平坐标系x轴和y轴的坐标， vx_k和vy_k分别表示智能终端在时刻k时水平坐标系x和y轴的速度，dt_k为状态 更新的时间间隔；

所述差分方向卡尔曼滤波器的状态转移矩阵为：

在一种可选的实施例中，所述差分方向卡尔曼滤波器2，用于根据所述初始 航向角差值，确定所述差分方向卡尔曼滤波器的差分方向旋转矩阵；

所述差分方向卡尔曼滤波器2，用于通过所述差分方向旋转矩阵旋转所述状 态向量中的速度矢量，并根据旋转后的速度矢量以及所述状态向量中的速度矢 量，计算所述差分方向卡尔曼滤波器的激励输入值：

其中，v_k＝[vx_k vy_k]^T表示状态向量中的速度矢量；表示旋 转后的速度矢量；

所述差分方向卡尔曼滤波器2，用于根据所述激励输入值，确定匀变速运动 下的所述智能终端的位移改变量：

所述差分方向卡尔曼滤波器2，用于所述激励输入值对所述智能终端速度和 位置的改变量，确定所述差分方向卡尔曼滤波器的激励矩阵：

在一种可选的实施例中，所述差分方向卡尔曼滤波器的噪声矩阵为：

其中，为速度白噪声方差。

在一种可选的实施例中，所述差分方向卡尔曼滤波器的预测更新方程为：

其中，和为对k时刻的状态向量和误差协方差矩阵的预测；

所述测量更新方程为：

其中，H表示测量矩阵，R_k表示测量误差协方差矩阵；

所述差分方向卡尔曼滤波器2，用于通过所述测量更新方程，获取滤波后的 所述智能终端的最终位置。

在一种可选的实施例中，所述系统还包括：

步伐检测模块4，用于获取加速度计采集到的各轴加速度序列，并对所述各 轴加速度序列进行步伐检测，获得步伐检测结果；

定时器5，用于根据所述步伐检测结果，对用于获取当前时刻航向角与所述 上一时刻航向角的航向角差值的差分方向模块和用于对所述智能终端进行定位 的定位模块进行时间更新。

需说明的是，以上所描述的装置实施例仅仅是示意性的，其中所述作为分 离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件 可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多 个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实 施例方案的目的。另外，本发明提供的装置实施例附图中，模块之间的连接关 系表示它们之间具有通信连接，具体可以实现为一条或多条通信总线或信号线。 本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

参见图4，是本发明第三实施例提供的基于卡尔曼滤波的定位设备的示意图。 如图4所示，该基于卡尔曼滤波的定位设备包括：至少一个处理器11，例如CPU， 至少一个网络接口14或者其他用户接口13，存储器15，至少一个通信总线12， 通信总线12用于实现这些组件之间的连接通信。其中，用户接口13可选的可 以包括USB接口以及其他标准接口、有线接口。网络接口14可选的可以包括 Wi-Fi接口以及其他无线接口。存储器15可能包含高速RAM存储器，也可能 还包括非不稳定的存储器(non-volatilememory)，例如至少一个磁盘存储器。存 储器15可选的可以包含至少一个位于远离前述处理器11的存储装置。

在一些实施方式中，存储器15存储了如下的元素，可执行模块或者数据结 构，或者他们的子集，或者他们的扩展集:

操作系统151，包含各种系统程序，用于实现各种基础业务以及处理基于硬 件的任务；

程序152。

具体地，处理器11用于调用存储器15中存储的程序152，执行上述实施 例所述的室内定位方法，例如图1所示的步骤S11。或者，所述处理器执行所述 计算机程序时实现上述各装置/系统实施例中各模块/单元的功能，例如差分方向 模块。

示例性的，所述计算机程序可以被分割成一个或多个模块/单元，所述一个 或者多个模块/单元被存储在所述存储器中，并由所述处理器执行，以完成本发 明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指 令段，该指令段用于描述所述计算机程序在所述基于卡尔曼滤波的定位设备中 的执行过程。

所述基于卡尔曼滤波的定位设备可以是桌上型计算机、笔记本、掌上电脑 及云端服务器等计算设备。所述基于卡尔曼滤波的定位设备可包括，但不仅限 于，处理器、存储器。本领域技术人员可以理解，所述示意图仅仅是基于卡尔 曼滤波的定位设备的示例，并不构成对基于卡尔曼滤波的定位设备的限定，可 以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件。

所称处理器11可以是中央处理单元(Central Processing Unit，CPU)，还可以 是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成 电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列 (Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者 晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器 也可以是任何常规的处理器等，所述处理器11是所述室内定位设备的控制中心， 利用各种接口和线路连接整个室内定位设备的各个部分。

所述存储器15可用于存储所述计算机程序和/或模块，所述处理器11通过 运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储 器内的数据，实现所述基于卡尔曼滤波的定位设备的各种功能。所述存储器15 可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至 少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储 数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。 此外，存储器15可以包括高速随机存取存储器，还可以包括非易失性存储器， 例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数 字(Secure Digital,SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪 存器件、或其他易失性固态存储器件。

其中，所述基于卡尔曼滤波的定位设备集成的模块/单元如果以软件功能单 元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取 存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流 程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存 储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述 各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计 算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式 等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体 或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储 器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、 电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读 介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减， 例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波 信号和电信信号。

本发明第四实施例提供了一种计算机可读存储介质，所述计算机可读存储 介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计算机 可读存储介质所在设备执行第一实施例所述的基于卡尔曼滤波的定位方法。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技 术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这 些改进和润饰也视为本发明的保护范围。

Claims (10)

1.一种基于卡尔曼滤波的定位方法，其特征在于，包括：

获取智能终端的当前时刻航向角与上一时刻航向角，并计算所述当前时刻航向角与所述上一时刻航向角的航向角差值；

根据所述航向角差值，通过预测更新方程预测当前时刻的状态向量并更新误差协方差矩阵；

当未接收到定位模块的定位输出时，根据所述状态向量，获取智能终端的位置；

当接收到所述定位模块的定位输出时，获取所述定位模块的定位输出，作为测量值；

根据所述测量值，通过测量更新方程更新所述状态向量和误差协方差矩阵；

根据更新后的当前时刻的状态向量，获取所述智能终端的位置。

2.如权利要求1所述的基于卡尔曼滤波的定位方法，其特征在于，所述方法还包括：

根据接收到的所述定位模块输出的初始定位坐标、预设的初始速度以及预设的初始航向角对所述差分方向卡尔曼滤波器进行初始化处理。

3.如权利要求1所述的基于卡尔曼滤波的定位方法，其特征在于，所述根据所述航向角差值，通过预测更新方程预测当前时刻的状态向量并更新误差协方差矩阵包括：

通过预设的匀速运动模型，更新所述差分方向卡尔曼滤波器的状态转移矩阵；

根据所述航向角差值，计算所述差分方向卡尔曼滤波器的激励矩阵以及激励输入值；

通过预设的准静态白噪声模型，更新所述差分方向卡尔曼滤波器的噪声矩阵；

根据所述状态转移矩阵、所述激励矩阵、所述激励输入值以及所述噪声矩阵，通过预测更新方程预测当前时刻的状态向量并更新误差协方差矩阵。

4.如权利要求3所述的基于卡尔曼滤波的定位方法，其特征在于，所述预设的匀速运动模型为：

其中，x_k和y_k分别表示智能终端在时刻k时水平坐标系x轴和y轴的坐标，vx_k和vy_k分别表示智能终端在时刻k时水平坐标系x和y轴的速度，dt_k为状态更新的时间间隔；

所述差分方向卡尔曼滤波器的状态转移矩阵为：

5.如权利要求3所述的基于卡尔曼滤波的定位方法，其特征在于，所述根据所述航向角差值，计算所述差分方向卡尔曼滤波器的激励矩阵以及激励输入值，具体包括：

根据所述航向角差值，确定所述差分方向卡尔曼滤波器的差分方向旋转矩阵；

通过所述差分方向旋转矩阵旋转所述状态向量中的速度矢量，并根据旋转后的速度矢量以及所述状态向量中的速度矢量，计算所述差分方向卡尔曼滤波器的激励输入值：

其中，v_k＝[vx_k vy_k]^T，表示状态向量中的速度矢量；表示旋转后的速度矢量；

根据所述激励输入值，确定匀变速运动下所述激励输入值对所述智能终端速度和位置的改变量：

根据所述激励输入值对所述智能终端的速度和位置的改变量，确定所述差分方向卡尔曼滤波器的激励矩阵：

6.如权利要求1所述的基于卡尔曼滤波的定位方法，其特征在于，所述预测更新方程为：

其中，和为对k时刻的状态向量和误差协方差矩阵的预测；

所述测量更新方程为：

其中，H表示测量矩阵，R_k表示测量误差协方差矩阵。

7.如权利要求1所述的基于卡尔曼滤波的定位方法，其特征在于，所述方法还包括：

获取加速度计采集到的各轴加速度序列，并对所述各轴加速度序列进行步伐检测，获得步伐检测结果；

根据所述步伐检测结果，通过定时器对用于获取当前时刻航向角与所述上一时刻航向角的航向角差值的差分方向模块和用于对所述智能终端进行定位的定位模块进行时间更新。

8.一种基于卡尔曼滤波的定位系统，其特征在于，包括：

差分方向模块，用于获取智能终端的当前时刻航向角与上一时刻航向角，并计算所述当前时刻航向角与所述上一时刻航向角的航向角差值；

差分方向卡尔曼滤波器，用于根据所述航向角差值，通过预测更新方程预测当前时刻的状态向量并更新误差协方差矩阵；

所述差分方向卡尔曼滤波器，用于当未接收到定位模块的定位输出时，根据所述状态向量，获取智能终端的位置；

所述差分方向卡尔曼滤波器，用于当接收到所述定位模块的定位输出时，获取所述定位模块的定位输出，作为测量值；

所述差分方向卡尔曼滤波器，用于根据所述测量值，通过测量更新方程更新所述状态向量和误差协方差矩阵；

所述差分方向卡尔曼滤波器，用于根据更新后的当前时刻的状态向量，获取所述智能终端的位置。

9.一种室内定位设备，其特征在于，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至7中任意一项所述的基于卡尔曼滤波的定位方法。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计算机可读存储介质所在设备执行如权利要求1至7中任意一项所述的基于卡尔曼滤波的定位方法。