一种基于多传感器的消防员室内三维定位方法与流程

本发明涉及室内三维定位领域，尤其涉及一种基于多传感器的消防员室内三维定位方法。

背景技术：

火灾是各种灾害发生最为频繁且具有相当大破坏性的灾害之一，火灾所造成的直接经济损失仅次于干旱和洪涝所造成的损失。根据有关统计结果显示：全世界每天发生火灾1万多起，数百人由于火灾而导致直接死亡，在一些发达国家里，每年火灾所造成的直接经济损失占国民经济总值的2％。

在中国，火灾已成为我国公民主要死亡原因之一。仅2016年，全国共接报火灾31.2万起，死亡1582人，受伤1065人，直接财产损失37.2亿元。火灾事故不仅造成人民群众的财产损失和生命伤亡，还严重威胁着消防员的生命安全。根据公安部相关统计数据，2010-2014年，全国在救火抢险一线牺牲的消防员达144人，每年有近30名消防员在灭火救援战斗中牺牲，300多名消防员受伤甚至致残。尤其在2015年8月12日，天津港事件中瑞海公司危险品仓库发生火灾爆炸事故，造成95名消防员牺牲，大量消防员受伤。因此消防员已成为和平时期危险性最高的职业之一。同时，火灾发生后，现场出现大量的浓烟、毒气和烈火，浓烟、毒气和烈火的不受控，通信设施的瘫痪，人员的受伤和恐慌等情况，都会影响火灾的救援效率，也非常考验指挥员的指挥调度。因此，急需一种适用于火灾救援现场的消防员室内三维定位方法，可准确获得消防员的位置信息，从而保障消防员的生命安全和提高救援效率。

目前较多学者侧重于利用wifi信号、摄像头信号、无线信号等信号，研究室内二维定位方法，但是这些方法只考虑室内人员的平面定位，没有考虑人员的高度，较难应用到高楼、地下室等多层结构的建筑内，且大多数需要在室内安装较多额外的硬件设备，不适用于缺电、出现大量浓烟、烈火等火场环境下的消防员定位。一部分学者侧重于利用单个或多个信号研究室内三维定位方法，如利用现场部署摄像头的图像数据，激光信号，声音信号，led灯光信号和超宽带信号等单一信号实现室内三维定位，但是这些方法也都需要在定位现场部署较多额外的硬件设备，也不适用于火灾救援现场，且没有考虑无法在建筑物内快速大量部署硬件、大量浓烟和烈火导致现场视线受阻等特殊火灾环境，不适用于复杂火场环境下的室内消防员的三维定位。目前关于消防员的室内定位方法的研究结果非常少，主要集中在rfid、惯性导航方面的定位方法研究。如x.z.wang等人采集所有消防员的信息，采用集中式方法计算消防员的位置。集中式方法需要收集所有传感节点信息，实时性较差。x.zeng等人利用rfid、声音等信号对室内人员进行定位。d.wang等人考虑野外火灾救援场景，考虑部署在货车上的基站设备和消防员身上的beacon节点，根据基站设备信号计算自身位置坐标。但是这些方法需要部署较多的参考节点，降低了消防员的救援效率。

总之，目前基于图像信号、激光信号、声音信号、led灯光信号和超宽带信号等单一或多个信号的定位方法需要在定位现场部署较多额外硬件设备，基于激光扫描仪的定位方法需要事先获知整个建筑物的扫描数据。基于惯性导航数据的定位方法存在累计误差较大的问题。这些方法不适用于缺电、出现大量浓烟、烈火等火场环境下的火灾救援现场。

技术实现要素：

为了提高火灾救援效率和保障消防员安全，克服目前室内三维定位方法没有考虑到火灾救援现场的浓烟、烈火等恶劣环境条件限制，本发明提供一种可实时精准定位消防员三维位置的基于多传感器的消防员室内三维定位方法。

本发明解决其技术问题所采用的技术方案是：

一种基于多传感器的消防员室内三维定位方法，包括如下步骤：

1)初始化：初始化各个参数，包括起始位置的海拔高度、横轴、纵轴和竖轴的方向，二维平面坐标和姿态角信息；

2)采集当前时刻t的姿态传感器中的三轴加速度计数据并记为其中表示t时刻的横轴加速度，表示t时刻的纵轴加速度，表示t时刻的竖轴加速度；采集当前时刻t的姿态传感器中的三轴陀螺仪数据并记为其中表示t时刻的横轴角速度，表示t时刻的纵轴角速度，表示t时刻的竖轴角速度；采集当前时刻t的姿态传感器中的三轴磁力计数据并记为其中表示t时刻的横轴地磁强度，表示t时刻的纵轴地磁强度，表示t时刻的竖轴地磁强度；采集当前时刻t的气压计的气压值并记为ρt，然后将所有采集的数据存储到存储器中；

3)根据加速度计和陀螺仪输出的信息，采用过零检测方法判断当前时刻t是处于平稳期还是处于迈步期：

4)获取当前时刻t的气压计数据，通过公式(9)计算消防员所在的海拔高度，通过公式(10)计算相对海拔高度值，采用卡尔曼滤波方法对相对海拔高度数据进行滤波；

δht＝ht-h0(10)

其中,ρt表示t时刻消防员所处位置的大气压力值，ρ0表示标准大气压力值1013.25mbar，ht表示t时刻消防员所处的海拔高度，单位m，δht表示t时刻消防员所处的相对海拔高度，h0表示起始位置的海拔高度；

5)根据步骤3过零检测的结果，判断当前时刻是否处于平稳期，如果处于平稳期且该平稳期内卡尔曼滤波后输出的相对海拔高度数据个数大于或等于100个，则选择该100个相对海拔高度数据，否则如果处于平稳期且该平稳期内卡尔曼滤波后输出的相对海拔高度数据个数小于100个，则直接选择该平稳期内所有相对海拔高度数据，否则直接跳到步骤2)，采用箱型图方法对所选择的相对海拔高度数据进行异常点检测和修正，将这些相对海拔数据的平均值作为当前时刻的消防员的高度值，跳到步骤6)；

6)根据前后时刻消防员高度，判别消防员的行为，对行为判别结果进行修正；

7)根据加速度计和陀螺仪的值，计算当前二维移动距离，根据陀螺仪和磁力计的值，采用互补滤波修正航向角，并通过移动距离计算自身的二维位置坐标；

8)采用kalman融合方法对信息进行融合处理，计算和输出消防员的室内三维坐标；

9)如果消防员手动结束室内三维定位功能，则退出，否则跳到步骤2)。

进一步，所述步骤3)中，过零检测方法包含如下步骤：

3.1)获取t时刻的三轴加速度计数据，通过公式(1)计算t时刻的合加速度，通过公式(2)计算合加速度幅值均方根；

其中，at表示t时刻的三个方向的合加速度大小；

其中，表示t时刻的合加速度幅值均方根，n表示窗口长度；

3.2)判断合加速度的均方根是否满足公式(3)，如果满足，则判断指示符c1置1，否则判断指示符c1置0；

其中，c1表示加速度幅值均方根的判断指示符，thmin和thmax表示根据实际情况设置的幅值均方根条件最小和最大阈值；

3.3)通过公式(4)计算t时刻的加速度的平均加速度值，通过公式(5)计算加速度的幅值方差；

其中，表示t时刻的平均加速度值，s表示平滑窗口长度；

其中，表示t时刻的加速度的幅值方差；

3.4)判断加速度的幅值方差是否满足公式(6)，如果满足，则判断指示符c2置1，否则判断指示符c2置0；

其中，c2表示加速度幅值方差的判断指示符，thσ-min表示根据实际情况设置的幅值方差阈值；

3.5)获取t时刻的角速度值，通过公式(7)计算t时刻的角速度幅值均方根；

其中，表示t时刻的角速度幅值均方根；

3.6)判断角速度幅值均方根是否满足公式(8)，如果满足，则判断指示符c3置1，否则判断指示符c3置0，

其中，c3表示角速度幅值均方根的判断指示符，thw-max表示根据实际情况设置的角速度阈值；

3.7)如果c1，c2和c3同时为1，则当前时刻t处于平稳期，否则当前时刻t处于迈步期。

再进一步，所述步骤4)中，所述卡尔曼滤波方法包含如下步骤：

4.1)建立空间模型为

其中，bt表示t时刻状态向量的估计值，表示t时刻的相对海拔高度变化速率，wt-1表示t-1时刻的过程噪声，δt表示数据更新周期；

4.2)建立观测方程为：

其中，表示滤波方法的测量矩阵，qt表示t时刻的测量噪声，表示t时刻滤波方法的测量值，表示为：

4.3)通过时间更新方程和测量更新方程迭代计算，获得滤波后的相对海拔高度值。

更进一步，所述步骤5)中，所述箱型图方法包含如下步骤：

5.1)令h表示所选择的相对海拔高度数据集合，对集合h内所有元素进行排序，选择排序第25％的相对海拔高度值作为上四分位值u，选择排序第75％的相对海拔高度值作为下四分位值l，即集合h内只有1/4个数大于u，只有1/4个数小于l，则定义上界高度和下界高度为

hupper＝u+1.5(u-l)(14)

hlower＝u-1.5(u-l)(15)

其中，hupper表示上界高度数值，hlower表示下界高度数值；

5.2)选择上下界高度以外的相对海拔高度值为异常相对海拔高度值，其他值为正常相对海拔高度值，对所有正常相对海拔高度值取平均，得到平均值将所有异常相对海拔高度值赋值为获得新的相对海波位置集合

5.3)通过公式(16)计算新的相对海波位置集合h的平均相对海拔高度值，并将该值作为当前时刻t的消防员所在的高度：

其中，表示t时刻相对海拔位置集合的平均相对海拔高度值，表示t时刻相对海拔位置集合的第j个相对海拔高度值，nh表示相对海拔位置集合内元素个数。

进一步，所述步骤6)中，消防员行为判别包含如下步骤：

6.1)令平面行为判断阈值为λh，楼梯行走判断阈值为μh，如果相邻时刻的消防员高度变化的绝对值不大于λh，即|hk-hk-1|≤λh，则认为消防员在平面上行动，否则如果相邻时刻中后一时刻的消防员高度减去前一个时刻的消防员高度大于或等于μh，则认为消防员在乘坐电梯且上行，否则如果相邻时刻中后一时刻的消防员高度减去前一个时刻的消防员高度小于或等于-μh，则认为消防员在乘坐电梯且下行，否则如果相邻时刻中后一时刻的消防员高度减去前一个时刻的消防员高度大于λh，则认为消防员在向上走楼梯，否则认为消防员在向下走楼梯；

6.2)修正对当前平稳期内的行为判断结果。

步骤6.2)中，当前平稳期内的行为判断结果修正方法包含如下步骤：

6.2.1)在当前平稳期内，如果行为判断结果中乘坐电梯的行为占80％以上，则判断消防员在乘坐电梯，否则如果行为判断结果中走楼梯的行为占80％以上，则判断消防员在走电梯，否则判断消防员在平面上行动；

6.2.2)如果当前平稳期内消防员的行为判断为乘坐电梯，且在当前平稳期的开始，中间和结束阶段部分判断结果为走楼梯，则直接修正为乘坐电梯；如果该平稳期的结束部分判断结果为在平面上行动，则结束修正，退出，否则跳到步骤6.2.3)；

6.2.3)如果当前平稳期内消防员的行为判断为走楼梯，且在当前平稳期的中间部分判断结果为乘坐电梯，则直接修正为走楼梯；如果当前平稳期的中间部分判断结果为在平面上行动，则计算开始在平面上行动判断结果的相邻部分时间长度；若时间长度大于第一修正阈值η1，则保留判断结果，否则修正为走楼梯；如果该平稳期的结束部分判断结果为在平面上行动，则结束修正，退出，否则跳到步骤6.2.4)；

6.2.4)如果当前平稳期内消防员的行为判断为平面上行走，且当前平稳期的中间部分判断结果出现上下楼梯和乘坐电梯时，则计算开始上下楼梯和乘坐电梯判断结果的时间长度；若时间长度小于第二修正阈值η2，则修正为平面上行走，结束修正，退出，否则直接结束修正，退出。

所述步骤7)中，所述二维位置坐标计算方法包括以下步骤：

7.1)通过公式(17)将加速度计输出的载体坐标系b的加速度值转换到导航坐标系n的加速度值：

其中，表示经过转换后的导航坐标系n的加速度值，表示载体坐标系到导航坐标系的旋转矩阵；

7.2)通过公式(18)利用加速度得到导航坐标系n的速度

其中，t表示采样周期；

7.3)根据过零检测的结果，分析当前时刻是否处于平稳期，如果处于平稳期且该平稳期内接收的速度数据个数大于或等于100个，选择该100个速度数据，否则如果处于平稳期且该平稳期内接收的速度数据个数小于100个，选择该平稳期内所有相对速度数据，否则直接退出；

7.4)令vpi表示所选择平稳期的速度数据集合，通过公式(19)计算vpi的速度平均值；

其中，表示集合vpi中第i个速度，βpi表示集合vpi的速度平均值，nω表示集合vpi的所有元素个数；

7.5)根据平均速度βpi，通过公式(20)修正迈步期的速度：

其中，vma表示迈步期的速度数据集合，表示集合vma中经过修正后的导航坐标系n的第i个速度值，表示集合vma中导航坐标系n的第i个速度值，nλ表示迈步期速度数据集合vma内所有元素个数，i0表示迈步期速度数据集合vma的第一个元素序号，i表示迈步期速度数据集合vma内元素的序号；

7.6)根据平均速度βpi，通过公式(21)修正平稳期的速度：

其中，表示集合vpi中经过修正后的导航坐标系n的第j个速度值，表示集合vpi中的导航坐标系n的第j个速度值；将平稳期和迈步期的速度进行组合，可获得修正的速度

7.7)通过公式(22)计算位移增量δpt：

其中,δpt＝(δpt^x,δpt^y)表示位移增量，δpt^x表示位移增量的横轴坐标增量，δpt^y表示位移增量的纵轴坐标增量；

7.8)通过公式(23)计算平面上移动距离：

其中，slt表示t时刻的移动距离；

7.9)根据陀螺仪输出值和磁力计输出值采用互补滤波修正航向角，并根据移动距离，通过公式(24)计算自身二维位置坐标，

其中，xt表示t时刻的消防员位置的横轴坐标，yt表示t时刻的消防员位置的纵轴坐标，表示结合旋转矩阵计算的t时刻的航向角。

进一步，所述步骤8)中，所述kalman融合方法包含如下步骤：

8.1)令xt表示t时刻kalman融合方法的状态向量估计值，则为

其中，rt表示3维位置估计值，ct表示3维速度估计值，表示3维姿态角估计值，bt表示3维加速度计零偏误差值，ηt表示3维陀螺仪零偏值；

8.2)令加速度aⁿ的反对称矩阵ζ为

其中，表示t时刻导航坐标系n的横轴加速度，表示t时刻导航坐标系n的纵轴加速度，表示t时刻导航坐标系n的竖轴加速度，令状态转移矩阵ft为

其中，ξ表示t时刻的旋转矩阵，δt表示采样间隔，i3*3表示3×3单位矩阵，o3*3表示全为0的3×3矩阵，令系统噪声增益矩阵γt-1为

则建立空间模型为

xt＝ftxt-1+γt-1wt-1(29)

其中，ft表示状态转移矩阵，wt-1表示t-1时刻的过程噪声，γt-1表示系统噪声增益矩阵；

8.3)令融合方法的测量矩阵ht为

ht＝[i3*3δti3*3o3*3o3*3o3*3o3*3](30)

则建立观测方程为：

zt＝htxt+nt(31)

其中，zt表示t时刻融合方法的测量值，nt表示t时刻的测量噪声；

8.4)根据当前时刻消防员的高度值判断消防员所在的楼层，如果当前时刻消防员在平面上行动，则选择消防员所在楼层的高度作为当前消防员的竖轴坐标zt，选择二维位置坐标计算方法输出结果为平面二维坐标，令zt＝(xt,yt,zt)，否则如果当前时刻消防员在走楼梯，选择消防员的高度值作为当前消防员的竖轴坐标，选择二维位置坐标计算方法输出结果为当前消防员的平面二维坐标，令当消防员走出楼梯且进入平面上行动时，选择消防员所在楼层的高度作为当前消防员的竖轴坐标zt，选择楼梯出口的二维位置为当前消防员的平面二维坐标，令对消防员的三维位置进行修正，其中表示楼梯出口的横轴坐标，表示楼梯出口的纵轴坐标，否则当前时刻消防员在乘坐电梯，选择消防员的高度值作为当前消防员的竖轴坐标，选择二维位置坐标计算方法输出结果为当前消防员的平面二维坐标，令当消防员走出电梯且进入在平面上行动时，选择消防员所在楼层的高度作为当前消防员的竖轴坐标zt，选择二维位置坐标计算方法输出结果为当前消防员的平面二维坐标，令zt＝(xt,yt,zt)对消防员的三维位置进行修正；

8.5)通过时间更新方程和测量更新方程迭代计算，计算和输出融合后的消防员室内三维位置坐标。

本发明的有益效果主要表现在：

当消防员进入室内进行救援行动时，本发明通过气压计输出数据和kalman滤波，计算消防员所在的高度和楼层，判断平地行走、上下楼梯、上下电梯等行为。通过三轴加速度计、三轴陀螺仪和三轴磁力计的数据，采用过零检测方法识别消防员静止和运动情况，消除传感器的漂移误差，并选择二维位置坐标计算方法消防员的二维平面位置坐标。最终选择第一层的楼梯位置作为原点，以楼梯为参照点，采用kalman融合方法计算消防员的三维位置坐标。总之，当消防员处于复杂室内环境下运动时，本发明能够实时判别消防员的移动行为情况，降低了位置误差，解决火灾复杂环境下室内消防员的定位问题，为火场中消防员的自身危机救援和整体火灾救援行为提供消防员的位置信息，从而保障消防员的生命安全和提高救援效率。

附图说明

图1是本发明的工作流程框图。

具体实施方式

下面结合附图对本发明作进一步描述。

参照图1，一种基于多传感器的消防员室内三维定位方法，包括如下步骤：

1)初始化：初始化各个参数，包括起始位置的海拔高度、横轴、纵轴和竖轴的方向，二维平面坐标和姿态角等信息；

2)采集当前时刻t的姿态传感器中的三轴加速度计数据并记为其中表示t时刻的横轴加速度，表示t时刻的纵轴加速度，表示t时刻的竖轴加速度，采集当前时刻t的姿态传感器中的三轴陀螺仪数据并记为其中表示t时刻的横轴角速度，表示t时刻的纵轴角速度，表示t时刻的竖轴角速度，采集当前时刻t的姿态传感器中的三轴磁力计数据并记为其中表示t时刻的横轴地磁强度，表示t时刻的纵轴地磁强度，表示t时刻的竖轴地磁强度，采集当前时刻t的气压计的气压值并记为ρt，然后将所有采集的数据存储到存储器中；

3)根据加速度计和陀螺仪输出的信息，采用过零检测方法判断当前时刻t是处于平稳期还是处于迈步期：所述过零检测方法包含如下步骤：

3.1)获取t时刻的三轴加速度计数据，通过公式(1)计算t时刻的合加速度，通过公式(2)计算合加速度幅值均方根；

其中，at表示t时刻的三个方向的合加速度大小；

其中，表示t时刻的合加速度幅值均方根，n表示窗口长度；

3.2)判断合加速度的均方根是否满足公式(3)，如果满足，则判断指示符c1置1，否则判断指示符c1置0，

其中，c1表示加速度幅值均方根的判断指示符，thmin和thmax表示根据实际情况设置的幅值均方根条件最小和最大阈值；

3.3)通过公式(4)计算t时刻的加速度的平均加速度值，通过公式(5)计算加速度的幅值方差，

其中，表示t时刻的平均加速度值，s表示平滑窗口长度；

其中，表示t时刻的加速度的幅值方差；

3.4)判断加速度的幅值方差是否满足公式(6)，如果满足，则判断指示符c2置1，否则判断指示符c2置0，

其中，c2表示加速度幅值方差的判断指示符，thσ-min表示根据实际情况设置的幅值方差阈值；

3.5)获取t时刻的角速度值，通过公式(7)计算t时刻的角速度幅值均方根，

其中，表示t时刻的角速度幅值均方根；

3.6)判断角速度幅值均方根是否满足公式(8)，如果满足，则判断指示符c3置1，否则判断指示符c3置0，

其中，c3表示角速度幅值均方根的判断指示符，thw-max表示根据实际情况设置的角速度阈值；

3.7)如果c1，c2和c3同时为1，则当前时刻t处于平稳期，否则当前时刻t处于迈步期；

4)获取当前时刻t的气压计数据，通过公式(9)计算消防员所在的海拔高度，通过公式(10)计算相对海拔高度值；采用卡尔曼滤波方法对相对海拔高度数据进行滤波，消除噪声；

δht＝ht-h0(10)

其中,ρt表示t时刻消防员所处位置的大气压力值，ρ0表示标准大气压力值1013.25mbar，ht表示t时刻消防员所处的海拔高度，单位m，δht表示t时刻消防员所处的相对海拔高度，h0表示起始位置的海拔高度，所述卡尔曼滤波方法包含如下步骤：

4.1)建立空间模型为

其中，bt表示t时刻状态向量的估计值，表示t时刻的相对海拔高度变化速率，wt-1表示t-1时刻的过程噪声，δt表示数据更新周期；

4.2)建立观测方程为：

其中，表示滤波方法的测量矩阵，qt表示t时刻的测量噪声，表示t时刻滤波方法的测量值，表示为：

4.3)通过时间更新方程和测量更新方程迭代计算，获得滤波后的相对海拔高度值；

5)根据步骤3过零检测的结果，判断当前时刻是否处于平稳期，如果处于平稳期且该平稳期内卡尔曼滤波后输出的相对海拔高度数据个数大于或等于100个，则选择该100个相对海拔高度数据，否则如果处于平稳期且该平稳期内卡尔曼滤波后输出的相对海拔高度数据个数小于100个，则直接选择该平稳期内所有相对海拔高度数据，否则直接跳到步骤2)，采用箱型图方法对所选择的相对海拔高度数据进行异常点检测和修正，将这些相对海拔数据的平均值作为当前时刻的消防员的高度值，跳到步骤6)；所述箱型图方法包含如下步骤：

5.1)令h表示所选择的相对海拔高度数据集合，对集合h内所有元素进行排序，选择排序第25％的相对海拔高度值作为上四分位值u，选择排序第75％的相对海拔高度值作为下四分位值l，即集合h内只有1/4个数大于u，只有1/4个数小于l，则定义上界高度和下界高度为

hupper＝u+1.5(u-l)(14)

hlower＝u-1.5(u-l)(15)

其中，hupper表示上界高度数值，hlower表示下界高度数值；

5.2)选择上下界高度以外的相对海拔高度值为异常相对海拔高度值，其他值为正常相对海拔高度值；对所有正常相对海拔高度值取平均，得到平均值将所有异常相对海拔高度值赋值为获得新的相对海波位置集合

5.3)通过公式(16)计算新的相对海波位置集合h的平均相对海拔高度值，并将该值作为当前时刻t的消防员所在的高度：

其中，表示t时刻相对海拔位置集合的平均相对海拔高度值，表示t时刻相对海拔位置集合的第j个相对海拔高度值，nh表示相对海拔位置集合内元素个数；

6)根据前后时刻消防员高度，判别消防员的行为，对行为判别结果进行修正，从而保证判断的准确度；消防员行为判别包含如下步骤：

6.1)令平面行为判断阈值为λh，楼梯行走判断阈值为μh；如果相邻时刻的消防员高度变化的绝对值不大于λh，即|hk-hk-1|≤λh，则认为消防员在平面上行动，否则如果相邻时刻中后一时刻的消防员高度减去前一个时刻的消防员高度大于或等于μh，则认为消防员在乘坐电梯且上行，否则如果相邻时刻中后一时刻的消防员高度减去前一个时刻的消防员高度小于或等于-μh，则认为消防员在乘坐电梯且下行，否则如果相邻时刻中后一时刻的消防员高度减去前一个时刻的消防员高度大于λh，则认为消防员在向上走楼梯，否则认为消防员在向下走楼梯；

6.2)修正对当前平稳期内的行为判断结果，步骤6.2)中，当前平稳期内的行为判断结果修正方法包含如下步骤：

6.2.1)在当前平稳期内，如果行为判断结果中乘坐电梯的行为占80％以上，则判断消防员在乘坐电梯，否则如果行为判断结果中走楼梯的行为占80％以上，则判断消防员在走电梯，否则判断消防员在平面上行动；

6.2.2)如果当前平稳期内消防员的行为判断为乘坐电梯，且在当前平稳期的开始，中间和结束阶段部分判断结果为走楼梯，则直接修正为乘坐电梯；如果该平稳期的结束部分判断结果为在平面上行动，则结束修正，退出，否则跳到步骤6.2.3)；

6.2.3)如果当前平稳期内消防员的行为判断为走楼梯，且在当前平稳期的中间部分判断结果为乘坐电梯，则直接修正为走楼梯，如果当前平稳期的中间部分判断结果为在平面上行动，则计算开始在平面上行动判断结果的相邻部分时间长度；若时间长度大于第一修正阈值η1，则保留判断结果，否则修正为走楼梯，如果该平稳期的结束部分判断结果为在平面上行动，则结束修正，退出，否则跳到步骤6.2.4)；

6.2.4)如果当前平稳期内消防员的行为判断为平面上行走，且当前平稳期的中间部分判断结果出现上下楼梯和乘坐电梯时，则计算开始上下楼梯和乘坐电梯判断结果的时间长度，若时间长度小于第二修正阈值η2，则修正为平面上行走，结束修正，退出，否则直接结束修正，退出；

7)根据加速度计和陀螺仪的值，计算当前二维移动距离，根据陀螺仪和磁力计的值，采用互补滤波修正航向角，并通过移动距离计算自身的二维位置坐标；二维位置坐标计算包括以下步骤：

7.1)通过公式(17)将加速度计输出的载体坐标系b的加速度值转换到导航坐标系n的加速度值：

其中，表示经过转换后的导航坐标系n的加速度值，表示载体坐标系到导航坐标系的旋转矩阵；

7.2)通过公式(18)利用加速度得到导航坐标系n的速度

其中，t表示采样周期；

7.3)根据过零检测的结果，分析当前时刻是否处于平稳期，如果处于平稳期且该平稳期内接收的速度数据个数大于或等于100个，选择该100个速度数据，否则如果处于平稳期且该平稳期内接收的速度数据个数小于100个，选择该平稳期内所有相对速度数据，否则直接退出；

7.4)令vpi表示所选择平稳期的速度数据集合，通过公式(19)计算vpi的速度平均值；

其中，表示集合vpi中第i个速度，βpi表示集合vpi的速度平均值，nω表示集合vpi的所有元素个数；

7.5)根据平均速度βpi，通过公式(20)修正迈步期的速度：

其中，vma表示迈步期的速度数据集合，表示集合vma中经过修正后的导航坐标系n的第i个速度值，表示集合vma中导航坐标系n的第i个速度值，nλ表示迈步期速度数据集合vma内所有元素个数，i0表示迈步期速度数据集合vma的第一个元素序号，i表示迈步期速度数据集合vma内元素的序号；

7.6)根据平均速度βpi，通过公式(21)修正平稳期的速度：

其中，表示集合vpi中经过修正后的导航坐标系n的第j个速度值，表示集合vpi中的导航坐标系n的第j个速度值；将平稳期和迈步期的速度进行组合，可获得修正的速度

7.7)通过公式(22)计算位移增量δpt：

其中,δpt＝(δpt^x,δpt^y)表示位移增量，δpt^x表示位移增量的横轴坐标增量，δpt^y表示位移增量的纵轴坐标增量；

7.8)通过公式(23)计算平面上移动距离：

其中，slt表示t时刻的移动距离；

7.9)根据陀螺仪输出值和磁力计输出值采用互补滤波修正航向角，并根据移动距离，通过公式(24)计算自身二维位置坐标，

其中，xt表示t时刻的消防员位置的横轴坐标，yt表示t时刻的消防员位置的纵轴坐标，表示结合旋转矩阵计算的t时刻的航向角；

8)采用kalman融合方法对信息进行融合处理，计算和输出消防员的室内三维坐标，所述kalman融合方法包含如下步骤：

8.1)令xt表示t时刻kalman融合方法的状态向量估计值，则为

其中，rt表示3维位置估计值，ct表示3维速度估计值，表示3维姿态角估计值，bt表示3维加速度计零偏误差值，ηt表示3维陀螺仪零偏值；

8.2)令加速度aⁿ的反对称矩阵ζ为

其中，表示t时刻导航坐标系n的横轴加速度，表示t时刻导航坐标系n的纵轴加速度，表示t时刻导航坐标系n的竖轴加速度，令状态转移矩阵ft为

其中，ξ表示t时刻的旋转矩阵，δt表示采样间隔，i3*3表示3×3单位矩阵，o3*3表示全为0的3×3矩阵，令系统噪声增益矩阵γt-1为

则建立空间模型为

xt＝ftxt-1+γt-1wt-1(29)

其中，ft表示状态转移矩阵，wt-1表示t-1时刻的过程噪声，γt-1表示系统噪声增益矩阵；

8.3)令融合方法的测量矩阵ht为

ht＝[i3*3δti3*3o3*3o3*3o3*3o3*3](30)

则建立观测方程为：

zt＝htxt+nt(31)

其中，zt表示t时刻融合方法的测量值，nt表示t时刻的测量噪声；

8.4)根据当前时刻消防员的高度值判断消防员所在的楼层，如果当前时刻消防员在平面上行动，则选择消防员所在楼层的高度作为当前消防员的竖轴坐标zt，选择二维位置坐标计算方法输出结果为平面二维坐标，令zt＝(xt,yt,zt)，否则如果当前时刻消防员在走楼梯，选择消防员的高度值作为当前消防员的竖轴坐标，选择二维位置坐标计算方法输出结果为当前消防员的平面二维坐标，令当消防员走出楼梯且进入平面上行动时，选择消防员所在楼层的高度作为当前消防员的竖轴坐标zt，选择楼梯出口的二维位置为当前消防员的平面二维坐标，令对消防员的三维位置进行修正，其中表示楼梯出口的横轴坐标，表示楼梯出口的纵轴坐标，否则当前时刻消防员在乘坐电梯，选择消防员的高度值作为当前消防员的竖轴坐标，选择二维位置坐标计算方法输出结果为当前消防员的平面二维坐标，令当消防员走出电梯且进入在平面上行动时，选择消防员所在楼层的高度作为当前消防员的竖轴坐标zt，选择二维位置坐标计算方法输出结果为当前消防员的平面二维坐标，令zt＝(xt,yt,zt)对消防员的三维位置进行修正；

8.5)通过时间更新方程和测量更新方程迭代计算，计算和输出融合后的消防员室内三维位置坐标；

9)如果消防员手动结束室内三维定位功能，则退出，否则跳到步骤2)。