CN106225784A - 基于低成本多传感器融合行人航位推算方法 - Google Patents

Abstract

本发明公开了基于低成本多传感器融合行人航位推算方法，给出了详细的相关方法流程，以解决因低成本传感器精度低，硬件质量差及易受外界干扰等缺点。提出了利用扩展卡尔曼滤波(EKF，extended Kalman Filter)与零速度更新(ZUPT，Zero Velocity Update)和磁力计结合的方法，用于步态检测、修正速度误差，并且对通过加速度计和磁力计对陀螺仪进行误差修正，同时对航向角误差修正。经实际测试，利用该方法可以较好的满足室内行人定位要求，定位误差占总路程的2%左右。该方法利用多传感器融合，可大幅度减少多传感器精度低带来的误差，并且较惯性测量单元能可有效的提升姿态精度。该方法在机器人、基于位置的服务(LBS，Location‑Basic Service)、室内行人航位推算等都有很高的应用前景与价值，具有广泛的实用性和通用性。

Description

基于低成本多传感器融合行人航位推算方法

技术领域

本发明涉及计算机科学、控制科学、室内定位系统、惯性导航系统等技术领域。

背景技术

行人航位推算(Pedestrian Dead Reckoning，PDR)系统是目前实现精确室内定位的主要系统；基于惯性测量单元(Inertial Measurement Unit，IMU)，利用加速度计和陀螺仪推算下一时刻行人的位置，无需任何外来信息，也不向外辐射任何信息，拥有良好的自主性、隐蔽性和数据更新速度快等特点。加速度计可以用来测量行人的运动加速度，利用已知的初始位置和速度，对加速度进行积分计算，获取行人的速度和位置等信息；陀螺仪测量行人的角运动，获取行人角度的变换信息，最终获取行人的方位等信息；但是由于低成本惯性测量单元精度较低，测量误差会随时间累积增大，最终造成较大的位置偏差。如：目前被广泛使用的微机电系统(Micro Electro Mechanical System，MEMS)，单纯的测量单元输出的数据夹杂着大量噪声，对定位结果有很大影响；对于精度比较高的惯性测量单元，如：激光惯导设备精度在0.1°/h，虽然精度高，但是价格昂贵而且体积大，不便用于室内行人定位。

目前广泛用于室内定位还有利用无线传感网络的方式，即事先布置AP节点，通过行人接收端采集到的无线信号值来判断行人在室内的位置信息；但此方法事先需进行大量的专业室内数据测量和人工的信号采集，依赖于AP硬件的部署，需耗费大量的前期数据库构造成本。

在申请专利“行人步长估计及航位推算方法(申请号：2013103884669)”中提到“利用加速度计测量行人行走时个人步长的参数，同时检测行走过程中加速度幅值的最大值a_max和最小值a_min，计算行人行走一步的步长”，该申请主要是利用非线性步长模型，估算步伐状态和步长，从而推算行人的位置；但是该专利未提出步伐频率不同对步长影响的分析，所以一旦行人在行走过程中，步伐频率不定，势必造成行人位置的偏差。

发明内容

本发明公开了一种基于低成本多传感器融合行人航位推算方法，以解决因低成本传感器精度低，硬件质量差及易受外界干扰等缺点。提出了利用扩展卡尔曼滤波与零速度更新和磁力计结合的方法，用于步态检测、修正速度误差，并且对通过加速度计和磁力计对陀螺仪进行误差修正，同时对航向角误差修正。

本发明是通过下述技术方案得以实现的：

基于低成本多传感器融合行人航位推算方法，包括下列步骤：

1)静态粗对准

1.1)若i表示惯性坐标系，e表示地球坐标系。表示e系相对i系的角速度在b系下的分量。表示b系相对e系的角速度在b系下的分量,表示b系相对i系的加速度在b系下的分量。静态初始时刻速度为0，加速度计的输出为：

$a_{ib}^b=v^b+(2{\mathrm{\ω}}_{ie}^b+{\mathrm{\ω}}_{eb}^b)\×v^b-g^b=-g^b=-C_n^b{g}^n=g\·\left[\begin{array}{c}-sin\mathrm{\θ}\\ sin\mathrm{\φ}cos\mathrm{\θ}\\ \cos \mathrm{\φ}cos\mathrm{\θ}\end{array}\right]$

1.2)俯仰角θ和横滚角φ分别为：

$\mathrm{\θ}=-arcsin(a_{ibx}^b/g)$

$\mathrm{\φ}=arcsin(a_{iby}^b/(g\·cos\mathrm{\θ}\left)\right)$

1.3)采用三轴磁力计来计算航向角：

$tan\mathrm{\Δ}\mathrm{\ψ}=\frac{m_{y}cos\mathrm{\φ}-m_z}{m_{x}cos\mathrm{\θ}+m_y\sin \mathrm{\φ}+m_{z}cos\mathrm{\φ}}$

m_x、m_y和m_z分别代表磁力计三个方向的输出值，所以航向角为：

ψ＝Δψ+D

D为本地磁偏角；

1.4)利用初始姿态角获取方向余弦矩阵，g是本地重力加速度，b表示载体坐标系，n表示导航坐标系(选择北西天地理坐标系)，表示从导航坐标系到载体坐标系的方向余弦矩阵；

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\θ}\\ -\cos \mathrm{\φ}\sin \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\cos \mathrm{\θ}\\ \sin \mathrm{\φ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\φ}\cos \mathrm{\ψ}+\cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$

gⁿ＝[0 0 -g]′

1.5)四元数与姿态矩阵的关系：

$C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$

联立步骤1.4)和步骤1.5)可解得：

$q_0=\frac{1}{2}\·\sqrt{1+C_n^b(1,1)-C_n^b(2,2)-C_n^b(3,3)}$

$q_1=\frac{1}{2}\·\sqrt{1-C_n^b(1,1)+C_n^b(2,2)-C_n^b(3,3)}$

$q_2=\frac{1}{2}\·\sqrt{1-C_n^b(1,1)-C_n^b(2,2)+C_n^b(3,3)}$

$q_3=\frac{1}{2}\·\sqrt{1+C_n^b(1,1)+C_n^b(2,2)+C_n^b(3,3)}$

q＝[q₀ q₁ q₂ q₃]

1.6)对四元数进行共轭处理：

$\stackrel{\‾}{q}=\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\end{array}\right]$

1.7)陀螺转子相对地球的自转角速率：

${\mathrm{\ω}}_{ie}^n={\left[\begin{array}{c}{\mathrm{\ω}}_{ie}\·\cos L\\ 0\\ {\mathrm{\ω}}_{ie}\·\sin L\end{array}\right]}^{\′}$

1.8)陀螺转子四元数：

${\tilde{q}}_{\mathrm{\ω}}=\stackrel{\‾}{q}\⊗\left[\begin{array}{cc}0& {\mathrm{\ω}}_{ie}^n\end{array}\right]$

1.9)陀螺仪y系下的矢量：

${\mathrm{\ω}}_{ie}^y=q\⊗{\tilde{q}}_{\mathrm{\ω}}$

1.10)初始时刻角增量的偏差：

$\mathrm{\ϵ}\mathrm{\ω}=\mathrm{\ω}-\left[\begin{array}{ccc}{\mathrm{\ω}}_{ie}^y(2,1)& {\mathrm{\ω}}_{ie}^y(3,1)& {\mathrm{\ω}}_{ie}^y(4,1)\end{array}\right]$

1.11)加速度计四元数：

${\tilde{q}}_a=\stackrel{\‾}{q}\⊗\left[\begin{array}{cc}0& g^n\end{array}\right]$

1.12)加速度计y系下的矢量：

$a_i^y=q\⊗{\tilde{q}}_a$

1.13)初始时刻速度计的偏差：

$\mathrm{\ϵ}a=a-\left[\begin{array}{ccc}{a}_i^y(2,1)& a_i^y(3,1)& a_i^y(4,1)\end{array}\right]$

1.14)转入步骤2)。

2)静态精对准

遍历静止时每一时刻的加速度、角速率、时间间隔和加速度漂移，根据子样进行角速度和加速度拟合,更新系统的角增量和速度增量；

2.1)时间间隔Δt＝1/f，f为采样频率；

2.2)采样样本数n_sample＝f·t，t初始对准时长；

2.3)样本间隔时间矩阵I为单位矩阵；

2.4)速度间隔时间矩阵

2.5)增量时间矩阵

2.6)角增量

2.7)速度增量

2.8)角增量求和

${\mathrm{\ω}}_{sum}=\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,1)+\mathrm{\Δ}\mathrm{\ω}(1,2)\\ \mathrm{\Δ}\mathrm{\ω}(2,1)+\mathrm{\Δ}\mathrm{\ω}(2,2)\\ \mathrm{\Δ}\mathrm{\ω}(3,1)+\mathrm{\Δ}\mathrm{\ω}(3,2)\end{array}\right]$

2.9)速度增量求和

$v_{sum}=\left[\begin{array}{c}\mathrm{\Δ}v(1,1)+\mathrm{\Δ}v(1,2)\\ \mathrm{\Δ}v(2,1)+\mathrm{\Δ}v(2,2)\\ \mathrm{\Δ}v(3,1)+\mathrm{\Δ}v(3,2)\end{array}\right]$

2.10)可由步骤2.6)和步骤2.8)，求得圆锥补偿后的角增量：

$\▿\mathrm{\ω}={\mathrm{\ω}}_{sum}+\frac{2}{3}\·\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,1)\\ \mathrm{\Δ}\mathrm{\ω}(2,1)\\ \mathrm{\Δ}\mathrm{\ω}(3,1)\end{array}\right]\⊗\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,2)\\ \mathrm{\Δ}\mathrm{\ω}(2,2)\\ \mathrm{\Δ}\mathrm{\ω}(3,2)\end{array}\right]$

2.11)可由步骤2.6)～步骤2.9)，求得划船补偿后的速度增量：

$\▿v=v_{sum}+\frac{1}{2}\·v_{sum}\⊗{\mathrm{\ω}}_{sum}+\frac{2}{3}\·\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,1)\\ \mathrm{\Δ}\mathrm{\ω}(2,1)\\ \mathrm{\Δ}\mathrm{\ω}(3,1)\end{array}\right]\⊗\left[\begin{array}{c}\mathrm{\Δ}v(1,2)\\ \mathrm{\Δ}v(2,2)\\ \mathrm{\Δ}v(3,2)\end{array}\right]+\left[\begin{array}{c}\mathrm{\Δ}v(1,1)\\ \mathrm{\Δ}v(2,1)\\ \mathrm{\Δ}v(3,1)\end{array}\right]\⊗\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,2)\\ \mathrm{\Δ}\mathrm{\ω}(2,2)\\ \mathrm{\Δ}\mathrm{\ω}(3,2)\end{array}\right]$

2.12)速度四元数：

${\tilde{q}}_v=q\⊗\left[\begin{array}{cc}0& \▿v\end{array}\right]$

2.13)加速度计y系下的矢量：

$v_i^y={\tilde{q}}_a\⊗\stackrel{\‾}{q}$

2.14)更新后的速度：

v₀为初始速度；

2.15)更新后的四元数

如果为0，则否则

2.16)更新后的位置：

r_更新＝r₀+(v₀+v_更新)/2·Δt

r₀为初始位置；

2.17)EKF的量测方程为：

Z_k＝HδX(k,k)+n_k

Z_k是量测值，H是量测矩阵，n_k是量测噪声，其方差矩阵为Q_k；

2.18)EKF的系统误差状态为：

δX(k,k)＝δX(k,k-1)+K_k[Z_k-HδX(k,k-1)]

2.19)EKF的系统状态方程为：

δX(k,k-1)＝Φ_kδX(k-1,k-1)+w_k-1

δX(k,k-1)是k时刻的预测误差状态，δX(k-1,k-1)是k-1时刻的估计误差状态。w_k-1是k-1时刻的系统噪声，其方差矩阵是R_k；

2.20)Φ_k是15阶的误差状态转移矩阵：

${\mathrm{\Φ}}_k=\left[\begin{array}{ccccc}{I}_{3\×3}& \mathrm{\Δ}t\×C_b^n(k,k-1)& 0& 0& 0\\ 0& I_{3\×3}& 0& 0& 0\\ 0& 0& I_{3\×3}& \mathrm{\Δ}t\×I_3& 0\\ -\mathrm{\Δ}t\×S\left(\stackrel{\‾}{a_k^n}\right)& 0& 0& I_{3\×3}& \mathrm{\Δ}t\×C_b^n(k,k-1)\\ 0& 0& 0& 0& I_{3\×3}\end{array}\right]$

2.21)是的反对称矩阵：

$\stackrel{\‾}{a_k^n}=C_b^n(k,k-1)\×\stackrel{\‾}{a_k^b}$

$S\left(\stackrel{\‾}{a_k^n}\right)=\left[\begin{array}{ccc}0& -\stackrel{\‾}{a_k^n}\left(3\right)& \stackrel{\‾}{a_k^n}\left(2\right)\\ \stackrel{\‾}{a_k^n}\left(3\right)& 0& -\stackrel{\‾}{a_k^n}\left(1\right)\\ -\stackrel{\‾}{a_k^n}\left(2\right)& \stackrel{\‾}{a_k^n}\left(1\right)& 0\end{array}\right]$

2.22)EKF的状态误差向量为：

其中，δX_k|k为k时刻的系统状态误差，和分别表示k时刻加速度三维误差值、角速度三维误差值、位置三维误差值、速度三维误差值和姿态三维误差值；

2.23)在静态初始对准时刻，位置和速度的增量都为零，修正的量测值为：

$Z_k={\left[\begin{array}{cccc}{\mathrm{\ω}}_{ibk}^b-C_{n\left(k\right|k-1)}^b{\mathrm{\ω}}_{ie}^n& r_k^n-r_0^n& v_k^n-0_{3\×1}& {\mathrm{\ψ}}_{k|k-1}-{\mathrm{\ψ}}_k^{mag}\end{array}\right]}^{\′}$

其中，ψ_k|k-1为k时刻未修正的航向角，为k时刻由磁力计计算得到的航向角，和分别为k时刻陀螺仪的角速度和地球自转角速度在n系的投影，和分别为k时刻的位置和初始的位置；

2.24)相对应的量测矩阵为：

H＝[0_10×2 Ι_10×10 0_10×3]

2.25)滤波增益方程：

K_k＝P_k|k-1H_k′(H_kP_k|k-1H_k′+R_k)^-1

2.26)一步预测均方误差方程：

P_k|k-1＝Φ_k-1P_k-1|k-1Φ_k-1 ^T+Q_k-1

2.27)估计均方误差方程：

P_k|k＝(I_15×15-K_kH)P_k|k-1(I_15×15-K_kH)′+K_kR_kK'_k

其中，下标k|k-1表示由k-1时刻到k时刻的预测值，下标k|k表示k时刻的估计值；

2.28)依次循环遍历步骤2.1)～2.27)，从而获取精准的初始姿态、状态估计误差和协方差矩阵；进入步骤3)。

3)步态检测

3.1)k时刻加速度的振幅：

$\left|a_k\right|=\sqrt{{a_{x\left(k\right)}^b}^2+{a_{y\left(k\right)}^b}^2+{a_{z\left(k\right)}^b}^2}$

3.2)k时刻加速度的均值：

$\stackrel{\‾}{a_k^b}=\frac{1}{2s+1}{\Σ}_{q=k-s}^{k+s}\left|a_q\right|$

其中，代表k时刻平均加速度振幅，s＝15为平滑窗口大小；

3.3)加速度的振幅方差：

$\mathrm{\σ}{\left(a_k^b\right)}^2=\frac{1}{2s+1}\underset{j=k-s}{\overset{k+s}{\Σ}}{\left(\right|a_j|-\stackrel{\‾}{a_k^b})}^2$

3.4)角速度的振幅：

$\left|w_k\right|=\sqrt{{w_{x\left(k\right)}^b}^2+{w_{y\left(k\right)}^b}^2+{w_{z\left(k\right)}^b}^2}$

3.5)角速度的均值：

$\stackrel{\‾}{w_k^b}=\frac{1}{2s+1}{\Σ}_{q=k-s}^{k+s}\left|w_q\right|$

3.6)角速度的振幅方差：

$\mathrm{\σ}{\left(w_k^b\right)}^2=\frac{1}{2s+1}\underset{j=k-s}{\overset{k+s}{\Σ}}{\left(\right|w_j|-\stackrel{\‾}{w_k^b})}^2$

3.7)步伐状态判定阈值：

3.8)循环遍历步骤3.1)～3.7)，将从平稳进入移动的时刻，记为k_i时刻；进入步骤4)。

4)ZUPT与磁力计

4.1)时间间隔Δt＝1/f，f为采样频率；

4.2)采样样本数n_sample＝f·t，t初始对准时长；

4.3)样本间隔时间矩阵I为单位矩阵；

4.4)速度间隔时间矩阵

4.5)增量时间矩阵

4.6)角增量

4.7)速度增量

4.8)角增量求和：

${\mathrm{\ω}}_{sum}=\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,1)+\mathrm{\Δ}\mathrm{\ω}(1,2)\\ \mathrm{\Δ}\mathrm{\ω}(2,1)+\mathrm{\Δ}\mathrm{\ω}(2,2)\\ \mathrm{\Δ}\mathrm{\ω}(3,1)+\mathrm{\Δ}\mathrm{\ω}(3,2)\end{array}\right]$

4.9)速度增量求和：

$v_{sum}=\left[\begin{array}{c}\mathrm{\Δ}v(1,1)+\mathrm{\Δ}v(1,2)\\ \mathrm{\Δ}v(2,1)+\mathrm{\Δ}v(2,2)\\ \mathrm{\Δ}v(3,1)+\mathrm{\Δ}v(3,2)\end{array}\right]$

4.10)可由步骤4.6)和步骤4.8)，求得圆锥补偿后的角增量：

$\▿\mathrm{\ω}={\mathrm{\ω}}_{sum}+\frac{2}{3}\·\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,1)\\ \mathrm{\Δ}\mathrm{\ω}(2,1)\\ \mathrm{\Δ}\mathrm{\ω}(3,1)\end{array}\right]\⊗\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,2)\\ \mathrm{\Δ}\mathrm{\ω}(2,2)\\ \mathrm{\Δ}\mathrm{\ω}(3,2)\end{array}\right]$

4.11)可由步骤4.6)～步骤4.9)，求得划船补偿后的速度增量：

$\▿v=v_{sum}+\frac{1}{2}\·v_{sum}\⊗{\mathrm{\ω}}_{sum}+\frac{2}{3}\·\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,1)\\ \mathrm{\Δ}\mathrm{\ω}(2,1)\\ \mathrm{\Δ}\mathrm{\ω}(3,1)\end{array}\right]\⊗\left[\begin{array}{c}\mathrm{\Δ}v(1,2)\\ \mathrm{\Δ}v(2,2)\\ \mathrm{\Δ}v(3,2)\end{array}\right]+\left[\begin{array}{c}\mathrm{\Δ}v(1,1)\\ \mathrm{\Δ}v(2,1)\\ \mathrm{\Δ}v(3,1)\end{array}\right]\⊗\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,2)\\ \mathrm{\Δ}\mathrm{\ω}(2,2)\\ \mathrm{\Δ}\mathrm{\ω}(3,2)\end{array}\right]$

4.12)速度四元数：

${\tilde{q}}_v=q\⊗\left[\begin{array}{cc}0& \▿v\end{array}\right]$

4.13)加速度计y系下的矢量：

$v_i^y={\tilde{q}}_a\⊗\stackrel{\‾}{q}$

4.14)更新后的速度：

v₀为初始速度；

4.15)更新后的四元数：

如果为0，则否则

4.16)更新后的位置：

r_更新＝r₀+(v₀+v_更新)/2·Δt

r₀为初始位置；

4.17)判定该时刻是否为平稳进入移动的时刻，当进入k_i时刻，则在该时刻采用零速度修正(ZUPT)与磁力计组合，构造量测矩阵为：

$H=\left[\begin{array}{ccccc}\left[\begin{array}{ccc}0& 0& 1\end{array}\right]& 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×3}\end{array}\right]$

4.18)组合后的量测噪声为：

$R=\left[\begin{array}{ccccc}1\×{10}_{1\×3}^{-4}& 0_{1\×3}& 0_{1\×3}& 1\×{10}_{1\×3}^{-4}& 0_{1\×3}\end{array}\right]$

4.19)组合后的量测值为：

$Z_{k_i}=\left[\begin{array}{cc}{\mathrm{\ΔV}}_{k_i}& {\mathrm{\Δ\ψ}}_{k_i}\end{array}\right]$

4.20)EKF的量测方程为：

Z_k＝HδX(k,k)+n_k

4.21)EKF的系统误差状态为：

δX(k,k)＝δX(k,k-1)+K_k[Z_k-HδX(k,k-1)]

4.22)EKF的系统状态方程为：

δX(k,k-1)＝Φ_kδX(k-1,k-1)+w_k-1

4.23)滤波增益方程：

K_k＝P_k|k-1H_k′(H_kP_k|k-1H_k′+R_k)^-1

4.24)一步预测均方误差方程：

P_k|k-1＝Φ_k-1P_k-1|k-1Φ_k-1 ^T+Q_k-1

4.25)估计均方误差方程：

P_k|k＝(I_15×15-K_kH)P_k|k-1(I_15×15-K_kH)′+K_kR_kK'_k

4.26)根据δX(k,k)值，更新速度、位置和姿态信息；

4.27)循环遍历步骤4.1)～步骤4.26)，可得行人行走过程中的速度、位置和姿态。

本发明的有益效果

本发明提出并实现了基于低成本多传感器融合行人航位推算方法；该方法的优势在于使用低成本低精度多传感器，同样可以获得较理想的室内定位精度，验证了该发明的实用性和通用性。利用惯性测量单元采集行人线运动信息和角运动信息，三轴磁力计获取相应的感应电势。基于扩展卡尔曼滤波对各传感器误差补偿和修正，而后计算出行人准确的位置信息。

附图说明

图1本发明实施例中的流程图；

图2是图1中的初始对准图；

图3是图1中的捷联惯性导航图；

图4是图1中的步态检测图；

图5本发明实施例中实验结果对比图。

具体实施方式

下面结合附图和实施例对本发明做进一步的说明。

实施例1

基于低成本多传感器融合行人航位推算方法，包括下列步骤：

本发明完整的流程图见图1；

步骤一：初始粗对准获取姿态

1.1)若i表示惯性坐标系，e表示地球坐标系。表示e系相对i系的角速度在b系下的分量。表示b系相对e系的角速度在b系下的分量,表示b系相对i系的加速度在b系下的分量。静态初始时刻速度为0，加速度计的输出为：

$a_{ib}^b=v^b+(2{\mathrm{\ω}}_{ie}^b+{\mathrm{\ω}}_{eb}^b)\×v^b-g^b=-g^b=-C_n^b{g}^n=g\·\left[\begin{array}{c}-sin\mathrm{\θ}\\ sin\mathrm{\φ}cos\mathrm{\θ}\\ \cos \mathrm{\φ}cos\mathrm{\θ}\end{array}\right]$

1.2)俯仰角θ和横滚角φ分别为：

$\mathrm{\θ}=-arcsin(a_{ibx}^b/g)$

$\mathrm{\φ}=arcsin(a_{iby}^b/(g\·cos\mathrm{\θ}\left)\right)$

1.3)采用三轴磁力计来计算航向角：

$tan\mathrm{\Δ}\mathrm{\ψ}=\frac{m_{y}cos\mathrm{\φ}-m_z}{m_{x}cos\mathrm{\θ}+m_y\sin \mathrm{\φ}+m_{z}cos\mathrm{\φ}}$

m_x、m_y和m_z分别代表磁力计三个方向的输出值，所以航向角为：

ψ＝Δψ+D

D为本地磁偏角；

1.4)利用初始姿态角获取方向余弦矩阵，g是本地重力加速度，b表示载体坐标系，n表示导航坐标系(选择北西天地理坐标系)，表示从导航坐标系到载体坐标系的方向余弦矩阵；

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\θ}\\ -\cos \mathrm{\φ}\sin \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\cos \mathrm{\θ}\\ \sin \mathrm{\φ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\φ}\cos \mathrm{\ψ}+\cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$

gⁿ＝[0 0 -g]′

1.5)四元数与姿态矩阵的关系：

$C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$

联立步骤1.4)和步骤1.5)可解得：

$q_0=\frac{1}{2}\·\sqrt{1+C_n^b(1,1)-C_n^b(2,2)-C_n^b(3,3)}$

$q_1=\frac{1}{2}\·\sqrt{1-C_n^b(1,1)+C_n^b(2,2)-C_n^b(3,3)}$

$q_2=\frac{1}{2}\·\sqrt{1-C_n^b(1,1)-C_n^b(2,2)+C_n^b(3,3)}$

$q_3=\frac{1}{2}\·\sqrt{1+C_n^b(1,1)+C_n^b(2,2)+C_n^b(3,3)}$

q＝[q₀ q₁ q₂ q₃]

1.6)对四元数进行共轭处理：

$\stackrel{\‾}{q}=\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\end{array}\right]$

1.7)陀螺转子相对地球的自转角速率：

${\mathrm{\ω}}_{ie}^n={\left[\begin{array}{c}{\mathrm{\ω}}_{ie}\·\cos L\\ 0\\ {\mathrm{\ω}}_{ie}\·\sin L\end{array}\right]}^{\′}$

1.8)陀螺转子四元数：

${\tilde{q}}_{\mathrm{\ω}}=\stackrel{\‾}{q}\⊗\left[\begin{array}{cc}0& {\mathrm{\ω}}_{ie}^n\end{array}\right]$

1.9)陀螺仪y系下的矢量：

${\mathrm{\ω}}_{ie}^y=q\⊗{\tilde{q}}_{\mathrm{\ω}}$

1.10)初始时刻角增量的偏差：

$\mathrm{\ϵ}\mathrm{\ω}=\mathrm{\ω}-\left[\begin{array}{ccc}{\mathrm{\ω}}_{ie}^y(2,1)& {\mathrm{\ω}}_{ie}^y(3,1)& {\mathrm{\ω}}_{ie}^y(4,1)\end{array}\right]$

1.11)加速度计四元数：

${\tilde{q}}_a=\stackrel{\‾}{q}\⊗\left[\begin{array}{cc}0& g^n\end{array}\right]$

1.12)加速度计y系下的矢量：

$a_i^y=q\⊗{\tilde{q}}_a$

1.13)初始时刻速度计的偏差：

$\mathrm{\ϵ}a=a-\left[\begin{array}{ccc}{a}_i^y(2,1)& a_i^y(3,1)& a_i^y(4,1)\end{array}\right]$

1.14)转入步骤2)

步骤二：初始精对准获取姿态、状态估计误差、协方差矩阵(详细技术流程如图2所示)

遍历静止时每一时刻的加速度、角速率、时间间隔和加速度漂移，根据子样进行角速度和加速度拟合,更新系统的角增量和速度增量；

2.1)时间间隔Δt＝1/f，f为采样频率；

2.2)采样样本数n_sample＝f·t，t初始对准时长；

2.3)样本间隔时间矩阵I为单位矩阵；

2.4)速度间隔时间矩阵

2.5)增量时间矩阵

2.6)角增量

2.7)速度增量

2.8)角增量求和

${\mathrm{\ω}}_{sum}=\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,1)+\mathrm{\Δ}\mathrm{\ω}(1,2)\\ \mathrm{\Δ}\mathrm{\ω}(2,1)+\mathrm{\Δ}\mathrm{\ω}(2,2)\\ \mathrm{\Δ}\mathrm{\ω}(3,1)+\mathrm{\Δ}\mathrm{\ω}(3,2)\end{array}\right]$

2.9)速度增量求和

$v_{sum}=\left[\begin{array}{c}\mathrm{\Δ}v(1,1)+\mathrm{\Δ}v(1,2)\\ \mathrm{\Δ}v(2,1)+\mathrm{\Δ}v(2,2)\\ \mathrm{\Δ}v(3,1)+\mathrm{\Δ}v(3,2)\end{array}\right]$

2.10)可由步骤2.6)和步骤2.8)，求得圆锥补偿后的角增量：

$\▿\mathrm{\ω}={\mathrm{\ω}}_{sum}+\frac{2}{3}\·\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,1)\\ \mathrm{\Δ}\mathrm{\ω}(2,1)\\ \mathrm{\Δ}\mathrm{\ω}(3,1)\end{array}\right]\⊗\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,2)\\ \mathrm{\Δ}\mathrm{\ω}(2,2)\\ \mathrm{\Δ}\mathrm{\ω}(3,2)\end{array}\right]$

2.11)可由步骤2.6)～步骤2.9)，求得划船补偿后的速度增量：

$\▿v=v_{sum}+\frac{1}{2}\·v_{sum}\⊗{\mathrm{\ω}}_{sum}+\frac{2}{3}\·\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,1)\\ \mathrm{\Δ}\mathrm{\ω}(2,1)\\ \mathrm{\Δ}\mathrm{\ω}(3,1)\end{array}\right]\⊗\left[\begin{array}{c}\mathrm{\Δ}v(1,2)\\ \mathrm{\Δ}v(2,2)\\ \mathrm{\Δ}v(3,2)\end{array}\right]+\left[\begin{array}{c}\mathrm{\Δ}v(1,1)\\ \mathrm{\Δ}v(2,1)\\ \mathrm{\Δ}v(3,1)\end{array}\right]\⊗\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,2)\\ \mathrm{\Δ}\mathrm{\ω}(2,2)\\ \mathrm{\Δ}\mathrm{\ω}(3,2)\end{array}\right]$

2.12)速度四元数：

${\tilde{q}}_v=q\⊗\left[\begin{array}{cc}0& \▿v\end{array}\right]$

2.13)加速度计y系下的矢量：

$v_i^y={\tilde{q}}_a\⊗\stackrel{\‾}{q}$

2.14)更新后的速度：

v₀为初始速度；

2.15)更新后的四元数

如果为0，则否则

2.16)更新后的位置：

r_更新＝r₀+(v₀+v_更新)/2·Δt

r₀为初始位置；

2.17)EKF的量测方程为：

Z_k＝HδX(k,k)+n_k

Z_k是量测值，H是量测矩阵，n_k是量测噪声，其方差矩阵为Q_k；

2.18)EKF的系统误差状态为：

δX(k,k)＝δX(k,k-1)+K_k[Z_k-HδX(k,k-1)]

2.19)EKF的系统状态方程为：

δX(k,k-1)＝Φ_kδX(k-1,k-1)+w_k-1

δX(k,k-1)是k时刻的预测误差状态，δX(k-1,k-1)是k-1时刻的估计误差状态。w_k-1是k-1时刻的系统噪声，其方差矩阵是R_k；

2.20)Φ_k是15阶的误差状态转移矩阵：

${\mathrm{\Φ}}_k=\left[\begin{array}{ccccc}{I}_{3\×3}& \mathrm{\Δ}t\×C_b^n(k,k-1)& 0& 0& 0\\ 0& I_{3\×3}& 0& 0& 0\\ 0& 0& I_{3\×3}& \mathrm{\Δ}t\×I_3& 0\\ -\mathrm{\Δ}t\×S\left(\stackrel{\‾}{a_k^n}\right)& 0& 0& I_{3\×3}& \mathrm{\Δ}t\×C_b^n(k,k-1)\\ 0& 0& 0& 0& I_{3\×3}\end{array}\right]$

2.21)是的反对称矩阵：

$\stackrel{\‾}{a_k^n}=C_b^n(k,k-1)\×\stackrel{\‾}{a_k^b}$

$S\left(\stackrel{\‾}{a_k^n}\right)=\left[\begin{array}{ccc}0& -\stackrel{\‾}{a_k^n}\left(3\right)& \stackrel{\‾}{a_k^n}\left(2\right)\\ \stackrel{\‾}{a_k^n}\left(3\right)& 0& -\stackrel{\‾}{a_k^n}\left(1\right)\\ -\stackrel{\‾}{a_k^n}\left(2\right)& \stackrel{\‾}{a_k^n}\left(1\right)& 0\end{array}\right]$

2.22)EKF的状态误差向量为：

其中，δX_k|k为k时刻的系统状态误差，和分别表示k时刻加速度三维误差值、角速度三维误差值、位置三维误差值、速度三维误差值和姿态三维误差值；

2.23)在静态初始对准时刻，位置和速度的增量都为零，修正的量测值为：

$Z_k={\left[\begin{array}{cccc}{\mathrm{\ω}}_{ibk}^b-C_{n\left(k\right|k-1)}^b{\mathrm{\ω}}_{ie}^n& r_k^n-r_0^n& v_k^n-0_{3\×1}& {\mathrm{\ψ}}_{k|k-1}-{\mathrm{\ψ}}_k^{mag}\end{array}\right]}^{\′}$

其中，ψ_k|k-1为k时刻未修正的航向角，为k时刻由磁力计计算得到的航向角，和分别为k时刻陀螺仪的角速度和地球自转角速度在n系的投影，和分别为k时刻的位置和初始的位置；

2.24)相对应的量测矩阵为：

H＝[0_10×2 Ι_10×10 0_10×3]

2.25)滤波增益方程：

K_k＝P_k|k-1H_k′(H_kP_k|k-1H_k′+R_k)^-1

2.26)一步预测均方误差方程：

P_k|k-1＝Φ_k-1P_k-1|k-1Φ_k-1 ^T+Q_k-1

2.27)估计均方误差方程：

P_k|k＝(I_15×15-K_kH)P_k|k-1(I_15×15-K_kH)′+K_kR_kK'_k

其中，下标k|k-1表示由k-1时刻到k时刻的预测值，下标k|k表示k时刻的估计值；

2.28)依次循环遍历步骤2.1)～2.27)，从而获取精准的初始姿态、状态估计误差和协方差矩阵；进入步骤3)。

步骤三：确定步伐状态(详细判定如图3所示)

3.1)k时刻加速度的振幅：

$\left|a_k\right|=\sqrt{{a_{x\left(k\right)}^b}^2+{a_{y\left(k\right)}^b}^2+{a_{z\left(k\right)}^b}^2}$

3.2)k时刻加速度的均值：

$\stackrel{\‾}{a_k^b}=\frac{1}{2s+1}{\Σ}_{q=k-s}^{k+s}\left|a_q\right|$

其中，代表k时刻平均加速度振幅，s＝15为平滑窗口大小；

3.3)加速度的振幅方差：

$\mathrm{\σ}{\left(a_k^b\right)}^2=\frac{1}{2s+1}\underset{j=k-s}{\overset{k+s}{\Σ}}{\left(\right|a_j|-\stackrel{\‾}{a_k^b})}^2$

3.4)角速度的振幅：

$\left|w_k\right|=\sqrt{{w_{x\left(k\right)}^b}^2+{w_{y\left(k\right)}^b}^2+{w_{z\left(k\right)}^b}^2}$

3.5)角速度的均值：

$\stackrel{\‾}{w_k^b}=\frac{1}{2s+1}{\Σ}_{q=k-s}^{k+s}\left|w_q\right|$

3.6)角速度的振幅方差：

$\mathrm{\σ}{\left(w_k^b\right)}^2=\frac{1}{2s+1}\underset{j=k-s}{\overset{k+s}{\Σ}}{\left(\right|w_j|-\stackrel{\‾}{w_k^b})}^2$

3.7)步伐状态判定阈值：

3.8)循环遍历步骤3.1)～3.7)，将从平稳进入移动的时刻，记为k_i时刻；进入步骤4)。

步骤四：基于EKF求取更新后每一步的位置(详细技术流程如图4所示)

4.1)时间间隔Δt＝1/f，f为采样频率；

4.2)采样样本数n_sample＝f·t，t初始对准时长；

4.3)样本间隔时间矩阵I为单位矩阵；

4.4)速度间隔时间矩阵

4.5)增量时间矩阵

4.6)角增量

4.7)速度增量

4.8)角增量求和：

${\mathrm{\ω}}_{sum}=\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,1)+\mathrm{\Δ}\mathrm{\ω}(1,2)\\ \mathrm{\Δ}\mathrm{\ω}(2,1)+\mathrm{\Δ}\mathrm{\ω}(2,2)\\ \mathrm{\Δ}\mathrm{\ω}(3,1)+\mathrm{\Δ}\mathrm{\ω}(3,2)\end{array}\right]$

4.9)速度增量求和：

$v_{sum}=\left[\begin{array}{c}\mathrm{\Δ}v(1,1)+\mathrm{\Δ}v(1,2)\\ \mathrm{\Δ}v(2,1)+\mathrm{\Δ}v(2,2)\\ \mathrm{\Δ}v(3,1)+\mathrm{\Δ}v(3,2)\end{array}\right]$

4.10)可由步骤4.6)和步骤4.8)，求得圆锥补偿后的角增量：

$\▿\mathrm{\ω}={\mathrm{\ω}}_{sum}+\frac{2}{3}\·\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,1)\\ \mathrm{\Δ}\mathrm{\ω}(2,1)\\ \mathrm{\Δ}\mathrm{\ω}(3,1)\end{array}\right]\⊗\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,2)\\ \mathrm{\Δ}\mathrm{\ω}(2,2)\\ \mathrm{\Δ}\mathrm{\ω}(3,2)\end{array}\right]$

4.11)可由步骤4.6)～步骤4.9)，求得划船补偿后的速度增量：

$\▿v=v_{sum}+\frac{1}{2}\·v_{sum}\⊗{\mathrm{\ω}}_{sum}+\frac{2}{3}\·\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,1)\\ \mathrm{\Δ}\mathrm{\ω}(2,1)\\ \mathrm{\Δ}\mathrm{\ω}(3,1)\end{array}\right]\⊗\left[\begin{array}{c}\mathrm{\Δ}v(1,2)\\ \mathrm{\Δ}v(2,2)\\ \mathrm{\Δ}v(3,2)\end{array}\right]+\left[\begin{array}{c}\mathrm{\Δ}v(1,1)\\ \mathrm{\Δ}v(2,1)\\ \mathrm{\Δ}v(3,1)\end{array}\right]\⊗\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}(1,2)\\ \mathrm{\Δ}\mathrm{\ω}(2,2)\\ \mathrm{\Δ}\mathrm{\ω}(3,2)\end{array}\right]$

4.12)速度四元数：

${\tilde{q}}_v=q\⊗\left[\begin{array}{cc}0& \▿v\end{array}\right]$

4.13)加速度计y系下的矢量：

$v_i^y={\tilde{q}}_a\⊗\stackrel{\‾}{q}$

4.14)更新后的速度：

v₀为初始速度；

4.15)更新后的四元数：

如果为0，则否则

4.16)更新后的位置：

r_更新＝r₀+(v₀+v_更新)/2·Δt

r₀为初始位置；

4.17)判定该时刻是否为平稳进入移动的时刻，当进入k_i时刻，则在该时刻采用零速度修正(ZUPT)与磁力计组合，构造量测矩阵为：

$H=\left[\begin{array}{ccccc}\left[\begin{array}{ccc}0& 0& 1\end{array}\right]& 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×3}\end{array}\right]$

4.18)组合后的量测噪声为：

$R=\left[\begin{array}{ccccc}1\×{10}_{1\×3}^{-4}& 0_{1\×3}& 0_{1\×3}& 1\×{10}_{1\×3}^{-4}& 0_{1\×3}\end{array}\right]$

4.19)组合后的量测值为：

$Z_{k_i}=\left[\begin{array}{cc}{\mathrm{\ΔV}}_{k_i}& {\mathrm{\Δ\ψ}}_{k_i}\end{array}\right]$

4.20)EKF的量测方程为：

Z_k＝HδX(k,k)+n_k

4.21)EKF的系统误差状态为：

δX(k,k)＝δX(k,k-1)+K_k[Z_k-HδX(k,k-1)]

4.22)EKF的系统状态方程为：

δX(k,k-1)＝Φ_kδX(k-1,k-1)+w_k-1

4.23)滤波增益方程：

K_k＝P_k|k-1H_k′(H_kP_k|k-1H_k′+R_k)^-1

4.24)一步预测均方误差方程：

P_k|k-1＝Φ_k-1P_k-1|k-1Φ_k-1 ^T+Q_k-1

4.25)估计均方误差方程：

P_k|k＝(I_15×15-K_kH)P_k|k-1(I_15×15-K_kH)′+K_kR_kK'_k

4.26)根据δX(k,k)值，更新速度、位置和姿态信息；

4.27)循环遍历步骤4.1)～步骤4.26)，可得行人行走过程中的速度、位置和姿态。

实施例2

为了评估“基于低成本多传感器融合行人航位推算方法”在实际中定位情况，使用实施例1中的技术流程，使用的设备为苹果公司于2014年10月推出的iPad air 2产品。实验在室内的长方形会议厅内完成的，测试路线长度为50.4m，设置采样频率为100Hz，通过手持iPad方式行走采集数据，原始实验数据可以通过SensorLog软件获得。

实验绕着闭合路线行走，其中包含了3个90°的直角转弯，图5为某一次实验路线与真实路线的对比，由图可见在一开始直线行走过程中，实验算法路线与真实路线基本吻合，但当经过第二个90°转弯，ZUPT算法的方向发生了很大的偏移，慢慢地由于误差积累，最终造成后期实验位置与真实位置存在较大的偏差。

本实验路线利用实验终点和真实终点的距离误差与总路程的比值来表示定位误差。经过多次实验定位误差如表1所示。

表1定位误差分布

从表中结果对比可知，基于EKF、ZUPT与磁力计，通过惯性测量单元与磁力计融合，利用加速度计和磁力计对陀螺仪进行误差修正，并且对航向角误差修正；最终减小了航向角误差和速度误差积累，从而提高位置精度。

Claims (1)

1.一种基于低成本多传感器融合行人航位推算方法，其特征在于，包括下列步骤：

1)静态粗对准

1.1)i表示惯性坐标系，e表示地球坐标系，表示e系相对i系的角速度在b系下的分量，表示b系相对e系的角速度在b系下的分量，表示b系相对i系的加速度在b系下的分量，静态初始时刻速度为0，加速度计的输出为：

$a_{ib}^b=v^b+\left(2{\mathrm{\ω}}_{ie}^b+{\mathrm{\ω}}_{eb}^b\right)\×v^b-g^b=-g^b=-C_n^b{g}^n=g\·\left[\begin{array}{c}-\sin \mathrm{\θ}\\ \sin \mathrm{\φ}\cos \mathrm{\θ}\\ \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$

1.2)俯仰角θ和横滚角φ分别为：

$\mathrm{\θ}=-\mathrm{arc}sin\left(a_{ibx}^b/g\right)$

$\mathrm{\φ}=\mathrm{arc}sin\left(a_{iby}^b/\left(g\·\cos \mathrm{\θ}\right)\right)$

1.3)采用三轴磁力计来计算航向角：

$\tan \mathrm{\Δ}\mathrm{\ψ}=\frac{m_y\cos \mathrm{\φ}-m_z}{m_x\cos \mathrm{\θ}+m_y\sin \mathrm{\φ}+m_z\cos \mathrm{\φ}}$

m_x、m_y和m_z分别代表磁力计三个方向的输出值，所以航向角为：

ψ＝Δψ+D

D为本地磁偏角；

1.4)利用初始姿态角获取方向余弦矩阵，g是本地重力加速度，b表示载体坐标系，n表示导航坐标系，表示从导航坐标系到载体坐标系的方向余弦矩阵；

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\θ}\\ -\cos \mathrm{\φ}\sin \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\cos \mathrm{\θ}\\ \sin \mathrm{\φ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\φ}\cos \mathrm{\ψ}+\cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$

gⁿ＝[0 0 -g]′

1.5)四元数与姿态矩阵的关系：

$C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2\left(q_1{q}_2-q_0{q}_3\right)& 2\left(q_1{q}_3+q_0{q}_2\right)\\ 2\left(q_1{q}_2+q_0{q}_3\right)& q_0^2-q_1^2+q_2^2-q_3^2& 2\left(q_2{q}_3-q_0{q}_1\right)\\ 2\left(q_1{q}_3-q_0{q}_2\right)& 2\left(q_2{q}_3+q_0{q}_1\right)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$

联立步骤1.4)和步骤1.5)可解得：

$q_0=\frac{1}{2}\·\sqrt{1+C_n^b\left(1,1\right)-C_n^b\left(2,2\right)-C_n^b\left(3,3\right)}$

$q_1=\frac{1}{2}\·\sqrt{1-C_n^b\left(1,1\right)+C_n^b\left(2,2\right)-C_n^b\left(3,3\right)}$

$q_2=\frac{1}{2}\·\sqrt{1-C_n^b\left(1,1\right)-C_n^b\left(2,2\right)+C_n^b\left(3,3\right)}$

$q_3=\frac{1}{2}\·\sqrt{1+C_n^b\left(1,1\right)+C_n^b\left(2,2\right)+C_n^b\left(3,3\right)}$

q＝[q₀ q₁ q₂ q₃]

1.6)对四元数进行共轭处理：

$\stackrel{\‾}{q}=\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\end{array}\right]$

1.7)陀螺转子相对地球的自转角速率：

${\mathrm{\ω}}_{ie}^n={\left[\begin{array}{c}{\mathrm{\ω}}_{ie}\·\cos L\\ 0\\ {\mathrm{\ω}}_{ie}\·\sin L\end{array}\right]}^{\′}$

1.8)陀螺转子四元数：

${\tilde{q}}_{\mathrm{\ω}}=\stackrel{\‾}{q}\⊗\left[\begin{array}{cc}0& {\mathrm{\ω}}_{ie}^n\end{array}\right]$

1.9)陀螺仪y系下的矢量：

${\mathrm{\ω}}_{ie}^y=q\⊗{\tilde{q}}_{\mathrm{\ω}}$

1.10)初始时刻角增量的偏差：

$\mathrm{\ϵ}\mathrm{\ω}=\mathrm{\ω}-\left[\begin{array}{ccc}{\mathrm{\ω}}_{ie}^y\left(2,1\right)& {\mathrm{\ω}}_{ie}^y\left(3,1\right)& {\mathrm{\ω}}_{ie}^y\left(4,1\right)\end{array}\right]$

1.11)加速度计四元数：

${\tilde{q}}_a=\stackrel{\‾}{q}\⊗\left[\begin{array}{cc}0& g^n\end{array}\right]$

1.12)加速度计y系下的矢量：

$a_i^y=q\⊗{\tilde{q}}_a$

1.13)初始时刻速度计的偏差：

$\mathrm{\ϵ}a=a-\left[\begin{array}{ccc}{a}_i^y\left(2,1\right)& a_i^y\left(3,1\right)& a_i^y\left(4,1\right)\end{array}\right]$

1.14)转入步骤2)；

2)静态精对准

遍历静止时每一时刻的加速度、角速率、时间间隔和加速度漂移，根据子样进行角速度和加速度拟合,更新系统的角增量和速度增量；

2.1)时间间隔Δt＝1/f，f为采样频率；

2.2)采样样本数n_sample＝f·t，t初始对准时长；

2.3)样本间隔时间矩阵I为单位矩阵；

2.4)速度间隔时间矩阵

2.5)增量时间矩阵

2.6)角增量

2.7)速度增量

2.8)角增量求和

${\mathrm{\ω}}_{sum}=\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}\left(1,1\right)+\mathrm{\Δ}\mathrm{\ω}\left(1,2\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(2,1\right)+\mathrm{\Δ}\mathrm{\ω}\left(2,2\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(3,1\right)+\mathrm{\Δ}\mathrm{\ω}\left(3,2\right)\end{array}\right]$

2.9)速度增量求和

$v_{sum}=\left[\begin{array}{c}\mathrm{\Δ}v\left(1,1\right)+\mathrm{\Δ}v\left(1,2\right)\\ \mathrm{\Δ}v\left(2,1\right)+\mathrm{\Δ}v\left(2,2\right)\\ \mathrm{\Δ}v\left(3,1\right)+\mathrm{\Δ}v\left(3,2\right)\end{array}\right]$

2.10)可由步骤2.6)和步骤2.8)，求得圆锥补偿后的角增量：

$\▿\mathrm{\ω}={\mathrm{\ω}}_{sum}+\frac{2}{3}\·\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}\left(1,1\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(2,1\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(3,1\right)\end{array}\right]\⊗\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}\left(1,2\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(2,2\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(3,2\right)\end{array}\right]$

2.11)可由步骤2.6)～步骤2.9)，求得划船补偿后的速度增量：

$\▿v=v_{sum}+\frac{1}{2}\·v_{sum}\⊗{\mathrm{\ω}}_{sum}+\frac{2}{3}\·\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}\left(1,1\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(2,1\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(3,1\right)\end{array}\right]\⊗\left[\begin{array}{c}\mathrm{\Δ}v\left(1,2\right)\\ \mathrm{\Δ}v\left(2,2\right)\\ \mathrm{\Δ}v\left(3,2\right)\end{array}\right]+\left[\begin{array}{c}\mathrm{\Δ}v\left(1,1\right)\\ \mathrm{\Δ}v\left(2,1\right)\\ \mathrm{\Δ}v\left(3,1\right)\end{array}\right]\⊗\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}\left(1,2\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(2,2\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(3,2\right)\end{array}\right]$

2.12)速度四元数：

${\tilde{q}}_v=q\⊗\left[\begin{array}{cc}0& \▿v\end{array}\right]$

2.13)加速度计y系下的矢量：

$v_i^y={\tilde{q}}_a\⊗\stackrel{\‾}{q}$

2.14)更新后的速度：

v₀为初始速度；

2.15)更新后的四元数

如果为0，则q_▽ω＝[1 0 0 0]′；否则

2.16)更新后的位置：

r_更新＝r₀+(v₀+v_更新)/2·Δt

r₀为初始位置；

2.17)EKF的量测方程为：

Z_k＝HδX(k,k)+n_k

Z_k是量测值，H是量测矩阵，n_k是量测噪声，其方差矩阵为Q_k；

2.18)EKF的系统误差状态为：

δX(k,k)＝δX(k,k-1)+K_k[Z_k-HδX(k,k-1)]

2.19)EKF的系统状态方程为：

δX(k,k-1)＝Φ_kδX(k-1,k-1)+w_k-1

δX(k,k-1)是k时刻的预测误差状态，δX(k-1,k-1)是k-1时刻的估计误差状态，w_k-1是k-1时刻的系统噪声，其方差矩阵是R_k；

2.20)Φ_k是15阶的误差状态转移矩阵：

${\mathrm{\Φ}}_k=\left[\begin{array}{ccccc}{I}_{3\×3}& \mathrm{\Δ}t\×C_b^n\left(k,k-1\right)& 0& 0& 0\\ 0& I_{3\×3}& 0& 0& 0\\ 0& 0& I_{3\×3}& \mathrm{\Δ}t\×I_3& 0\\ -\mathrm{\Δ}t\×S\left(\stackrel{\‾}{a_k^n}\right)& 0& 0& I_{3\×3}& \mathrm{\Δ}t\×C_b^n\left(k,k-1\right)\\ 0& 0& 0& 0& I_{3\×3}\end{array}\right]$

2.21)是的反对称矩阵：

$\stackrel{\‾}{a_k^n}=C_b^n\left(k,k-1\right)\×\stackrel{\‾}{a_k^b}$

$S\left(\stackrel{\‾}{a_k^n}\right)=\left[\begin{array}{ccc}0& -\stackrel{\‾}{a_k^n}\left(3\right)& \stackrel{\‾}{a_k^n}\left(2\right)\\ \stackrel{\‾}{a_k^n}\left(3\right)& 0& -\stackrel{\‾}{a_k^n}\left(1\right)\\ -\stackrel{\‾}{a_k^n}\left(2\right)& \stackrel{\‾}{a_k^n}\left(1\right)& 0\end{array}\right]$

2.22)EKF的状态误差向量为：

其中，δX_k|k为k时刻的系统状态误差，和分别表示k时刻加速度三维误差值、角速度三维误差值、位置三维误差值、速度三维误差值和姿态三维误差值；

2.23)在静态初始对准时刻，位置和速度的增量都为零，修正的量测值为：

$Z_k={\left[\begin{array}{cccc}{\mathrm{\ω}}_{ibk}^b-C_{n\left(k|k-1\right)}^b{\mathrm{\ω}}_{ie}^n& r_k^n-r_0^n& v_k^n-0_{3\×1}& {\mathrm{\ψ}}_{k|k-1}-{\mathrm{\ψ}}_k^{mag}\end{array}\right]}^{\′}$

其中，ψ_k|k-1为k时刻未修正的航向角，为k时刻由磁力计计算得到的航向角，和分别为k时刻陀螺仪的角速度和地球自转角速度在n系的投影，和分别为k时刻的位置和初始的位置；

2.24)相对应的量测矩阵为：

H＝[0_10×2 Ι_10×10 0_10×3]

2.25)滤波增益方程：

K_k＝P_k|k-1H_k′(H_kP_k|k-1H_k′+R_k)^-1

2.26)一步预测均方误差方程：

P_k|k-1＝Φ_k-1P_k-1|k-1Φ_k-1 ^T+Q_k-1

2.27)估计均方误差方程：

P_k|k＝(I_15×15-K_kH)P_k|k-1(I_15×15-K_kH)′+K_kR_kK'_k

其中，下标k|k-1表示由k-1时刻到k时刻的预测值，下标k|k表示k时刻的估计值；

2.28)依次循环遍历步骤2.1)～2.27)，从而获取精准的初始姿态、状态估计误差和协方差矩阵；进入步骤3)；

3)步态检测

3.1)k时刻加速度的振幅：

$\left|a_k\right|=\sqrt{{a_{x\left(k\right)}^b}^2+{a_{y\left(k\right)}^b}^2+{a_{z\left(k\right)}^b}^2}$

3.2)k时刻加速度的均值：

$\stackrel{\‾}{a_k^b}=\frac{1}{2s+1}{\mathrm{\Σ}}_{q=k-s}^{k+s}\left|a_q\right|$

其中，代表k时刻平均加速度振幅，s＝15为平滑窗口大小；

3.3)加速度的振幅方差：

$\mathrm{\σ}{\left(a_k^b\right)}^2=\frac{1}{2s+1}\underset{j=k-s}{\overset{k+s}{\mathrm{\Σ}}}{\left(\left|a_j\right|-\stackrel{\‾}{a_k^b}\right)}^2$

3.4)角速度的振幅：

$\left|w_k\right|=\sqrt{{w_{x\left(k\right)}^b}^2+{w_{y\left(k\right)}^b}^2+{w_{z\left(k\right)}^b}^2}$

3.5)角速度的均值：

$\stackrel{\‾}{w_k^b}=\frac{1}{2s+1}{\mathrm{\Σ}}_{q=k-s}^{k+s}\left|w_q\right|$

3.6)角速度的振幅方差：

$\mathrm{\σ}{\left(w_k^b\right)}^2=\frac{1}{2s+1}\underset{j=k-s}{\overset{k+s}{\mathrm{\Σ}}}{\left(\left|w_j\right|-\stackrel{\‾}{w_k^b}\right)}^2$

3.7)步伐状态判定阈值：

3.8)循环遍历步骤3.1)～3.7)，将从平稳进入移动的时刻，记为k_i时刻；进入步骤4)；

4)ZUPT与磁力计

4.1)时间间隔Δt＝1/f，f为采样频率；

4.2)采样样本数n_sample＝f·t，t初始对准时长；

4.3)样本间隔时间矩阵I为单位矩阵；

4.4)速度间隔时间矩阵

4.5)增量时间矩阵

4.6)角增量

4.7)速度增量

4.8)角增量求和：

${\mathrm{\ω}}_{sum}=\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}\left(1,1\right)+\mathrm{\Δ}\mathrm{\ω}\left(1,2\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(2,1\right)+\mathrm{\Δ}\mathrm{\ω}\left(2,2\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(3,1\right)+\mathrm{\Δ}\mathrm{\ω}\left(3,2\right)\end{array}\right]$

4.9)速度增量求和：

$v_{sum}=\left[\begin{array}{c}\mathrm{\Δ}v\left(1,1\right)+\mathrm{\Δ}v\left(1,2\right)\\ \mathrm{\Δ}v\left(2,1\right)+\mathrm{\Δ}v\left(2,2\right)\\ \mathrm{\Δ}v\left(3,1\right)+\mathrm{\Δ}v\left(3,2\right)\end{array}\right]$

4.10)可由步骤4.6)和步骤4.8)，求得圆锥补偿后的角增量：

$\▿\mathrm{\ω}={\mathrm{\ω}}_{sum}+\frac{2}{3}\·\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}\left(1,1\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(2,1\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(3,1\right)\end{array}\right]\⊗\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}\left(1,2\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(2,2\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(3,2\right)\end{array}\right]$

4.11)可由步骤4.6)～步骤4.9)，求得划船补偿后的速度增量：

$\▿v=v_{sum}+\frac{1}{2}\·v_{sum}\⊗{\mathrm{\ω}}_{sum}+\frac{2}{3}\·\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}\left(1,1\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(2,1\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(3,1\right)\end{array}\right]\⊗\left[\begin{array}{c}\mathrm{\Δ}v\left(1,2\right)\\ \mathrm{\Δ}v\left(2,2\right)\\ \mathrm{\Δ}v\left(3,2\right)\end{array}\right]+\left[\begin{array}{c}\mathrm{\Δ}v\left(1,1\right)\\ \mathrm{\Δ}v\left(2,1\right)\\ \mathrm{\Δ}v\left(3,1\right)\end{array}\right]\⊗\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\ω}\left(1,2\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(2,2\right)\\ \mathrm{\Δ}\mathrm{\ω}\left(3,2\right)\end{array}\right]$

4.12)速度四元数：

${\tilde{q}}_v=q\⊗\left[\begin{array}{cc}0& \▿v\end{array}\right]$

4.13)加速度计y系下的矢量：

$v_i^y={\tilde{q}}_a\⊗\stackrel{\‾}{q}$

4.14)更新后的速度：

v₀为初始速度；

4.15)更新后的四元数：

如果为0，则q_▽ω＝[1 0 0 0]′；否则

4.16)更新后的位置：

r_更新＝r₀+(v₀+v_更新)/2·Δt

r₀为初始位置；

4.17)判定该时刻是否为平稳进入移动的时刻，当进入k_i时刻，则在该时刻采用零速度修正(ZUPT)与磁力计组合，构造量测矩阵为：

$H=\left[\begin{array}{ccccc}\left[\begin{array}{ccc}0& 0& 1\end{array}\right]& 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×3}\end{array}\right]$

4.18)组合后的量测噪声为：

$R=\left[\begin{array}{ccccc}1\×{10}_{1\×3}^{-4}& 0_{1\×3}& 0_{1\×3}& 1\×{10}_{1\×3}^{-4}& 0_{1\×3}\end{array}\right]$

4.19)组合后的量测值为：

$Z_{k_i}=\left[\begin{array}{cc}{\mathrm{\ΔV}}_{k_i}& {\mathrm{\Δ\ψ}}_{k_i}\end{array}\right]$

4.20)EKF的量测方程为：

Z_k＝HδX(k,k)+n_k

4.21)EKF的系统误差状态为：

δX(k,k)＝δX(k,k-1)+K_k[Z_k-HδX(k,k-1)]

4.22)EKF的系统状态方程为：

δX(k,k-1)＝Φ_kδX(k-1,k-1)+w_k-1

4.23)滤波增益方程：

K_k＝P_k|k-1H_k′(H_kP_k|k-1H_k′+R_k)^-1

4.24)一步预测均方误差方程：

P_k|k-1＝Φ_k-1P_k-1|k-1Φ_k-1 ^T+Q_k-1

4.25)估计均方误差方程：

P_k|k＝(I_15×15-K_kH)P_k|k-1(I_15×15-K_kH)′+K_kR_kK'_k

4.26)根据δX(k,k)值，更新速度、位置和姿态信息；

4.27)循环遍历步骤4.1)～步骤4.26)，可得行人行走过程中的速度、位置和姿态。