CN107289935A

CN107289935A - 一种适用于可穿戴设备的室内导航算法

Info

Publication number: CN107289935A
Application number: CN201610206367.8A
Authority: CN
Inventors: 张璐; 邬俊; 陈璞; 王韬; 刘海亮; 谢阳光
Original assignee: No 618 Research Institute of China Aviation Industry
Current assignee: No 618 Research Institute of China Aviation Industry
Priority date: 2016-04-01
Filing date: 2016-04-01
Publication date: 2017-10-24
Anticipated expiration: 2036-04-01
Also published as: CN107289935B

Abstract

本发明提供一种适用于可穿戴设备的室内导航算法，采用了同时构建WI‑Fi指纹数据库并导航的办法，基于惯性敏感组件、气压计、磁力计等传感器实现室内无缝导航能力。其中，导航算法采用数据融合的方式，集成了惯性传感器数据，WI‑Fi无线网卡数据，气压计数据和磁力计数据，并进行导航解算从而输出导航结果。与此同时，通过无线网卡收集到的附近WI‑Fi信号信息实时生成指纹数据库进而辅助导航。本发明是基于惯性器件和WI‑Fi指纹识别的室内导航算法，基于低成本的MEMS惯性传感器、气压计、磁力计等器件，辅助以WI‑Fi定位算法，能够快速的生成并更新WI‑Fi指纹数据库辅助航迹推算，进而大幅提升室内导航能力，可适用于穿戴设备上，能够有效的完成室内导航定位。

Description

一种适用于可穿戴设备的室内导航算法

技术领域

本发明属于综合导航技术，特别是涉及一种基于惯性器件导航，同时构建WI-Fi(无线网络信号)指纹数据库辅助导航的室内导航定位算法。

背景技术

在室内环境下，由于卫星信号的大幅衰减，基于卫星定位的方法无法实施，成为了导航定位“最后一公里”的问题。由于近年来人们对于无线上网的需求日益增大，越来越多的WI-Fi无线接入点被架设在楼宇内以满足人们无缝接入互联网的需求，进而为基于WI-Fi信号的室内导航技术提供了空间。但传统的基于Wi-FI位置指纹信息的定位技术误差大，易受干扰，且指纹库的建立与更新效率较低。与此同时，Wi-FI接入点的移动或增加，也会大幅影响指纹库的性能，从而影响导航效果。

发明内容

本发明所要解决的技术问题是：提供一种适用于可穿戴设备的室内导航方案。

本发明的技术方案是：解决思路主要分为三个步骤：(1)通过以惯性传感器为主的多传感器融合技术、步速探测、零位校正等技术实现室内导航；(2)同时构建并实时更新WI-Fi指纹数据库；(3)通过WI-Fi指纹识别的定位结果辅助导航，提升导航精度。该方案所使用的器件可封装成为便携式模块，满足可穿戴设备使用。一种适用于可穿戴设备的室内导航算法，具体方案如下：以下步骤：

步骤1：初始化阶段

在产品初始化阶段，需要导入室内地图并指定初始位置，用户基于提前内置的室内地图集选择当下室内地图，并手动指定目前所在位置；如没有该地图，可通过网络数据库进行下载；

步骤2：基于多传感器的航迹推算

a)预处理陀螺、加计数据

将三轴陀螺信号分别记为NX、NY、NZ，对其进行低通滤波，滤除外界高频噪声

GX＝B0·NX+B1·NX_1+B2·NX_2+B3·NX_3+B4·NX_4+B5·NX_5+...

B6·NX_6+B7·NX_7-(A1·GX_1+A2·GX_2+A3·GX_3+A4·GX_4+...

A5·GX_5+A6·GX_6+A7·GX_7)

GY＝B0·NY+B1·NY_1+B2·NY_2+B3·NY_3+B4·NY_4+B5·NY_5+...

B6·NY_6+B7·NY_7-(A1·GY_1+A2·GY_2+A3·GY_3+A4·GY_4+...

A5·GY_5+A6·GY_6+A7·GY_7)

GZ＝B0·NZ+B1·NZ_1+B2·NZ_2+B3·NZ_3+B4·NZ_4+B5·NZ_5+...

B6·NZ_6+B7·NZ_7-(A1·GZ_1+A2·GZ_2+A3·GZ_3+A4·GZ_4+...

A5·GZ_5+A6·GZ_6+A7·GZ_7)

其中，每个采样时刻采集到的数据为NX，滤波后的输出值为GX，

NX_1为上次采样值，NX_2为上上次采样值，其它类同；

GX_1为上次滤波值，GX_2为上上次滤波值，其它类同；

A1-A7，B0-B7为滤波器参数，需根据陀螺加计信号进行适当选择；

加速度计同理，得到陀螺输出GX、GY、GZ和加计输出AX、AY、AZ；

b)磁力计确定航向

静止时只进行定位，移动时初始时刻以磁航向作为初始航向，不进行初始对准；

航向角ψ＝磁力计航向角

俯仰角θ＝0

横滚角γ＝0

c)捷联惯性解算

计算

C₁₁＝cos(γ)·cos(ψ)+sin(γ)·sin(θ)·sin(ψ)

C₁₂＝cos(θ)·sin(ψ)

C₁₃＝sin(γ)·cos(ψ)-cos(γ)·sin(θ)·sin(ψ)

C₂₁＝-cos(γ)·sin(ψ)+sin(γ)·sin(θ)·cos(ψ)

C₂₂＝cos(θ)·cos(ψ)

C₂₃＝-sin(γ)·sin(ψ)-cos(γ)·sin(θ)·cos(ψ)

C₃₁＝-cosθ·sinγ

C₃₂＝sinθ

C₃₃＝cosθ·cosγ

计算

d)用陀螺数据进行姿态角零速校正

平台偏角修正

e)用加计数据进行位置零速校正

λ(+)＝λ(-)-δλ

λ(-)＝λ(+)

速度修正

V_E(+)＝V_E(-)-δV_E

V_N(+)＝V_N(-)-δV_N

V_E(-)＝V_E(+)

V_N(-)＝V_N(+)

f)气压计阻尼高度

V_U＝V_U+K₁(h_b-h)

V_U(-)＝V_U

其中：K1＝0.0005

若满足高度阻尼计算条件，则按照下式进行阻尼计算：

h＝h+K₂·(h_b-h)

h(-)＝h

其中：K₂＝0.01

基于以上四步判断步速并进行基本航迹推算；

步骤3：WI-Fi指纹数据库的生成与更新

在本阶段，WI-Fi无线网卡实时接收来自附近无线接入点(AP,Access Point)的烽火信号(Beacon Signal)，其信号中包含了每个AP的MAC地址信息和信号强度(RSSI)信息；WI-Fi指纹数据库即包含了在室内环境下任意某点位置能够接收到来自附近AP发出的烽火信号的数据强度值的集合；

假设每个位置都可以用来自不同无线接入点的RSSI向量表示，那么某位置a可以表示为：

代表着该位置的RSS值向量，而RSS₁到RSS_n代表在随机位置a接收到的来自不同无线接入点的信号强度；在一个完整的Wi-Fi指纹数据库中，应该包含着每个采样点的位置信息和信号强度信息；因此，该区域的Wi-Fi指纹数据库可以写为：

其中x_i和y_i分别代表采样点的坐标位置，m表示采样点的数量；在本方案中，使用了一种快速实时生成并更新数据库的方式，即采用了基于高斯过程的机器学习方法；假设每个采样点可以被写为：

D＝{(x_i,y_i)|i＝1,2,...,n}

其中x表示输入向量(即坐标)，同时y表示测量值；假设这些数据取自于一个加噪的过程：

y_i＝f(x_i)+ε

其中误差ε服从均值为零，方差为的高斯分布；通过优选出的协方差函数(kernel)k(x_i，x_j)，我们可以得出f(x_i)和f(x_j)之间的关系；使用协方差函数：

实现高斯过程回归；利用这种办法可以通过有限的采样点快速生成整个环境下的数据库；刷新时间为1秒/次，每刷新一次更新一次WI-Fi指纹数据库；

步骤4：多信息源融合导航算法

a)根据步骤3的融合wifi信号比对数据，圈定室内位置，融合惯导阻尼

气压计数据并wifi信号，圈定楼层

Δφ_x＝(C₁₁·ω_e'+C₂₁·ω_n'+C₃₁·ω_u')·Ts

Δφ_y＝(C₁₂·ω_e'+C₂₂·ω_n'+C₃₂·ω_u')·Ts

Δφ_z＝(C₁₃·ω_e'+C₂₃·ω_n'+C₃₃·ω_u')·Ts

其中：

du₁＝(ω₈-ω₇)×(V_N-K₁·V_rN)-ω₇·u₁

du₂＝(ω₅-ω₆)×(V_N-K₁·V_rN+u₁)-ω₆·u₂

du₃＝(ω₄-ω₃)×(V_E-K₁·V_rE)-ω₃·u₃

du₄＝(ω₁-ω₂)×(V_E-K₁·V_rE+u₃)-ω₂·u₄

du₅＝C×B×(V_N-K₁·V_rN+u₁+u₂)-ω₉·u₅

du₆＝ω₁₀·u₅-ω₁₀·u₆

u₁＝u₁+du₁×T_S

u₂＝u₂+du₂×T_S

u₃＝u₃+du₃×T_S

u₄＝u₄+du₄×T_S

u₅＝u₅+du₅×T_S

u₆＝u₆+du₆×T_S

u₁、u₂、u₃、u₄、u₅、u₆初值均为0

b)设计扩展卡尔曼算法实现室内导航定位

X_K/K-1＝φ_K/K-1·X_K-1

P_K-1＝P_K/K-1

X_K-1＝X_K/K-1

X_K＝X_K/K-1+K_K·(Z_K-H_K·X_K/K-1)

P_K＝(I-K_K·H_K)·P_K/K-1

P_K-1＝P_K

当满足零速校正条件时：

δλ＝X′_K(2)

否则：

δλ＝0

δD_N、δD_U保持不变；

●量测矩阵

其中：

零速校正时量测矩阵：

P＝diag(6e-4，6e-4，1，1，6e-6，6e-6，3.0e-4，1e-12，1e-12，1e-12，2.5e-5，2.5e-5，2.5e-5，0，0，0，0，0，0，0,0,0)

Q＝diag(0，0，5.e-5，5.e-5，5.e-13，5.e-13，5.e-13，0，0，0，0，0，0,0,0,0,0,0，0，0,0,0)

R＝diag(2.6e-10，2.6e-10)

航迹推算过程中更换wifi接入点时，要对P_1,1、P_2,2进行重置：

P_1,1＝6e-4

P_2,2＝6e-4

●零速校正滤波参数

Q＝diag(0，0，0，0，5.e-13，5.e-13，5.e-13，0，0，0，0，0，0,0,0,0,0,0，0，0，0，0)

R＝diag(2.6e-10，2.6e-10，9，9)

步骤5：导航结果显示

导航结果将会通过无线网络传输至终端显示器进行显示(手机屏幕、手持设备屏幕等)，从而完成室内导航的任务。

根据多敏感组件进行惯性导航，并利用wifi信号进行实时，实现室内导航。

本发明的有益效果是：本方案设计的一种基于惯性器件和WI-Fi指纹识别的室内导航算法，基于低成本的MEMS惯性传感器、气压计、磁力计等器件，辅助以WI-Fi定位算法，能够快速的生成并更新WI-Fi指纹数据库辅助航迹推算，进而大幅提升室内导航能力。该方案适用于可穿戴设备上，能够有效的完成室内导航定位的工作。

附图说明

图1为本发明的处理流程图。

具体实施方式

如图1所示，本发明的技术方案是：一种适用于可穿戴设备的室内导航算法，其特点为如下所述：

1.1)初始化地图信息及位置信息

1.2)基于MEMS陀螺加计的惯性导航算法

1.3)对陀螺加计信号进行分析，找出步速的计算点，实行步速计算

1.4)当静止不动时，通过速度进行零位修正

1.5)有GPS信号时进行实时修正

1.6)利用基于高斯过程回归的机器学习算法实时生成并更新WI-Fi指纹数据库

1.7)WI-Fi指纹识别技术定位

1.8)采用扩展卡尔曼滤波器，对信息源进行融合计算，从而计算出导航结果

1.9)导航结果显示

结果表明，采用本发明的方法，基于可穿戴设备的室内导航算法可以实现偏差为1米(RMS)的室内导航精度。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

最后所应说明的是：以上实施实例仅用以说明而非限制本发明的技术方案，所有的不脱离本发明的精神和范围的修改或局部替换，均应涵盖在本发明的权利要求范围当中。

Claims

1.一种适用于可穿戴设备的室内导航算法，其特征在于以下步骤：

步骤1：初始化阶段

步骤2：基于多传感器的航迹推算

a)预处理陀螺、加计数据

GX＝B0·NX+B1·NX_1+B2·NX_2+B3·NX_3+B4·NX_4+B5·NX_5+...

B6·NX_6+B7·NX_7-(A1·GX_1+A2·GX_2+A3·GX_3+A4·GX_4+...

A5·GX_5+A6·GX_6+A7·GX_7)

GY＝B0·NY+B1·NY_1+B2·NY_2+B3·NY_3+B4·NY_4+B5·NY_5+...

B6·NY_6+B7·NY_7-(A1·GY_1+A2·GY_2+A3·GY_3+A4·GY_4+...

A5·GY_5+A6·GY_6+A7·GY_7)

GZ＝B0·NZ+B1·NZ_1+B2·NZ_2+B3·NZ_3+B4·NZ_4+B5·NZ_5+...

B6·NZ_6+B7·NZ_7-(A1·GZ_1+A2·GZ_2+A3·GZ_3+A4·GZ_4+...

A5·GZ_5+A6·GZ_6+A7·GZ_7)

NX_1为上次采样值，NX_2为上上次采样值，其它类同；

GX_1为上次滤波值，GX_2为上上次滤波值，其它类同；

b)磁力计确定航向

航向角ψ＝磁力计航向角

俯仰角θ＝0

横滚角γ＝0

c)捷联惯性解算

计算

C₁₁＝cos(γ)·cos(ψ)+sin(γ)·sin(θ)·sin(ψ)

C₁₂＝cos(θ)·sin(ψ)

C₁₃＝sin(γ)·cos(ψ)-cos(γ)·sin(θ)·sin(ψ)

C₂₁＝-cos(γ)·sin(ψ)+sin(γ)·sin(θ)·cos(ψ)

C₂₂＝cos(θ)·cos(ψ)

C₂₃＝-sin(γ)·sin(ψ)-cos(γ)·sin(θ)·cos(ψ)

C₃₁＝-cosθ·sinγ

C₃₂＝sinθ

C₃₃＝cosθ·cosγ

计算

<mrow> <msubsup> <mi>&Delta;&phi;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>X</mi> </mrow> <mi>b</mi> </msubsup> <mo>=</mo> <msubsup> <mi>&Delta;&phi;</mi> <mi>X</mi> <mi>b</mi> </msubsup> <mo>-</mo> <msub> <mi>&Delta;&phi;</mi> <mi>x</mi> </msub> </mrow>

<mrow> <msubsup> <mi>&Delta;&phi;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>Y</mi> </mrow> <mi>b</mi> </msubsup> <mo>=</mo> <msubsup> <mi>&Delta;&phi;</mi> <mi>Y</mi> <mi>b</mi> </msubsup> <mo>-</mo> <msub> <mi>&Delta;&phi;</mi> <mi>y</mi> </msub> </mrow>

<mrow> <msubsup> <mi>&Delta;&phi;</mi> <mrow> <mi>n</mi> <mi>b</mi> <mi>Z</mi> </mrow> <mi>b</mi> </msubsup> <mo>=</mo> <msubsup> <mi>&Delta;&phi;</mi> <mi>Z</mi> <mi>b</mi> </msubsup> <mo>-</mo> <msub> <mi>&Delta;&phi;</mi> <mi>z</mi> </msub> </mrow>

d)用陀螺数据进行姿态角零速校正

平台偏角修正

<mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&phi;</mi> <mi>U</mi> </msub> </mrow> </mtd> <mtd> <msub> <mi>&phi;</mi> <mi>N</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&phi;</mi> <mi>U</mi> </msub> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&phi;</mi> <mi>E</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mi>&phi;</mi> <mi>N</mi> </msub> </mrow> </mtd> <mtd> <msub> <mi>&phi;</mi> <mi>E</mi> </msub> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>&CenterDot;</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mo>-</mo> <mo>)</mo> </mrow> </mrow>

e)用加计数据进行位置零速校正

λ(+)＝λ(-)-δλ

λ(-)＝λ(+)

速度修正

V_E(+)＝V_E(-)-δV_E

V_N(+)＝V_N(-)-δV_N

V_E(-)＝V_E(+)

V_N(-)＝V_N(+)

f)气压计阻尼高度

V_U＝V_U+K₁(h_b-h)

V_U(-)＝V_U

其中：K₁＝0.0005

若满足高度阻尼计算条件，则按照下式进行阻尼计算：

h＝h+K₂·(h_b-h)

h(-)＝h

其中：K₂＝0.01

基于以上四步判断步速并进行基本航迹推算；

步骤3：WI-Fi指纹数据库的生成与更新

<mrow> <mover> <mi>r</mi> <mo>&RightArrow;</mo> </mover> <mo>=</mo> <mfenced open = "(" close = ")"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>RSS</mi> <mn>1</mn> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>RSS</mi> <mn>2</mn> </msub> </mrow> </mtd> <mtd> <mn>......</mn> </mtd> <mtd> <mrow> <msub> <mi>RSS</mi> <mi>n</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>

D＝{(x_i，y_i)|i＝1，2，...，n}

y_i＝f(x_i)+ε

步骤4：多信息源融合导航算法

a)根据步骤3的融合wifi信号比对数据，圈定室内位置，融合惯导阻尼气压计数据并wifi信号，圈定楼层

Δφ_x＝(C₁₁·ω_e'+C₂₁·ω_n'+C₃₁·ω_u')·Ts

Δφ_y＝(C₁₂·ω_e'+C₂₂·ω_n'+C₃₂·ω_u')·Ts

Δφ_z＝(C₁₃·ω_e'+C₂₃·ω_n'+C₃₃·ω_u')·Ts

其中：

du₁＝(ω₈-ω₇)×(V_N-K₁·V_rN)-ω₇·u₁

du₂＝(ω₅-ω₆)×(V_N-K₁·V_rN+u₁)-ω₆·u₂

du₃＝(ω₄-ω₃)×(V_E-K₁·V_rE)-ω₃·u₃

du₄＝(ω₁-ω₂)×(V_E-K₁·V_rE+u₃)-ω₂·u₄

du₅＝C×B×(V_N-K₁·V_rN+u₁+u₂)-ω₉·u₅

du₆＝ω₁₀·u₅-ω₁₀·u₆

u₁＝u₁+du₁×T_S

u₂＝u₂+du₂×T_S

u₃＝u₃+du₃×T_S

u₄＝u₄+du₄×T_S

u₅＝u₅+du₅×T_S

u₆＝u₆+du₆×T_S

<mrow> <msubsup> <mi>&omega;</mi> <mi>e</mi> <mo>&prime;</mo> </msubsup> <mo>=</mo> <mo>-</mo> <mfrac> <mrow> <mi>B</mi> <mo>&times;</mo> <mrow> <mo>(</mo> <msub> <mi>V</mi> <mi>N</mi> </msub> <mo>-</mo> <msub> <mi>K</mi> <mn>1</mn> </msub> <mo>&CenterDot;</mo> <msub> <mi>V</mi> <mrow> <mi>r</mi> <mi>N</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>u</mi> <mn>1</mn> </msub> <mo>+</mo> <msub> <mi>u</mi> <mn>2</mn> </msub> <mo>+</mo> <msub> <mi>K</mi> <mn>1</mn> </msub> <mo>&times;</mo> <msub> <mi>V</mi> <mrow> <mi>r</mi> <mi>N</mi> </mrow> </msub> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> </mrow>

<mrow> <msubsup> <mi>&omega;</mi> <mi>n</mi> <mo>&prime;</mo> </msubsup> <mo>=</mo> <msub> <mi>&omega;</mi> <mi>n</mi> </msub> <mo>+</mo> <mfrac> <mrow> <mi>A</mi> <mo>&times;</mo> <mrow> <mo>(</mo> <msub> <mi>V</mi> <mi>E</mi> </msub> <mo>-</mo> <msub> <mi>K</mi> <mn>1</mn> </msub> <mo>&CenterDot;</mo> <msub> <mi>V</mi> <mrow> <mi>r</mi> <mi>E</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>u</mi> <mn>3</mn> </msub> <mo>+</mo> <msub> <mi>u</mi> <mn>4</mn> </msub> <mo>+</mo> <msub> <mi>K</mi> <mn>1</mn> </msub> <mo>&times;</mo> <msub> <mi>V</mi> <mrow> <mi>r</mi> <mi>E</mi> </mrow> </msub> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>R</mi> <mi>E</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> <mo>+</mo> <mfrac> <mi>D</mi> <mrow> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> <mo>&CenterDot;</mo> <mrow> <mo>(</mo> <msub> <mi>u</mi> <mn>5</mn> </msub> <mo>-</mo> <msub> <mi>u</mi> <mn>6</mn> </msub> <mo>)</mo> </mrow> </mrow>

u₁、u₂、u3、u₄、u₅、u₆初值均为0

b)设计扩展卡尔曼算法实现室内导航定位

<mrow> <msub> <mi>&phi;</mi> <mrow> <mi>K</mi> <mo>/</mo> <mi>K</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mi>I</mi> <mo>+</mo> <mi>F</mi> <mo>&CenterDot;</mo> <msub> <mi>T</mi> <mi>F</mi> </msub> <mo>+</mo> <mfrac> <msup> <mrow> <mo>(</mo> <mi>F</mi> <mo>&CenterDot;</mo> <msub> <mi>T</mi> <mi>F</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mn>2</mn> </mfrac> </mrow>

<mrow> <msub> <mi>Q</mi> <mrow> <mi>K</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mi>Q</mi> <mo>&CenterDot;</mo> <msub> <mi>T</mi> <mi>F</mi> </msub> <mo>+</mo> <mrow> <mo>(</mo> <mi>F</mi> <mi>Q</mi> <mo>+</mo> <msup> <mrow> <mo>(</mo> <mrow> <mi>F</mi> <mi>Q</mi> </mrow> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>)</mo> </mrow> <mo>&CenterDot;</mo> <mfrac> <mrow> <msup> <msub> <mi>T</mi> <mi>F</mi> </msub> <mn>2</mn> </msup> </mrow> <mn>2</mn> </mfrac> </mrow>

X_K/K-1＝φ_K/K-1·X_K-1

<mrow> <msub> <mi>P</mi> <mrow> <mi>K</mi> <mo>/</mo> <mi>K</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>&phi;</mi> <mrow> <mi>K</mi> <mo>/</mo> <mi>K</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&CenterDot;</mo> <msub> <mi>P</mi> <mrow> <mi>K</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&CenterDot;</mo> <msubsup> <mi>&phi;</mi> <mrow> <mi>K</mi> <mo>/</mo> <mi>K</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>Q</mi> <mrow> <mi>K</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow>

P_K-1＝P_K/K-1

X_K-1＝X_K/K-1

<mrow> <msub> <mi>K</mi> <mi>K</mi> </msub> <mo>=</mo> <msub> <mi>P</mi> <mrow> <mi>K</mi> <mo>/</mo> <mi>K</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&CenterDot;</mo> <msubsup> <mi>H</mi> <mi>K</mi> <mi>T</mi> </msubsup> <mo>&CenterDot;</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>H</mi> <mi>K</mi> </msub> <mo>&CenterDot;</mo> <msub> <mi>P</mi> <mrow> <mi>K</mi> <mo>/</mo> <mi>K</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&CenterDot;</mo> <msubsup> <mi>H</mi> <mi>K</mi> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>R</mi> <mi>K</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow>

X_K＝X_K/K-1+K_K·(Z_K-H_K·X_K/K-1)

P_K＝(I-K_K·H_K)·P_K/K-1

P_K-1＝P_K

当满足零速校正条件时：

δλ＝X′_K (2)

<mrow> <msub> <mi>&delta;D</mi> <mi>N</mi> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>&OverBar;</mo> </mover> <mi>K</mi> </msub> <mrow> <mo>(</mo> <mn>21</mn> <mo>)</mo> </mrow> <mo>&times;</mo> <mn>0.7</mn> </mrow> 4

<mrow> <msub> <mi>&delta;D</mi> <mi>U</mi> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>&OverBar;</mo> </mover> <mi>K</mi> </msub> <mrow> <mo>(</mo> <mn>22</mn> <mo>)</mo> </mrow> <mo>&times;</mo> <mn>0.7</mn> </mrow>

否则：

δλ＝0

δD_N、δD_U保持不变；

●量测矩阵

<mfenced open = "" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>F</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>F</mi> <mn>1</mn> </msub> </mtd> <mtd> <msub> <mi>F</mi> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>7</mn> <mo>&times;</mo> <mn>13</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>7</mn> <mo>&times;</mo> <mn>7</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> <mtd> <mrow> <mi>F</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>F</mi> <mn>1</mn> </msub> </mtd> <mtd> <msub> <mi>F</mi> <mn>2</mn> </msub> </mtd> <mtd> <msub> <mi>F</mi> <mn>3</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>9</mn> <mo>&times;</mo> <mn>13</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>9</mn> <mo>&times;</mo> <mn>7</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>9</mn> <mo>&times;</mo> <mn>2</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> </mfenced>

其中：

<mrow> <msub> <mi>F</mi> <mn>2</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>C</mi> <mn>11</mn> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>f</mi> <mo>&OverBar;</mo> </mover> <mi>Z</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>C</mi> <mn>11</mn> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>f</mi> <mo>&OverBar;</mo> </mover> <mi>X</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>C</mi> <mn>13</mn> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>f</mi> <mo>&OverBar;</mo> </mover> <mi>Z</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>C</mi> <mn>21</mn> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>f</mi> <mo>&OverBar;</mo> </mover> <mi>Z</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>C</mi> <mn>21</mn> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>f</mi> <mo>&OverBar;</mo> </mover> <mi>X</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>C</mi> <mn>23</mn> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>f</mi> <mo>&OverBar;</mo> </mover> <mi>Z</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>C</mi> <mn>11</mn> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>&omega;</mi> <mo>&OverBar;</mo> </mover> <mi>Z</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>C</mi> <mn>11</mn> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>&omega;</mi> <mo>&OverBar;</mo> </mover> <mi>Y</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>C</mi> <mn>12</mn> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>&omega;</mi> <mo>&OverBar;</mo> </mover> <mi>Z</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>C</mi> <mn>13</mn> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>&omega;</mi> <mo>&OverBar;</mo> </mover> <mi>Y</mi> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>C</mi> <mn>21</mn> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>&omega;</mi> <mo>&OverBar;</mo> </mover> <mi>Z</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>C</mi> <mn>21</mn> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>&omega;</mi> <mo>&OverBar;</mo> </mover> <mi>Y</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>C</mi> <mn>22</mn> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>&omega;</mi> <mo>&OverBar;</mo> </mover> <mi>Z</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>C</mi> <mn>23</mn> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>&omega;</mi> <mo>&OverBar;</mo> </mover> <mi>Y</mi> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>C</mi> <mn>31</mn> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>&omega;</mi> <mo>&OverBar;</mo> </mover> <mi>Z</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>C</mi> <mn>31</mn> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>&omega;</mi> <mo>&OverBar;</mo> </mover> <mi>Y</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>C</mi> <mn>32</mn> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>&omega;</mi> <mo>&OverBar;</mo> </mover> <mi>Z</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>C</mi> <mn>33</mn> </msub> <mo>&CenterDot;</mo> <msub> <mover> <mi>&omega;</mi> <mo>&OverBar;</mo> </mover> <mi>Y</mi> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow>

<mrow> <msub> <mi>F</mi> <mn>3</mn> </msub> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>&prime;</mo> </msup> </mrow>

零速校正时量测矩阵：

R＝diag(2.6e-10，2.6e-10)

P_1,1＝6e-4

P_2,2＝6e-4

●零速校正滤波参数

R＝diag(2.6e-10，2.6e-10，9，9)

步骤5：导航结果显示

2.根据权利要求1所述的算法，其特征在于：根据多敏感组件进行惯性导航，并利用wifi信号进行实时，实现室内导航。