面向室内行人导航的KF/EM-KF/FIR混合滤波方法及系统与流程

本发明涉及复杂环境下组合定位，尤其涉及一种面向室内行人导航的kf/em-kf/fir混合滤波方法及系统。

背景技术：

1、本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。

2、目前，行人导航与定位正越来越受到重视，然而，随着行人导航应用范围的扩大，其所面临的导航环境也越发复杂。特别是在室内环境下，建筑物室内布局、建筑材料、甚至空间尺寸都会对导航信号产生影响，进而影响定位精度。与此同时，面向室内环境的行人相对较小的平台使其无法安装部分高精度导航设备。虽然近年来小型导航设备的精度随着导航器件小型化的进步有了一定的提高，但是其性能与传统的大型高精度导航器件相比仍存在差距。

3、在现有的定位方式中，全球卫星导航系统(global navigation satellitesystem，gnss)是最为常用的一种方式。虽然gnss能够通过精度持续稳定的位置信息，但是其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围，特别是在室内、地下巷道等一些密闭的、环境复杂的场景，gnss信号被严重遮挡，无法进行有效的工作。

4、现有技术提出将基于超宽带(ultra-wideband，uwb)的目标跟踪应用于gnss失效环境下的行人导航。这种方式虽然能够实现室内定位，但是由于室内环境复杂多变，uwb信号十分容易受到干扰而导致定位精度下降甚至失锁；与此同时，由于uwb采用的通信技术通常为短距离无线通信技术，因此若想完成大范围的室内目标跟踪定位，需要大量的网络节点共同完成，这必将引入网络组织结构优化设计、多节点多簇网络协同通信等一系列问题。现有的uwb定位技术的精度严重依赖uwb参考节点的位置精度，但在实际应用中这是很难实现的；另一方面，只依赖距离的uwb定位算法对uwb参考节点的精度不准确。因此现阶段基于uwb的目标跟踪在室内导航领域仍旧面临很多挑战。

5、在组合导航策略的基础上，数据融合算法能够进一步提高导航和定位的精度。近年来，卡尔曼滤波(kalman filter,kf)在导航与定位领域已经得到了广泛的应用。例如，wang s等人提出在系统导航中应用一种能够量化不同架构导航性能的kf算法，有效的提高了精度。为了克服组合导航模型的强组合和非线性问题，学者们提出通过泰勒级数展开实现线性化的扩展卡尔曼滤波(extended kf,ekf)，例如，杨元喜院士团队将ekf应用于imu/gps/vo组合导航系统中，显著提升了导航精度。虽然ekf能够降低非线性对组合导航的影响，但是其线性化过程中忽略的高阶量依旧会对组合导航精度产生影响。为了克服这一问题，学者们对现有的ekf算法进行了改进，例如huang y提出了利用迭代ekf(iterativeekf,iekf)算法改进非线性问题。与此同时，学者们提出了无迹卡尔曼滤波(unscented kf,ukf)和cubature卡尔曼滤波(cubature kf,ckf)等解决方案。例如yin x等提出了一种改进的ukf，并将其应用于bds导航中，取得了不错的导航精度；中北大学徐亮面向多auv协同导航对ukf算法进行了改进，提高了算法的鲁棒性。为了提高滤波算法的数据融合精度，学者们也提出对系统噪声统计进行预估。例如huang y提出利用极大期望值算法(expectationmaximization,em)对环境噪声进行预估，进而提高数据融合精度；huang d等人提出了利用em方法实现对系统和测量协方差的在线预估，以减少非线性、非高斯环境噪声对滤波精度的影响。卡尔曼滤波及其改进算法虽然能够实现对组合导航的数据融合，但算法的性能严重依赖于数据融合模型以及噪声估计的精度。但是需要指出的是，ukf、ckf等改进算法的提出虽然有效的提高了算法的鲁棒性，但其解算工作量大，面对精确的高维导航模型和惯性传感器的高频数据采集，这类算法在实时性上存在挑战；另外，传统kf算法在现实环境下由于实际的导航环境噪声不是传统的白噪声，造成kf的性能迅速下降，影响导航精度。

技术实现思路

1、为了解决上述问题，本发明提出了一种面向室内行人导航的kf/em-kf/fir混合滤波方法及系统，能够有效的提高复杂情况下数据融合滤波器的导航预估精度，进而提高整个导航的精度。

2、在一些实施方式中，采用如下技术方案：

3、一种面向室内行人导航的kf/em-kf/fir混合滤波方法，包括：

4、确定目标行人当前时刻k在东-北-天三个方向的运动姿态，所述运动姿态包括剧烈运动姿态和平稳运动姿态；

5、以目标行人k时刻在三个方向的位置误差和速度误差作为混合滤波算法的状态向量，以ins和uwb测量的目标行人k时刻的位置之差作为混合滤波算法的观测向量，对目标行人的位置进行预估；

6、其中，所述混合滤波算法具体为：在平稳运动姿态下，首先利用kf滤波算法的一步预估进行数据融合，对kf滤波算法性能进行评价：若不符合要求，则调用em-kf混合滤波算法对目标行人的位置进行预估，所述em-kf混合滤波算法具体为：在获取到kf滤波算法的一步预估之后，利用em滤波算法进行位置预估；对em-kf混合滤波算法性能进行评价，若不符合要求，则调用fir滤波算法对目标行人的位置进行预估。

7、其中，确定目标行人当前时刻在东-北-天三个方向的运动姿态变化信息，具体为：通过ins获取目标行人当前时刻在东-北-天三个方向的角速度信息，对三个角速度信息求和，得到运动姿态数据，若所述运动姿态数据大于或等于设定的阈值，则目标行人处于剧烈运动姿态；若所述运动姿态数据小于设定的阈值，则目标行人处于平稳运动姿态。

8、进一步地，利用马氏距离对kf滤波算法性能进行评价，若kf滤波算法的马氏距离小于设定的阈值，则继续利用kf滤波算法进行观测更新，得到目标行人的位置预估结果；若kf滤波算法的马氏距离大于设定的阈值，则调用em-kf混合滤波算法对目标行人的位置进行预估，所述em-kf混合滤波算法具体为：在获取到kf滤波算法的一步预估之后，利用em滤波算法进行位置预估。

9、进一步地，还包括：利用马氏距离对em-kf混合滤波算法性能进行评价，若em-kf混合滤波算法的马氏距离小于设定阈值，则继续利用em-kf混合滤波算法进行迭代，直至得到目标行人的位置预估结果；若em-kf混合滤波算法的马氏距离大于设定阈值，则调用fir滤波算法对目标行人的位置进行预估。

10、在另一些实施方式中，采用如下技术方案：

11、一种面向室内行人导航的kf/em-kf/fir混合滤波系统，包括：

12、运动姿态确定模块，用于确定目标行人当前时刻k在东-北-天三个方向的运动姿态，所述运动姿态包括剧烈运动姿态和平稳运动姿态；

13、位置预估模块，用于以目标行人k时刻在三个方向的位置误差和速度误差作为混合滤波算法的状态向量，以ins和uwb测量的目标行人k时刻的位置之差作为混合滤波算法的观测向量，对目标行人的位置进行预估；

14、其中，所述混合滤波算法具体为：在平稳运动姿态下，首先利用kf滤波算法的一步预估进行数据融合，对kf滤波算法性能进行评价：若不符合要求，则调用em-kf混合滤波算法对目标行人的位置进行预估，所述em-kf混合滤波算法具体为：在获取到kf滤波算法的一步预估之后，利用em滤波算法进行位置预估；对em-kf混合滤波算法性能进行评价，若不符合要求，则调用fir滤波算法对目标行人的位置进行预估。

15、在另一些实施方式中，采用如下技术方案：

16、一种终端设备，其包括处理器和存储器，处理器用于实现指令；存储器用于存储多条指令，其特征在于，所述指令适于由处理器加载并执行上述的面向室内行人导航的kf/em-kf/fir混合滤波方法。

17、在另一些实施方式中，采用如下技术方案：

18、一种计算机可读存储介质，其中存储有多条指令，所述指令适于由终端设备的处理器加载并执行上述的面向室内行人导航的kf/em-kf/fir混合滤波方法。

19、与现有技术相比，本发明的有益效果是：

20、(1)本发明首先通过对人体姿态的判断，先用kf滤波，用马氏距离判断效果，如果不好，就用em-kf滤波，再用马氏距离判断效果，如果不好就用fir滤波，选取最优的滤波方式完成对目标行人对位置变化小于设定阈值的方向进行平滑，能够有效的提高局部方向处于静止状态下的导航预估的精度，进而提高整个导航的精度。

21、本发明的其他特征和附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本方面的实践了解到。