基于IEZ框架下的多算法增强的室内导航系统及方法与流程

本发明属于导航技术领域，具体涉及一种基于iez框架下的多算法增强的室内导航系统及方法；其借助墙壁走向、磁力分布等多种外界导航因素和惯性自主导航结合辅助导航方法，实现不依赖导航卫星下的连续、精确定位的室内导航算法。

背景技术：

随着gps的发展，现如今打造的gps导航系统可以向全球各地提供全时段，高精度的三维位置等信息的无线电导航定位系统。然而，在深山密林或者城市室内这些环境中，接收的gps信号较弱或者缺失，这样用户将无法依靠gps完成对自己当前位置的定位，以及前往目的地的导航规划。

因此，在卫星信号被密林或建筑物遮挡这些特殊环境下，便可以通过人员自主定位技术进行“补盲”。而惯性导航是一种自主性导航，通过固定于载体上的惯性测量器件，测得载体的角速度信息和加速度信息，并通过积分运算后求得载体的速度与位置信息，从而完成对载体的导航定位功能。由于惯性器件的复杂结构与高昂成本，限制了相关应用的发展。

随着微机电(mems)技术的发展，mems惯性器件逐渐取代了传统惯性器件，成为较为热门的研究对象。而基于mems的惯性测量单元(imu)因其低廉的成本，小型化体积，功耗低以及极强的环境适用能力等优点，被广泛地用于军用，民用产业之中。因而使用基于mems惯性技术为核心的自主导航，便于安装与携带，自主性强，适用于各种野外荒地。并且其不依赖任何外部信息，也不向外辐射能量，具有较高的隐蔽性，也适用于特战，单兵潜入作战等方式。

基于mems的惯性测量单元也存在一些缺陷，虽然在短程内具有较高的精度，然而随着时间的增长，因为积分运算以及测量元件的漂移，即使初始化修正完成，之后的误差仍然十分严重，经验上来看，一般位置误差与操作时间的立方成正比。

使用有关系统动力学信息以及对导航信息的约束可以减少立方误差的增长。通常是利用静止时速度与姿态恒定，以此来限制误差的增长，比如零速度更新算法、零角速率更新算法等。也可以借助使用建筑的先验信息，给行人导航提供约束，同样可以限制误差的增长，最终使得导航精度得到保证。

技术实现要素：

针对于上述现有技术的不足，本发明的目的在于提供一种基于iez框架下的多算法增强的室内导航系统及方法，以解决现有技术中gps信号恶劣的环境下，即室内情况下无gps信号或者信号不好的状态下行人自主导航的问题。本发明考虑适当利用行人所在环境提供的余度信息减小自主导航本身精度不足带来的影响，提高室内导航的精度。

为达到上述目的，本发明采用的技术方案如下：

本发明的一种基于iez框架下的多算法增强的室内导航系统，包含：数据测量模块、数据解算模块、姿态行为分析模块、多算法处理模块及数据输出模块；其中，

数据测量模块：六轴惯性测量单元、三轴磁力计和气压计组成的传感器组，采集每个时刻行人的导航数据；

数据解算模块，对传感器组采集到的导航数据进行惯导解算得到行人状态向量；

姿态行为分析模块，对传感器组采集到的导航数据进行处理、判断，得出行人的运动模式；

多算法处理模块，基于数据解算模块解算出的行人状态向量以及姿态行为分析模块得出行人的运动模式，通过多种增强算法并行处理得到行人状态误差向量并输入到扩展卡尔曼滤波器，得到行人状态误差向量的最优估计；

数据输出模块，用于将行人状态误差向量的最优估计与数据解算模块解算出的行人状态向量进行数据融合校准，得到最优的导航信息并输出。

优选地，所述姿态行为分析模块包含：静止状态检测模块和行为模式分析模块；其中，静止状态检测模块包含的三组逻辑判断标准，对六轴惯性测量单元测量得到的加速度和角速度数据进行逻辑判断，确定行人静止或运动状态；行为模式分析模块利用数据解算模块得到的行人状态向量对行人每一步的步伐长度和步伐方向进行计算，再通过行为模式分析模块内部的三组逻辑判断标准对行人的步伐长度和步伐方向进行判断，确定行人的运动模式；检测到行人静止时，静止状态检测模块触发零速度更新算法、零角速率更新算法、零粗加速度更新算法；检测到行人处于运动状态时，计算出行人步伐长度和步伐方向，并结合建筑物墙体走向，判断并得出行人的运动模式，行为模式分析模块触发改进启发式漂移消除算法。

优选地，所述零速度更新算法(zupt算法)，将速度误差输出至扩展卡尔曼滤波器，获得速度误差状态量的最优估计；

所述零角速率更新算法(zaru算法)，将角速率误差输出至扩展卡尔曼滤波器，获得角速率误差状态量的最优估计；

所述零粗加速度更新算法(zuna算法)，将加速度误差输出至扩展卡尔曼滤波器，获得加速度误差状态量的最优估计；

所述改进启发式漂移消除算法(ihde算法)，测量计算得到的步幅方向和步幅大小数据经逻辑判断后输出结果，然后按照逻辑判断结果对行人运动方位角进行处理并将方位角误差输入到扩展卡尔曼滤波器，获得行人运动方位角误差的最优估计。

优选地，所述多算法处理模块具体包括：

多算法处理模块分为两部分，一部分是由零速度更新算法、零角速率更新算法、零粗加速度更新算法组成，这些算法在静止状态检测模块逻辑判断行人处于静止状态时对行人状态误差向量的速度误差分量、角速率误差分量、加速度误差分量提取处理并输入至扩展卡尔曼滤波器中；另一部分由启发式漂移消除算法组成，当静止状态检测模块逻辑判断行人处于运动状态时，触发行为模式分析模块，行为模式分析模块利用数据解算模块得到的行人状态向量对行人每一步的步伐长度和步伐方向进行计算，再通过行为模式分析模块内部的逻辑判断对行人的步伐长度和步伐方向进行判断确定行人的运动模式，触发启发式漂移消除算法，该算法根据行人的多种运动模式，选择对应的处理方法对行人状态误差向量的航向角误差分量进行提取处理并输入至扩展卡尔曼滤波器中。

优选地，所述行人状态向量、行人状态误差向量、三轴磁力计测量值和气压计的测量值具体如下：

(1)行人状态向量为：

其中，分别为惯性测量单元测量的载体的x轴、y轴、z轴三个方向上的姿态角；px、py、pz分别为x轴、y轴、z轴的方向上的位置；vx、vy、vz分别为x轴、y轴、z轴的三个方向上的速度；wx、wy、wz分别为x轴、y轴、z轴的角速度；分别为x轴、y轴、z轴的加速度；

简写为；

其中，为惯性测量单元测量的载体的姿态角；p为位置；v为速度；w为角速度；为加速度；[·]^t表示矩阵的转置；

(2)行人状态误差向量：

其中，分别为惯性测量单元测量的载体的x轴、y轴、z轴的方向上的姿态角误差；δpx、δpy、δpz分别为x轴、y轴、z轴的方向上的位置误差；δvx、δvy、δvz分别x轴、y轴、z轴的方向上的速度误差；δwx、δwy、δwz分别为x轴、y轴、z轴的角速度误差；分别为x轴、y轴、z轴的加速度计零位误差；

简写为：

其中，为惯性测量单元测量的载体的姿态角误差；δp为位置误差；δv为速度误差；w为角速度；为加速度零位误差；

(3)三轴磁力计测量值为：

xmg＝[ψmgxψmgyψmgz]

其中，ψmgx、ψmgy、ψmgz为传感器运动方向上的磁分量、水平方向上与运动方向垂直的右手分量和垂向分量；

(4)气压计的测量值为：

xb＝δhb

其中，δhb为气压计的高度测量误差。

本发明的一种基于iez框架下的多算法增强的室内导航方法，包括步骤如下：

1)采集每个时刻行人的导航数据；

2)对上述采集到的导航数据进行惯导解算得到行人状态向量；

3)对采集到的数据进行分析得出行人的运动模式；

4)通过多种增强算法并行处理得到行人状态误差向量并输入到扩展卡尔曼滤波器，得到行人状态误差向量的最优估计；

5)将行人状态误差向量的最优估计与解算出的行人状态向量进行数据融合校准，得到最优的导航信息并输出；

6)针对多种导航环境，借助三轴磁力计和气压计设计了基于多种导航环境下的算法调整方案，实时监控导航环境，切换增强算法使用方案确保最优的导航精度和可靠性。

优选的，所述步骤4)具体包含：基于扩展卡尔曼滤波器建立的状态方程和量测方程，具体为：

其中，φk为状态转移矩阵；δxk、δx′k-1分别为采样时间k下估计状态误差矩阵和采样时间k-1下纠正后的状态误差估计矩阵；wk-1为系统过程白噪声矢量；h为系统量测矩阵；vk为系统噪声量测矢量；zk为观测误差量；

系统量测矩阵h由多个量测子矩阵组成；系统量测矩阵h以及观测量zk定义如下：

其中，为第k个采样时间下的角度误差观测矩阵；为第k个采样时间下的位置误差观测矩阵；为第k个采样时间下的速度误差观测矩阵；为第k个采样时间下的角速度误差观测矩阵；为第k个采样时间下的加速度测量误差观测矩阵；h^θ根据行人状态误差向量的角度误差分量定义，为系统量测矩阵h的子矩阵；h^p根据行人状态误差向量的位置误差分量定义，为系统量测矩阵h的子矩阵；h^z根据行人状态误差向量的速度误差分量定义，为系统量测矩阵h的子矩阵；h^r根据行人状态误差向量的角速度误差分量定义，为系统量测矩阵h的子矩阵；hⁿ根据行人状态误差向量的加速度误差分量定义，为系统量测矩阵h的子矩阵；

最终更新后的观测量和系统量测矩阵组合输入到扩展卡尔曼滤波器，经过滤波后的行人状态误差向量与惯导解算后的粗精度数据相融合最终得到行人状态向量的最优估计。

优选的，所述步骤6)中的基于多种导航环境下的算法调整方案具体为：

在正常对导航数据进行读取和导航解算后，增加了对导航环境进行判定的环节，确认是否有电磁干扰和气压等异常；由发生异常情况选择对应最优增强算法，最大化限度地提高导航精度；实时监控导航环境，切换最优状态以确保整体的导航精度和可靠性。

本发明的有益效果：

1.本发明通过设计姿态行为分析模块，可以判断行人的运动模式：行人是运动或者静止；行人是否沿着直线行走；行人行走方向是否与建筑物墙壁走向一致。

2.本发明使用多种增强算法，对目标的多个导航参数进行约束，从而提高导航精度。

3.本发明基于行人的运动模式，调整选择最佳处理方法，降低增强算法在行人运动模式变化后造成的测量误差，减小负面影响。

4.本发明将观测矩阵和量测矩阵分解为多个子矩阵，并与各个增强算法相对应。当某个增强算法满足条件，则对应子矩阵进行更新，从而增强导航算法的鲁棒性、可靠性和容错性能。

5.本发明针对多种导航环境，设置多增强算法调整的方案设计来选择最优增强算法，改善了增强算法使用策略，可以灵活选择各种增强算法，最大化限度地提高导航精度。并且实时监控导航环境，切换增强算法确保最优的导航精度和可靠性。

附图说明

图1是本发明方法的原理图；

图2是本发明方法的任务流程图；

图3是本发明的静止状态检测的逻辑判断图；

图4是本发明的行为模式分析的模块流程图；

图5是本发明的基于多种导航环境下的算法调整方案设计流程图。

具体实施方式

为了便于本领域技术人员的理解，下面结合实施例与附图对本发明作进一步的说明，实施方式提及的内容并非对本发明的限定。

本发明的一种基于iez框架下的多算法增强的室内导航系统，包含：数据测量模块、数据解算模块、姿态行为分析模块、多算法处理模块及数据输出模块；其中，

数据测量模块：六轴惯性测量单元、三轴磁力计和气压计组成的传感器组，采集每个时刻行人的导航数据；

数据解算模块，对传感器组采集到的导航数据进行惯导解算得到行人状态向量；

姿态行为分析模块，对传感器组采集到的导航数据进行处理、判断，得出行人的运动模式；

多算法处理模块，基于数据解算模块解算出的行人状态向量以及姿态行为分析模块得出行人的运动模式，通过多种增强算法并行处理得到行人状态误差向量并输入到扩展卡尔曼滤波器，得到行人状态误差向量的最优估计；

数据输出模块，用于将状态误差向量的最优估计与数据解算模块解算出的行人状态向量进行数据融合校准，得到最优的导航信息并输出。

其中，所述姿态行为分析模块包含：静止状态检测模块和行为模式分析模块；其中，静止状态检测模块包含的三组逻辑判断标准，对六轴惯性测量单元测量得到的加速度和角速度数据进行逻辑判断，确定行人静止或运动状态；行为模式分析模块利用数据解算模块得到的行人状态向量对行人每一步的步伐长度和步伐方向进行计算，再通过行为模式分析模块内部的三组逻辑判断标准对行人的步伐长度和步伐方向进行判断，确定行人的运动模式；检测到行人静止时，静止状态检测模块触发零速度更新算法、零角速率更新算法、零粗加速度更新算法；检测到行人处于运动状态时，计算出行人步伐长度和步伐方向，并结合建筑物墙体走向，判断并得出行人的运动模式，行为模式分析模块触发改进启发式漂移消除算法。

零速度更新算法(zupt算法)，将速度误差输出至扩展卡尔曼滤波器，获得速度误差状态量的最优估计；

零角速率更新算法(zaru算法)，将角速率误差输出至扩展卡尔曼滤波器，获得角速率误差状态量的最优估计；

零粗加速度更新算法(zuna算法)，将加速度误差输出至扩展卡尔曼滤波器，获得加速度误差状态量的最优估计；

改进启发式漂移消除算法(ihde算法)，测量计算得到的步幅方向和步幅大小数据经逻辑判断后输出结果，然后按照逻辑判断结果对行人运动方位角进行处理并将方位角误差输入到扩展卡尔曼滤波器，获得行人运动方位角误差的最优估计。

所述静止状态检测模块中包含的三组评判标准如下：

1)加速度大小：

分别为第k个采样时间下x轴、y轴、z轴的加速度；为第k个采样时间下加速度大小；

设定阈值thmin、thmax进行逻辑判断，则

当加速度大小在设定的阈值内时,则第一组逻辑判断结果c1为1。

2)加速度方差：

定义窗口大小s，则局部加速度方差为

设定阈值thσ进行逻辑判断，则

当加速度方差大于等于设定的阈值时,则第二组逻辑判断结果c2为1。

3)角速度大小：

分别为第k个采样时间下x轴、y轴、z轴的角速度；wk为第k个采样时间下角速度大小；

设定阈值thwmax进行逻辑判断，则

当角速度大小小于等于设定的阈值时,则第三组逻辑判断结果c3为1。

当三组评判标准独立判断后，通过与运算融合三组判断结果得出结论。当与运算结果为1时，判断行人处于静止状态，否则行人处于运动状态。

所述多算法处理模块具体包括：一部分是由零速度更新算法、零角速率更新算法、零粗加速度更新算法组成，这些算法在静止状态检测模块逻辑判断行人处于静止状态时对行人状态误差向量的速度误差分量、角速率误差分量、加速度误差分量提取处理并输入至扩展卡尔曼滤波器中；另一部分由启发式漂移消除算法组成，当静止状态检测模块逻辑判断行人处于运动状态时，触发行为模式分析模块，行为模式分析模块利用数据解算模块得到的行人状态向量对行人每一步的步伐长度和步伐方向进行计算，再通过行为模式分析模块内部的逻辑判断对行人的步伐长度和步伐方向进行判断确定行人的运动模式，触发启发式漂移消除算法，该算法根据行人的多种运动模式，选择对应的处理方法对行人状态误差向量的航向角误差分量进行提取处理并输入至扩展卡尔曼滤波器中。

参照图1、图2所示，本发明的一种基于iez框架下的多算法增强的室内导航方法，包括步骤如下：

采集每个时刻行人的导航数据：六轴惯性测量单元、三轴磁力计和气压计组成的传感器组收集每个时刻行人的运动数据；

对上述采集到的导航数据进行惯导解算：将采集到的行人运动数据进行初步解算得到行人的速度、位置等粗精度导航数据；

姿态行为分析任务包含静止状态检测部分和行为模式分析部分，首先进行静止状态检测，具体如图3所示。静止状态检测部分包含三组评判标准，各自独立判定后再结合三组判断结果得出结论，判断行人是否处于静止状态；若行人判定为静止状态的时候，触发零速度更新算法、零角速率更新算法、零粗加速度更新算法；同时对静止时间进行一个判定，当静止时间超过阈值5s，则表明行人处于完全静止状态，则锁定当前的位置和姿态；若行人处于运动状态，则启用行为模式分析部分，计算出行人步伐长度和步伐方向，并结合建筑物墙体走向，判断并得出行人的运动模式。逻辑判断内容如图4所示，触发改进启发式漂移消除算法；解算滤波任务中，将处理后的行人状态误差向量通过扩展卡尔曼滤波器，得到行人状态误差向量的最优估计；校准输出任务负责将行人状态误差向量的最优估计与行人状态向量进行数据融合校准，得到最优的导航信息并输出。

实时监控导航环境、对导航环境进行判定，确认是否有电磁干扰和气压异常。由异常情况选择对应最优增强算法，最大化限度地提高导航精度，确保最优的导航精度和可靠性。

运动状态判定具体如下：

运动状态判定包含两类数据计算用于逻辑判断；

(11)步伐方向，计算当前行人移动时的方向；

其中，表示在第k个采样时间下行人步伐的方向；表示第k个采样时间下行人所处平面位置的y轴坐标；表示第k个采样时间下行人所处平面位置的x轴坐标；

(12)步伐长度，计算当前行人行走一步的长度；

其中，slk表示在第k个采样时间下行人步伐的长度；表示第k个采样时间下行人所处平面位置的y轴坐标；表示第k个采样时间下行人所处平面位置的x轴坐标。

运动状态的逻辑判断如下：

(21)判断行走步伐大小

防止行走步伐较小的时候触发算法造成测量误差，对系统产生负面影响，给出阈值thsl进行逻辑判断：

(22)判断是否为直线行走

统计最近五次具有相似方向的步幅，将行走轨迹进行归类；并给出阈值thθ进行逻辑判断：

其中，表示在第k个采样时间下行人步伐的方向；thθ为角度阈值；wsk表示在第k个采样时间下的逻辑判断值，当所统计的步伐方向都满足约束，则wsk值为1，表示行人行走为直线，否则为曲线。

(23)判断是否沿着墙体方向运动

建筑墙体的主要方向为将每一步步伐方向与之比较：

其中，表示在第j个采样时间下行人步伐的方向；δθk表示在第k个采样时间下行人步伐方向与建筑墙体的主要方向之间的角度误差量；

并给出阈值thδθ进行逻辑判断：

其中，mdk表示在第k个采样时间下的逻辑判断值，当行人步伐方向与建筑墙体主要方向之间的角度误差量小于阈值约束角度时，mdk值为1，表示行人沿着墙体方向行走。

改进启发式漂移消除算法具体如下：

(31)行人为曲线行走时

即wsk＝0，此时算法不输出任何信息，保证滤波后的状态不受影响；

(32)行人不沿着墙壁方向直线行走时

即wsk＝1,mdk＝0,ssk＝1，此时将当前采样时间k下的步伐方向与采样时间k-4下步伐方向做差作为滤波器的观测值输入到扩展卡尔曼滤波器中；

(33)行人沿着墙壁方向行走时

即wsk＝1,mdk＝1,ssk＝1，此时将采样时间下行人步伐方向与建筑墙体的主要方向之间的角度误差量δθk作为滤波器的观测值输入到扩展卡尔曼滤波器中。

通过多种增强算法并行处理输入到扩展卡尔曼滤波器，得到行人状态误差向量的最优估计；

多种增强算法具体如下：

zupt算法，即零速度更新算法，用于对速度误差状态量的最优估计。在行人处于静止运动状态时，理想状态下的速度测量值理应保持为零，实际情况下由于惯性测量模块自身存在的漂移以及测量误差，导致在静止运动状态下行人的速度测量值并不为零。将速度误差输出至扩展卡尔曼滤波器，获得速度误差状态量的最优估计。

zaru算法，即零角速率更新算法，用于对角速度误差状态量的最优估计。在行人处于静止运动状态时，理想状态下陀螺仪测量的角速度应该保持为零，实际情况下由于陀螺自身存在的漂移误差以及测量误差，导致真实测量下角速度不为零。将角速度误差输出至扩展卡尔曼滤波器，获得角速度误差状态量的最优估计。

zuna算法，即零粗加速度更新算法，用于对加速度误差状态量的最优估计。在行人处于静止运动状态时，理想状态下的加速度测量值理应等于起始点处行人处于静止状态时的平均加速度，实际情况下由于惯性测量模块自身存在的漂移以及测量误差，导致在静止运动状态下行人的加速度测量值并不等于起始处的平均加速度。将加速度误差输出至扩展卡尔曼滤波器，获得加速度误差状态量的最优估计。

ihde算法，即改进启发式漂移消除算法，用于对行人运动方位角误差的最优估计。算法包含逻辑判断模块和处理模块；其中测量计算得到的步幅方向和步幅大小数据经由逻辑判断模块判断输出结果，然后按照逻辑判断结果对行人运动方位角进行处理并将方位角误差输入到扩展卡尔曼滤波器，获得行人运动方位角误差的最优估计；该算法通过分析行人的运动状态并且适当借助建筑物的墙体走向来修正行人的运动状态。

以上算法分别影响着观测矩阵和系统量测矩阵，当满足触发的条件，相应算法便进行对应更新，未被触发的算法对观测矩阵和系统量测矩阵无影响。

定义行人状态向量、行人状态误差向量、三轴磁力计的测量值和气压计的测量值具体如下：

(41)行人状态向量为：

其中，分别为惯性测量单元测量的载体的x轴、y轴、z轴三个方向上的姿态角；px、py、pz分别为x轴、y轴、z轴的方向上的位置；vx、vy、vz分别为x轴、y轴、z轴的三个方向上的速度；wx、wy、wz分别为x轴、y轴、z轴的角速度；分别为x轴、y轴、z轴的加速度；

简写为；

其中，为惯性测量单元测量的载体的姿态角；p为位置；v为速度；w为角速度；为加速度；[·]^t表示矩阵的转置；

(42)行人状态误差向量：

其中，分别为惯性测量单元测量的载体的x轴、y轴、z轴的方向上的姿态角误差；δpx、δpy、pz分别为x轴、y轴、z轴的方向上的位置误差；δvx、δvy、vz分别x轴、y轴、z轴的方向上的速度误差；δwx、δwy、δwz分别为x轴、y轴、z轴的角速度误差；分别为x轴、y轴、z轴的加速度计零位误差；

简写为：

其中，为惯性测量单元测量的载体的姿态角误差；δp为位置误差；δv为速度误差；w为角速度；为加速度零位误差。

(43)三轴磁力计的测量值为：

xmg＝[ψmgxψmgyψmgz]

其中，ψmgx、ψmgy、ψmgz为传感器运动方向上的磁分量、水平方向上与运动方向相互垂直的右手分量和垂向分量；

(44)气压计的测量值为：

xb＝δhb

其中，δhb为气压计的高度测量误差。

下面建立各增强算法更新下的观测矩阵和系统量测矩阵：

(51)zupt零速更新算法

其中，为第k个采样时间下传感器测量得到的速度测量值；为第k个采样时间下的速度误差观测矩阵，即行人在静止状态下传感器测量得到的速度测量矩阵与零矩阵的差值，在实际工作中，这个值虽然很小但是不等于零；h^z根据行人状态误差向量的速度误差分量定义，为系统量测矩阵h的子矩阵。

(52)zaru零角速率更新算法

其中，为第k个采样时间下三个正交陀螺仪传感器的测量值；为第k个采样时间下的角速度误差观测矩阵，即行人在静止状态下三个正交陀螺仪传感器的测量矩阵与零矩阵的差值，在实际工作中，这个值虽然很小但是不等于零；h^r根据行人状态误差向量的角速度误差分量定义，为系统量测矩阵h的子矩阵。

(53)zuna零粗加速度更新算法

其中，为起始点处行人处于静止状态时3秒内的平均加速度值；为第k个采样时间下三个正交加速度传感器的测量值；为第k个采样时间下的加速度测量误差观测矩阵，即行人在静止状态下加速度传感器的测量值与初始测量加速度值的差值；hⁿ根据行人状态误差向量的加速度误差分量定义，为系统量测矩阵h的子矩阵。

(54)ihde改进启发式漂移消除算法：

(541)行人曲线行走时

即wsk＝0，此时算法不输出任何信息，保证滤波后的状态不受影响；

(542)行人不沿着墙壁方向直线行走时

即wsk＝1,mdk＝0，此时将当前采样时间k下的步伐方向与采样时间k-4下步伐方向做差作为滤波器的观测值输入到扩展卡尔曼滤波器中；

a为转移矩阵的第三行向量；δt为两次采样间的时间差；hi根据行人状态误差向量的角度误差分量定义，为系统量测矩阵h的子矩阵。

(543)行人沿着墙壁方向行走时

即wsk＝1,mdk1，此时将采样时间下行人步伐方向与建筑墙体的主要方向之间的角度误差量δθk作为滤波器的观测值输入到扩展卡尔曼滤波器中；

hⁱ根据行人状态误差向量的角度误差分量定义，为系统量测矩阵h的子矩阵。为第k个采样时间下的角度误差观测矩阵。

基于多种导航环境下的算法调整方案具体方法为：

在正常对导航数据进行读取和导航解算后，增加了对导航环境进行判定的环节，确认是否有电磁干扰和气压等异常。由发生异常情况选择对应最优增强算法，最大化限度地提高导航精度。实时监控导航环境，切换最优状态以确保整体的导航精度和可靠性。

在磁清洁环境中几公里范围内区域磁场强度以及磁倾角几乎保持不变，而磁航向角可以通过三维磁力计测量计算出来，如下：

其中，为第k个采样时间下水平方向上与传感器运动方向相互垂直的磁分量，为第k个采样时间下垂直向磁分量；再将磁航向角中减去磁偏角δ获得行人的真实航向

则行人的航向角误差为：

为第k个采样时间下的航向角误差。

但是在存在铁磁材料区域时，比如室内，磁力计将会受到严重影响。因此将磁力计测量到的磁场强度以及计算得到的倾角与参考值进行比较，即将行人从起始到静止期间的数值进行比较，可以轻松检测磁力计读数是否存在异常。通过磁异常检测，可以确保磁力计数据没有失真，保证行人航向数据不受污染。

由于气压值与温度和高度保持着比例关系，所以理论上行人在上下楼梯或者在楼道上行走时，气压值处于一个小幅度的变化状态。但是如果附近存在火源或者空调，气压值波动相对较大，当波动超过设定阈值，则气压检测出现异常。

(611)当行人正常在楼道行走时，未出现气压和磁异常检测，则利用磁力计和气压计测量数据辅助零速度更新算法、零角速率更新算法、零粗加速度更新算法对行人导航数据进行校准更新，如图5所示。

则系统观测矩阵zk

则系统量测矩阵h

hz根据行人状态误差向量的速度误差分量定义，为系统量测矩阵h的子矩阵；h^r根据行人状态误差向量的角速度误差分量定义，为系统量测矩阵h的子矩阵。

(612)由于铁磁材料分布的原因，会出现磁异常检测，则利用改进启发式漂移消除算法搭配气压计测量数据以及零速度更新算法、零角速率更新算法、零粗加速度更新算法对行人导航数据进行校准更新。

则系统观测矩阵zk

则系统量测矩阵h

hⁱ根据行人状态误差向量的角度误差分量定义，为系统量测矩阵h的子矩阵。当判断行人为直线行走时，系统量测矩阵h：

当判断行人为曲线行走时，系统量测矩阵h：

(621)当出现气压检测异常时，首先通过气压变化均值判断是否处于平地。当处于平地时，对导航数据的高度分量做一个约束，并利用磁力计数据辅助零速度更新算法、零角速率更新算法、零粗加速度更新算法对行人导航数据进行校准更新。

则系统观测矩阵zk

则系统量测矩阵h

(622)若磁异常检测，则利用改进启发式漂移消除算法搭配高度辅助以及零速度更新算法、零角速率更新算法、零粗加速度更新算法对行人导航数据进行校准更新。

则系统观测矩阵zk

则系统量测矩阵h

hⁱ根据行人状态误差向量的角度误差分量定义，为系统量测矩阵h的子矩阵。当判断行人为直线行走时，系统量测矩阵h：

当判断行人为曲线行走时，系统量测矩阵h：

(631)出现气压检测异常且判断行人未处于平地，则利用磁力计数据辅助零速度更新算法、零角速率更新算法、零粗速度更新算法对行人导航数据进行校准更新。

则系统观测矩阵zk

则系统量测矩阵h

(632)若磁异常检测，则利用改进启发式漂移消除算法搭配零速度更新算法、零角速率更新算法、零粗速度更新算法对行人导航数据进行校准更新，最大化限度地提高导航精度。

则系统观测矩阵zk

则系统量测矩阵h

hⁱ根据行人状态误差向量的角度误差分量定义，为系统量测矩阵h的子矩阵。当判断行人为直线行走时，系统量测矩阵h：

当判断行人为曲线行走时，系统量测矩阵h：

建立状态方程和量测方程：

其中，φk为状态转移矩阵；δxk、δx′k-1分别为采样时间k下估计状态误差矩阵和采样时间k-1下纠正后的状态误差估计矩阵；wk-1为系统过程白噪声矢量；h为系统系统量测矩阵；vk为系统噪声量测矢量；zk为观测误差量；

系统量测矩阵h由多个量测子矩阵组成；系统量测矩阵h以及观测量zk定义如下：

为第k个采样时间下的角度测量误差观测矩阵；为第k个采样时间下的位置误差观测矩阵；为第k个采样时间下的速度误差观测矩阵；为第k个采样时间下的角速度测量误差观测矩阵；为第k个采样时间下的加速度测量误差观测矩阵；

h^θ根据行人状态误差向量的角度误差分量定义；h^p根据行人状态误差向量的位置误差分量定义；h^z根据行人状态误差向量的速度误差分量定义；h^r根据行人状态误差向量的角速度误差分量定义；hⁿ根据行人状态误差向量的加速度误差分量定义；

最终更新后的观测量和系统量测矩阵组合输入到扩展卡尔曼滤波器，经过滤波后的行人状态误差向量与惯导解算后的粗精度数据相融合最终得到行人状态向量的最优估计。

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