CN105043385A - 一种行人自主导航定位的自适应卡尔曼滤波方法 - Google Patents

Abstract

本发明公开了一种行人自主导航定位的自适应卡尔曼滤波方法，包括：将一个集成了加速度计、陀螺仪、磁力计的MEMS-IMU系统连接于人体，在行人运动过程中进行IMU数据采集；建立包含18维状态量、9维观测量的自适应滤波模型，在同时满足“四条件”时进行递推滤波，其间通过时变噪声统计估计器实时估计和修正系统噪声以及观测噪声的统计特性；本发明在使用零速校正作为误差补偿修正算法的基础上，设计融合人体运动特征的自适应滤波方法，实时处理人体晃动带入的噪声干扰信号，有效提高了行人自主导航定位的精度。本发明方法稳定性强、实时性好，并且不增加任何额外硬件成本。

Description

一种行人自主导航定位的自适应卡尔曼滤波方法

技术领域

本发明属于导航定位技术领域，尤其涉及一种行人自主导航定位的自适应卡尔曼滤波方法。

背景技术

近些年来导航定位技术依然在迅猛发展，而其中多数是基于卫星的导航系统，并且只适用于室外的环境，如应用最广泛的GPS，在城市楼群、山区、森林和地下建筑等遮挡环境下，其信号弱，可用性差。

随着人们生活节奏的加快，行人自主导航定位的需求也显得越发迫切。尤其是室内环境下，如火灾、楼宇坍塌等紧急环境的救援、商场内需要寻人或某个地方等。很多人采用地图信息匹配的方法进行行人导航，如国外专利CN1705861A《Walkernavigationdeviceandprogram》，其利用存储在地图数据库中的地图信息来计算当前位置并显示。这种方法需要先验的预知信息，对于火灾救援等临时突发紧急情况难以达到良好的处理效果。本发明中应用MEMS-IMU固连于人体对行人进行室内实时定位，不需要依赖任何外部的信息，可以实现全自主定位，并且价格便宜，体积小，容易实现可穿戴。利用惯性传感器实现行人定位的经典方法是行人航位推算(PDR)，其中大体可分为两类：一种是通过一步计数和步长估计来实现定位，通常用陀螺仪/罗盘来增强航向信息。如专利CN201310388466.9《行人步长估计及航位推算方法》中所述，利用步数、步长、方向进行测量和计算。但是此种方式往往只适用于特定的用户，在不寻常的行走模式，包括拥挤环境、上坡/下坡等环境中，步行统计假设条件会被破坏，因此容易失败。另一种方法是采用直接积分策略，但是随着行人移动距离和航向角的增加，惯性器件累计误差会越来越大，导致其定位精度不理想。在考虑系统实时性及计算复杂度的前提下，通常采用卡尔曼滤波来消除漂移误差。

现有的用于行人导航系统的滤波方法主要采用零速修正的方法，而后续跟进的滤波一般均采取基础的经典卡尔曼滤波或扩展卡尔曼滤波。如国内编号为CN201310566710的专利《基于捷联惯导解算和零速校正的自主导航系统定位方法》中所用的是经典卡尔曼滤波，其通过判断人脚处于运动还是静止状态来区分更新选项。当检测为运动时，只启动时间更新；当检测为静止时，则同时启动时间更新和量测更新。经典卡尔曼滤波是将系统默认为线性系统来进行更新处理，而人体运动必然会伴有摆动等干扰因素，显然是非线性系统，所以若应用经典卡尔曼滤波进行估计，修正效果并不理想。再如专利CN201310520233《一种基于双MEMS-IMU的行人自主导航解算算法》中以双脚解算距离超过双脚间最大步长为触发点，利用EKF完成估计修正，即用导航解算的协方差阵来更新卡尔曼滤波的协方差阵。此方法是将系统局部线性化，比起经典卡尔曼滤波的方法更符合行人运动的实际情况，因此定位精度有所提升，但仍然没解决噪声实时变化的影响问题，并且其每次更新都必须通过计算雅可比矩阵才能完成，实时性变差。

综上所述，在某种程度上本发明从成本、可靠性、复杂性以及精确性等角度都表现出了其它相关研究不具备或不同时具备的新颖性和先进性。

发明内容

本发明所要解决的技术问题在于克服现有技术的不足，提供一种行人自主导航定位的自适应卡尔曼滤波方法，该方法简单、稳定性强，在不增加任何额外硬件成本的情况下，有效提高了行人导航定位的精度。

本发明采用以下技术方案：

一种行人自主导航定位的自适应卡尔曼滤波方法，包括以下步骤：

步骤(1)，将MEMS-IMU系统固定连接于人体，IMU初始对准后，行人开始运动，运动过程中采集IMU数据；

步骤(2)，合理选取观测量建立融合人体运动特征的自适应卡尔曼滤波模型；

步骤(3)，同时满足“四条件”时进行自适应卡尔曼滤波，同步修正晃动噪声干扰，完成各状态量的估计。

步骤(4)，利用估计误差修正运动人体信息。

进一步的，在步骤(1)中，IMU输出数据是通过固连于人体上的测量传感器件测量所得；测量传感器件包括三轴加速度计、三轴陀螺仪和三轴磁力计。

进一步的，在步骤(1)中的初始对准选取北东地为导航坐标系，俯仰角θ和滚转角γ由经过补偿后的加速度计输出值计算获得，偏航角由磁场强度B、俯仰角θ和滚转角γ计算获得。计算公式为： $\mathrm{\θ}=\arcsin \frac{-a_k^b\left(x\right)}{\sqrt{a_k^b{\left(x\right)}^2+a_k^b{\left(y\right)}^2+a_k^b{\left(z\right)}^2}},\mathrm{\γ}=\arctan \frac{a_k^b\left(y\right)}{a_k^b\left(z\right)},$

其中，k代表k时刻，b代表载体(行人)坐标系，n代表导航坐标系，表示k时刻载体坐标系下行人的加速度，表示k时刻导航坐标系下的磁场强度。

进一步的，在步骤(2)中，滤波模型为包括3个位置误差、3个姿态角误差(俯仰角θ、滚转角γ和偏航角)、3个速度误差、3个重力不确定误差、三轴陀螺仪零偏误差和三轴加速度计零偏误差，共计18维的状态空间方程。

$\left\{\begin{array}{c}\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)\\ Z\left(t\right)=H\left(t\right)X\left(t\right)+V\left(t\right)\end{array}\right.$

W(t)为系统过程噪声矩阵，其表达式为： $W\left(t\right)=\left[\begin{array}{cc}-C_b^n{\mathrm{\ω}}^b& -C_b^n{a}^b\end{array}\right];$ G(t)为对应的噪声矩阵系数；F(t)为系统状态矩阵；Z(t)为观测量；H(t)为观测矩阵；V(t)为观测噪声矩阵。

被估计状态向量为： $X\left(t\right)={\left[\begin{array}{cccccc}\mathrm{\δr}& \mathrm{\δv}& \mathrm{\δ\ψ}& {\▿}_b& {\mathrm{\ϵ}}_b& \mathrm{\δg}\end{array}\right]}^T$

其中，δr为位置误差，δv为速度误差，δψ为姿态角误差，为加速度计零偏误差，ε_b为陀螺仪零偏误差，δg为重力不确定误差。

观测向量和观测矩阵为：

Z_k＝[Δv_kΔω_kΔψ_k]^T

$H=\left[\begin{array}{cccccc}{O}_{3\×3}& I_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}\\ O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& I_{3\×3}& O_{3\×3}\\ O_{3\×3}& O_{3\×3}& I_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}\end{array}\right]$

其中，Δψ_k包括磁力计计算的航向角与捷联解算出的航向角作差获得的航向角误差，利用加速度计计算出的俯仰角和横滚角与捷联解算出的姿态角作差获得的俯仰角和横滚角误差。

进一步的，所述的步骤(3)中的“四条件”包括：

(1)三轴加速度计合成的加速度幅值输出量满足阈值条件；

(2)加速度计z轴加速度输出量满足阈值条件；

(3)三轴陀螺仪合成的角速度幅值输出量满足阈值条件；

(4)陀螺仪y轴角速度输出量满足阈值条件；

当上述四项条件同时满足时，进行自适应滤波。

其中，条件(1)中三轴加速度计合成的加速度幅值表示为 $\left|a_k\right|=\sqrt{a_k^b{\left(x\right)}^2+a_k^b{\left(y\right)}^2+a_k^b{\left(z\right)}^2},$ 定义给定的阈值条件为th_amin＝9m/s²＜|a_k|＜th_amax＝11m/s²。条件(3)中三轴陀螺仪合成的角速度幅值表示为 $\left|{\mathrm{\ω}}_k\right|=\sqrt{{\mathrm{\ω}}_k^b{\left(x\right)}^2+{\mathrm{\ω}}_k^b{\left(y\right)}^2+{\mathrm{\ω}}_k^b{\left(z\right)}^2},$ 定义给定的阈值条件为|ω_k|＜th_ωmax＝50°/s。

在行人运动过程中，z轴加速度和y轴角速度变化最明显，由于安装偏差、行走晃动等影响，IMU会有所倾斜。因此在静止时刻y轴角速度不是完全零值，z轴加速度也不是完全g值，但都不会大于所设定阈值。条件(2)和条件(4)中的阈值在初始静止阶段分别以取加速度计输出的平均值和陀螺仪输出的平均值而设定。

进一步的，在步骤(3)中，采用自适应卡尔曼滤波进行误差估计，利用观测数据进行递推滤波的同时，通过时变噪声统计估计器实时估计和修正系统噪声以及观测噪声的统计特性，从而降低模型误差、抑制滤波发散，提高滤波精度。选取遗忘因子b＝0.98。

进一步的，在步骤(4)中，通过公式和反馈修正行人速度、位置和姿态信息。

本发明比现有技术具有以下有益效果及优点：

(1)本发明可穿戴，不需要预设节点，比其它需要预先知道室内布局环境的定位方法更方便，适用性更广。另外，相比于其它基于WiFi、Zigbee等无线网络的室内定位技术，本发明稳定性更高。

(2)本发明针对人体运动特性，采用的“四条件”识别方法简单，实时性高，起到良好的触发作用。比应用步长限制条件判断零瞬态的方法应用范围更广，不针对指定用户。

(3)在不增加任何额外硬件成本的情况下，利用自适应卡尔曼滤波器解决惯性器件长时间工作产生的累积漂移影响定位精度的问题。自适应体现在利用观测数据进行递推滤波的同时，用时变噪声统计估计器实时估计和修正系统噪声以及观测噪声的统计特性，从而降低模型误差、抑制滤波发散，提高滤波精度。此方法针对行人运动这种噪声实时变换的情况有良好的效果。

附图说明

图1为本发明的流程图

图2为本发明的工作原理图；

图3为自适应卡尔曼滤波框图；

图4为行人自主导航定位的自适应卡尔曼滤波方法实施例结果对比图。

具体实施方式

下面结合附图和具体实施方式对本发明作进一步描述：

图1是本发明的流程图，其中，数据采集的IMU模块(1-1)包括加速度计、陀螺仪和磁力计。将数据进行捷联惯性导航解算(1-2)，之后选取合适观测量得到滤波模型(1-3)，随后用“四条件”触发AKF模块(1-4)进行滤波。人体在正常运动过程中，与地面接触时间为毫秒级或更长，其中某一时刻人体与地面接触会达到瞬间静止状态，当同时满足所述“四条件”时即为瞬间静止状态。AKF指自适应卡尔曼滤波，AKF模块实现对系统噪声以及观测噪声统计特性的实时估计，从而降低模型误差、抑制滤波发散，提高滤波精度。最后，用估计量完成定位信息的校正(1-5)。

图2是本发明的工作原理图。其中，MEMS-IMU(2-1)包括三轴加速度计、三轴陀螺仪和三轴磁力计。加速度计输出加速度信息(2-2)，陀螺仪输出角速度信息(2-3)，将以上两种直接输出信息进行捷联惯性导航解算(2-4)，可得到位置、速度、姿态信息(2-5)。当检测到行人处于瞬间静止状态(2-7)，即触发滤波器，以零速度修正量(2-8)、零角速度修正量(2-9)、姿态角修正量(2-6)同时成为观测量，利用自适应卡尔曼滤波器(2-10)进行估计，随后进行反馈修正(2-11)，得到最终输出的位置、速度、姿态信息(2-12)。

一种行人自主导航定位的自适应卡尔曼滤波方法，它包括如下步骤：

步骤(1)：将包括三轴加速度计、三轴陀螺仪和三轴磁力计的MEMS-IMU固定连接于人体，选取北东地为导航坐标系，由经过补偿后的加速度计输出值计算获得俯仰角θ和滚转角γ，由磁场强度B、俯仰角θ和滚转角γ计算获得偏航角完成初始对准。对准后，在行人运动过程中采集IMU数据。

步骤(2)，根据硬件结构及人体运动特征，建立包括3个位置误差、3个姿态角误差(俯仰角θ、滚转角γ和偏航角)、3个速度误差、3个重力不确定误差三轴陀螺仪零偏误差和三轴加速度计零偏误差，共计18维的状态空间方程，即滤波模型。

$\left\{\begin{array}{c}\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)\\ Z\left(t\right)=H\left(t\right)X\left(t\right)+V\left(t\right)\end{array}\right.$

W(t)为系统过程噪声矩阵，其表达式为 $W\left(t\right)=\left[\begin{array}{cc}-C_b^n{\mathrm{\ω}}^b& -C_b^n{a}^b\end{array}\right],$ 其中为行人坐标系(b系)到导航坐标系(n系)的方向余弦矩阵，ω^b为行人角速度，a^b为行人加速度；G(t)为对应的噪声矩阵系数；F(t)为系统状态矩阵；Z(t)为观测量；H(t)为观测矩阵；V(t)为观测噪声矩阵。

被估计状态向量为： $X\left(t\right)={\left[\begin{array}{cccccc}\mathrm{\δr}& \mathrm{\δv}& \mathrm{\δ\ψ}& {\▿}_b& {\mathrm{\ϵ}}_b& \mathrm{\δg}\end{array}\right]}^T$

其中，δr为位置误差，δv为速度误差，δψ为姿态角误差，为加速度计零偏误差，ε_b为陀螺仪零偏误差，δg为重力不确定误差。

人体在正常运动过程中，与地面接触时间为毫秒级或更长，当检测到人体与地面接触达到瞬间静止时，其输出速度、角速度为零，但这只是理论上。由于传感器的测量误差、噪声以及算法误差等原因，此时速度、角速度计算值并不为零。因此选取瞬间静止时刻速度计算值的误差和陀螺仪的角速度输出误差作为观测量。为了增加可观性，同时将磁力计计算的航向角与捷联解算出的航向角作差获得的航向角误差，利用加速度计计算出的俯仰角和横滚角与捷联解算出的姿态角作差获得的俯仰角误差和横滚角误差同样作为观测量。

观测向量和观测矩阵为：

Z_k＝[Δv_kΔω_kΔψ_k]^T

$H=\left[\begin{array}{cccccc}{O}_{3\×3}& I_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}\\ O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& I_{3\×3}& O_{3\×3}\\ O_{3\×3}& O_{3\×3}& I_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}\end{array}\right]$

步骤(3)，同时满足“四条件”时进行自适应卡尔曼滤波，完成各状态量的估计。

进一步的，“四条件”具体为：

(1)三轴加速度计合成的加速度幅值输出量满足阈值条件；

(2)加速度计z轴加速度输出量满足阈值条件；

(3)三轴陀螺仪合成的角速度幅值输出量满足阈值条件；

(4)陀螺仪y轴角速度输出量满足阈值条件；

当上述四项条件同时满足时，进行自适应滤波。

其中，条件(1)中三轴加速度计合成的加速度幅值表示为 $\left|a_k\right|=\sqrt{a_k^b{\left(x\right)}^2+a_k^b{\left(y\right)}^2+a_k^b{\left(z\right)}^2},$ 定义给定的阈值条件为th_amin＝9m/s²＜|a_k|＜th_amax＝11m/s²。条件(3)中三轴陀螺仪合成的角速度幅值表示为 $\left|{\mathrm{\ω}}_k\right|=\sqrt{{\mathrm{\ω}}_k^b{\left(x\right)}^2+{\mathrm{\ω}}_k^b{\left(y\right)}^2+{\mathrm{\ω}}_k^b{\left(z\right)}^2},$ 定义给定的阈值条件为|ω_k|＜th_ωmax＝50°/s。

在行人运动过程中，z轴加速度和y轴角速度变化最明显，由于安装偏差、行走晃动等影响，IMU会有所倾斜。因此在静止时刻y轴角速度不是完全零值，z轴加速度也不是完全g值，但都不会大于所设定阈值。条件(2)和条件(4)中的阈值在初始静止阶段分别以取加速度计输出的平均值和陀螺仪输出的平均值而设定。

进一步的，如图3自适应卡尔曼滤波框图所示。其中包括捷联惯性导航信息(3-1)、磁力计的地磁信息(3-2)、时变噪声统计估计器(3-3)和滤波增益(3-4)。自适应卡尔曼滤波是利用观测数据进行递推滤波的同时，通过时变噪声统计估计器实时估计和修正系统噪声以及观测噪声的统计特性，从而降低模型误差、抑制滤波发散，提高滤波精度。

自适应滤波的算法描述为：

$\left\{\begin{array}{c}{\hat{X}}_k={\hat{X}}_{k/k-1}+K_k{\tilde{Z}}_k\\ {\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+{\hat{q}}_{k-1}\\ {\tilde{Z}}_k=Z_k-H_k{\hat{X}}_{k/k-1}-{\hat{r}}_k\\ K_k=P_{k/k-1}{H}_k^T{[H_k{P}_{k/k-1}{H}_k^T+{\hat{R}}_k]}^{-1}\\ P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\hat{Q}}_{k-1}\\ P_k=[I-K_k{H}_k]P_{k/k-1}\end{array}\right.$

上式中，分别是k时刻系统状态X_k的当前和预测线性最小方差估计值，是k-1时刻的系统状态X_k-1的当前线性最小方差估计值；Φ_k，k-1是状态过渡矩阵，是Φ_k，k-1的转置矩阵；H_k是观测矩阵，Z_k是观测量，是观测量估计误差；K_k是滤波增益；P_k、P_k/k-1分别是X_k和X_k/k-1对应的状态估计协方差阵；分别是系统过程噪声和观测噪声的时变均值，分别是系统过程噪声和观测噪声的时变协方差阵。

其中，由时变噪声统计估值器递推获得：

$\left\{\begin{array}{c}{\hat{r}}_{k+1}=(1-d_k){\hat{r}}_k+d_k[Z_{k+1}-H_{k+1,k}{\hat{X}}_{k+1,k}]\\ {\hat{R}}_{k+1}=(1-d_k){\hat{R}}_k+d_k[{\tilde{Z}}_{k+1}{\tilde{Z}}_{k+1}^T-H_{k+1,k}{P}_{k+1,k}{H}_{k+1,k}^T]\\ {\hat{q}}_{k+1}=(1-d_k){\hat{q}}_k+d_k[{\hat{X}}_{k+1}-{\mathrm{\Φ}}_{k+1,k}{\hat{X}}_k]\\ {\hat{Q}}_{k+1}=(1-d_k){\hat{Q}}_k+d_k[K_{k+1}{\tilde{Z}}_{k+1}{\tilde{Z}}_{k+1}^T{K}_{k+1}^T+P_{k+1}-{\mathrm{\Φ}}_{k+1,k}{P}_k{\hat{X}}_k{\mathrm{\Φ}}_{k+1,k}^T]\end{array}\right.$

式中，0＜b＜1为遗忘因子，针对本方法取b＝0.98。

此处分母中的b^k+1表示b的(k+1)次幂。k为离散时间点，k＝1，2，3…N，N为采样次数。

步骤(4)，通过公式 $\hat{v}={\hat{v}}^{-}+\mathrm{\δv},\hat{r}={\hat{r}}^{-}+\mathrm{\δr}$ 和 $\widehat{\mathrm{\ψ}}={\widehat{\mathrm{\ψ}}}^{-}+\mathrm{\δ\ψ}$ 反馈修正行人速度、位置和姿态信息。

如图4为行人自主导航定位的自适应卡尔曼滤波方法的实施例结果对比图：

实验中，将自研的集成三轴加速度计、陀螺仪和磁力计的MEMS-IMU固定于人体脚部，其中各传感器参数如表1所示。

表1传感器参数

实验中，行人绕某花坛外侧行走一圈，约230m。图4给出了利用本发明行人自主导航定位的自适应卡尔曼滤波方法进行行人导航的结果，并与只有纯惯导时的轨迹结果图作比较。其中左侧为纯惯性导航时的轨迹，可看出轨迹发散情况严重。右侧为应用自适应卡尔曼滤波后的轨迹图，可以看出此方法的收敛效果极好，最终得到了较为平滑并且始末点重复性较高的行进轨迹图。经过多个人多种运动速度试验，此发明的水平定位误差为1.35％(3σ)。此实施例表明本发明行人自主导航定位的自适应卡尔曼滤波方法可显著提高行人导航定位的精度。

Claims (6)

1.一种行人自主导航定位的自适应卡尔曼滤波方法，其特征在于：包括以下步骤：

步骤(1)，将MEMS-IMU系统固定连接于人体，IMU初始对准后，行人开始运动，运动过程中采集IMU数据；

步骤(2)，合理选取观测量建立融合人体运动特征的自适应卡尔曼滤波模型；

步骤(3)，同时满足“四条件”时进行自适应卡尔曼滤波，同步修正晃动噪声干扰，完成各状态量的估计。

步骤(4)，利用估计误差修正运动人体信息。

2.根据权利要求1所述的行人自主导航定位的自适应卡尔曼滤波方法，其特征在于：步骤(1)中所述的初始对准选取北东地为导航坐标系，俯仰角θ和滚转角γ由经过补偿后的加速度计输出值计算获得，偏航角由磁场强度B、俯仰角θ和滚转角γ计算获得。

3.根据权利要求1所述的行人自主导航定位的自适应卡尔曼滤波方法，其特征在于：在步骤(2)中，滤波模型为包括3个位置误差、3个姿态角误差(俯仰角θ、滚转角γ和偏航角)、3个速度误差、3个重力不确定误差、三轴陀螺仪零偏误差和三轴加速度计零偏误差，共计18维的状态空间方程。

$\left\{\begin{array}{c}\stackrel{.}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)\\ Z\left(t\right)=H\left(t\right)X\left(t\right)+V\left(t\right)\end{array}\right.$

W(t)为系统过程噪声矩阵，其表达式为： $W\left(t\right)=\left[\begin{array}{cc}-C_b^n{\mathrm{\ω}}^b& -C_b^n{a}^b\end{array}\right];$ 其中为行人坐标系b系到导航坐标系n系的方向余弦矩阵，ω^b为行人角速度，a^b为行人加速度；G(t)为对应的噪声矩阵系数；F(t)为系统状态矩阵；Z(t)为观测量；H(t)为观测矩阵；V(t)为观测噪声矩阵。

被估计状态向量为： $X\left(t\right)={\left[\begin{array}{cccccc}\mathrm{\δr}& \mathrm{\δv}& \mathrm{\δ\ψ}& {\▿}_b& {\mathrm{\ϵ}}_b& \mathrm{\δg}\end{array}\right]}^T$

其中，δr为位置误差，δv为速度误差，δψ为姿态角误差，为加速度计零偏误差，ε_b为陀螺仪零偏误差，δg为重力不确定误差。

观测向量和观测矩阵为：

Z_k＝[Δv_kΔω_kΔψ_k]^T

$H=\left[\begin{array}{cccccc}{O}_{3\×3}& I_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}\\ O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& I_{3\×3}& O_{3\×3}\\ O_{3\×3}& O_{3\×3}& I_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}\end{array}\right]$

其中，Δψ_k包括磁力计计算的航向角与捷联解算出的航向角作差获得的航向角误差，利用加速度计计算出的俯仰角和横滚角与捷联解算出的姿态角作差获得的俯仰角和横滚角误差。

4.根据权利要求1所述的行人自主导航定位的自适应卡尔曼滤波方法，其特征在于：所述的步骤(3)中“四条件”包括：

(1)三轴加速度计合成的加速度幅值输出量满足阈值条件；

(2)加速度计z轴加速度输出量满足阈值条件；

(3)三轴陀螺仪合成的角速度幅值输出量满足阈值条件；

(4)陀螺仪y轴角速度输出量满足阈值条件；

当上述四项条件同时满足时，进行自适应滤波。

5.根据权利要求1所述的行人自主导航定位的自适应卡尔曼滤波方法，其特征在于：所述的步骤(3)中的自适应卡尔曼滤波是利用观测数据进行递推滤波的同时，通过时变噪声统计估计器实时估计和修正系统噪声以及观测噪声的统计特性，从而降低模型误差、抑制滤波发散，提高滤波精度。

自适应卡尔曼滤波的算法描述为：

$\left\{\begin{array}{c}{\hat{X}}_k={\hat{X}}_{k/k-1}+K_k{\tilde{Z}}_k\\ {\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+{\hat{q}}_{k-1}\\ {\tilde{Z}}_k=Z_k-H_k{\hat{X}}_{k/k-1}-{\hat{r}}_k\\ K_k=P_{k/k-1}{H}_k^T{[H_k{P}_{k/k-1}{H}_k^T+{\hat{R}}_k]}^{-1}\\ P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\hat{Q}}_{k-1}\\ P_k=[I-K_k{H}_k]P_{k/k-1}\end{array}\right.$

上式中，分别是k时刻系统状态X_k的当前和预测线性最小方差估计值，是k-1时刻的系统状态X_k-1的当前线性最小方差估计值；Φ_k，k-1是状态过渡矩阵，是Φ_k，k-1的转置矩阵；H_k是观测矩阵，Z_k是观测量，是观测量估计误差；K_k是滤波增益；P_k、P_k/k-1分别是X_k和X_k/k-1对应的状态估计协方差阵；分别是系统过程噪声和观测噪声的时变均值，分别是系统过程噪声和观测噪声的时变协方差阵。

其中，由时变噪声统计估值器递推获得：

$\left\{\begin{array}{c}{\hat{r}}_{k+1}=(1-d_k){\hat{r}}_k+d_k[Z_{k+1}-H_{k+1,k}{\hat{X}}_{k+1,k}]\\ {\hat{R}}_{k+1}=(1-d_k){\hat{R}}_k+d_k[{\tilde{Z}}_{k+1}{\tilde{Z}}_{k+1}^T-H_{k+1,k}{P}_{k+1,k}{H}_{k+1,k}^T]\\ {\hat{q}}_{k+1}=(1-d_k){\hat{q}}_k+d_k[{\hat{X}}_{k+1}-{\mathrm{\Φ}}_{k+1,k}{\hat{X}}_k]\\ {\hat{Q}}_{k+1}=(1-d_k){\hat{Q}}_k+d_k[K_{k+1}{\tilde{Z}}_{k+1}{\tilde{Z}}_{k+1}^T{K}_{k+1}^T+P_{k+1}-{\mathrm{\Φ}}_{k+1,k}{P}_k{\hat{X}}_k{\mathrm{\Φ}}_{k+1,k}^T\end{array}\right.$

式中，0＜b＜1为遗忘因子，针对本方法取b＝0.98。

此处分母中的b^k+1表示b的(k+1)次幂。k为离散时间点，k＝1，2，3…N，N为采样次数。

6.根据权利要求1所述的行人自主导航定位的自适应卡尔曼滤波方法，其特征在于：所述的步骤(4)中，通过公式和反馈修正行人速度、位置和姿态信息。