CN109579836A - 一种基于mems惯性导航的室内行人方位校准方法 - Google Patents

Abstract

本发明提出了一种基于MEMS惯性导航的室内行人方位校准方法，包括：对MEMS惯性传感器采集的数据进行地磁干扰评估，判断地磁场是否受到强烈干扰，如果是，则采用室内场景模式进行方位角校准，否则采用Kalman滤波和互补滤波相融合的算法进行方位角校准，得到最优方位角。本发明可以滤除地磁随机干扰的影响，也能有效消除陀螺仪方位解算的累积误差。

Description

一种基于MEMS惯性导航的室内行人方位校准方法

技术领域

本发明涉及MEMS惯性导航定位技术领域，特别涉及一种基于MEMS惯性导航的室内行人方位校准方法。

背景技术

对MEMS(Micro-Electro-Mechanical Systems，微机电系统)惯性导航定位系统来讲，方位角的精度是影响导航定位系统精度的主要因素之一，对影响方位角长时间精度保持技术的因素进行分析研究是提高方位角精度和长期定位的准确性的重要步骤。由MEMS惯性导航定位系统的方位角长时间精度保持技术的研究现状可知，影响方位角长时间精度保持的主要因素有三个：

第一，MEMS惯性元件精度较低。虽然当前MEMS惯性传感器已经有了长足的发展，但受工作原理和工艺技术水平的限制，MEMS传感器精度相对较低，其性能与传统惯性元件相比仍有一定差距，要实现长时间高精度的方位保持难度比较大。

第二，MEMS惯性导航所固有的传感器误差累积特性，图1为MEMS惯性传感器输出误差，惯性传感器误差的大小对于惯导系统的导航定位精度具有决定性的影响。

第三，捷联导航算法误差，在捷联式惯导系统中，利用陀螺仪测量载体各轴旋转运动的瞬时角度增量，通过积分计算其角度变化量，结合载体初始方位，解算出当前的方位角信息。由于采用积分的方式计算方位角信息，各种误差随着时间的增长而不断累积，使得系统的长期精度差。

发明内容

本发明的目的旨在至少解决所述技术缺陷之一。

为此，本发明的目的在于提出一种基于MEMS惯性导航的室内行人方位校准方法。

为了实现上述目的，本发明的实施例提供一种基于MEMS惯性导航的室内行人方位校准方法，包括如下步骤：

步骤S1，对MEMS惯性传感器采集的数据进行地磁干扰评估，判断地磁场是否受到强烈干扰，如果是，则采用室内场景模式进行方位角校准，否则采用Kalman滤波和互补滤波相融合的算法进行方位角校准；

步骤S2，当判断采用Kalman滤波和互补滤波相融合的算法进行方位角校准时，执行如下步骤：

(1)进行第一次Kalman滤波，将加速度计测量值与重力场在载体坐标系下投影的差值和磁强计测量值与当地磁场强度在载体坐标系下投影的差值共同作为Kalman滤波算法中的观测值；将陀螺仪的测量值通过四阶-龙格库塔法进行解算得到不断更新的四元数将其作为Kalman滤波的状态值；

(2)利用互补滤波算法，由电子罗盘对陀螺仪校正之后得到的校正角速度通过四阶龙格库塔法得到校正后的四元数；

(3)根据第一次Kalman滤波得到的优化四元数和互补滤波算法得到的校正四元数，进行第二次Kalman滤波，通过Kalman滤波算法的不断更新，得到最优方位角；

步骤S3，当判断采用室内场景模式进行方位角校准，执行如下步骤：利用将地理坐标系下的重力与加速度计在载体坐标系下测量得到的加速度做向量积的运算来校正陀螺偏差，校正后的角速度值，通过四阶龙格库塔法可直接得出最优方位角。

进一步，在所述步骤S1中，对采集数据中的地磁信号进行预处理，根据磁强计三个轴上磁场强度的输出矢量和与标准地磁作对比，对磁强计各个轴上的矢量和与标准地磁进行差值计算，如果计算得到的偏差超出预设阈值范围，则判断地磁场受到强烈干扰，采用室内场景模式进行方位角校准；如果计算得到的偏差未超出预设阈值范围，则判断地磁场没有受到强烈干扰，采用Kalman滤波和互补滤波相融合的算法进行方位角校准。

进一步，根据下述公式判断地磁场是否受到强烈干扰，

其中，m(i)为磁强计输出的磁场强度，m_x，m_y，mz表示三个轴的磁场强度，m₀表示本地标准地磁场强度，

|m(i)-m₀|＜0.1*m₀

当上式条件满足时，则判断外界磁场干扰强度大，采用Kalman滤波和互补滤波相融合的算法进行方位角校准；而当上述条件不成立时，判断外界磁场干扰强度不大，采用室内场景模式进行方位角校准。

进一步，在所述步骤S2中，对于步骤(1)中进行第一次Kalman滤波，

在第一次Kalman滤波算法中设状态估计量X₁(k)为

X₁(k)＝[q₀,q₁,q₂,q₃]^T

设第一次Kalman滤波器的观测量Z₁为

其中，a为加速度计测量值，a_s为重力场在载体坐标系下投影；m为磁强计测量值，m_s为当地磁场强度在载体坐标系下投影；为姿态矩阵参数，q₀,q₁,q₂,q₃为陀螺仪通过四阶龙格库塔法所得值；

将其按照所述的Kalman滤波算法可以得到第一次最优估计四元数q₀₀,q₁₁,q₂₂,q₃₃。

进一步，在所述步骤S2中，对于步骤(2)，将地理坐标系下的重力和地磁分量转换到载体坐标系下然后与加速度计和磁强计在载体坐标系下测量得到的加速度和磁场强度做向量积的运算，得到Δ_θ，γ和Δ_ψ，将二者相加经过比例积分低通滤波，将电子罗盘测量姿态中的高频抖动信号滤出，然后将比例积分的输出值W_{_offest}和陀螺仪测量的角速度进行融合，得到对陀螺仪补偿后的角速度，利用四阶龙格库塔法得到校正后的四元数，其中，Δ_θ，γ为俯仰角和横滚角的误差，Δ_ψ为航向角的误差。

进一步，在所述步骤S2中，对于步骤(3)，进行第二次Kalman滤波，设第二次Kalman滤波中的状态估计量为第一次Kalman滤波之后的优化四元数为：

X₂(k)＝[q₀₀,q₁₁,q₂₂,q₃₃]^T

设第二次Kalman滤波中的观测量为互补滤波算法得到的校正四元数：

Z₂＝[q(1),q(2),q(3),q(4)]^T。

进一步，在所述步骤S3中，

设W_{_offest}为经过K_p和K_i校正后的陀螺仪误差，则

W_{_offest}＝K_pΔ+K_i∫Δ

W_校正＝w+W_{_offest}；

K_p为比例系数，K_i为积分系数，w为陀螺仪测量得到的角速度值，W_校正为利用本算法校正后的角速度值，通过四阶龙格库塔法可直接得出最优方位角，a＝[a_x,a_y,a_z]^T表示载体坐标系下加速度计测量得到的加速度，将地理坐标系下的重力分量通过姿态转换矩阵变换为载体坐标系下的重力分量用a_s＝[a_sx,a_sy,a_sz]^T表示，对a、a_s做向量积的运算，设运算结果为Δ，将该结果通过比例积分调节得到加速度计对陀螺进行修正的角速度。

根据本发明实施例的基于MEMS惯性导航的室内行人方位校准方法，在传感器误差补偿之后的基础上进行方位角偏差校准算法的研究。由于在惯导系统中鉴于MEMS惯性传感器精度不高，在纯惯性定位条件下，即使采取了上述误差估计补偿措施，若不定期对方位角校正，其方位角误差都不可避免地将随时间而迅速增长，难以实现方位角长时间高精度输出的目的。基于此，本发明利用Kalman滤波和互补滤波相融合的算法进行方位角的校准，一方面可以滤除地磁随机干扰的影响，另一方面也能有效消除陀螺仪方位解算的累积误差。在地磁受到长时间严重干扰的场合，电子罗盘将无法准确确定方位信息，为此本发明采用运动场景约束模式的方位角校准方法进行方位角的解算。根据磁场干扰程度大小进行方案的选择，实验结果表明，方位角精度均可以保持在1°/h之内。本算法不仅可以有效解决方位角误差发散问题，还可根据人的运动场景有效的解决磁场干扰问题，实现高精度长时间的方位输出。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1为MEMS惯性传感器输出误差的示意图；

图2为根据本发明实施例的基于MEMS惯性导航的室内行人方位校准方法的流程图；

图3为根据本发明实施例的Kalman滤波和互补滤波融合的方位角校准算法的示意图；

图4为根据本发明实施例的单个陀螺解算(70min)的示意图；

图5为Kalman滤波和互补滤波融合解算(70min)的方位角示意图；

图6为根据本发明实施例的互补滤波算法图(70min)的示意图；

图7为根据本发明实施例的Kalman滤波算法(70min)的示意图；

图8为Kalman滤波和互补滤波融合解算(70min)的方位偏差示意图。

图9为根据本发明实施例的结合电梯运动约束模式的方位角校准算法的示意图；

图10为根据本发明实施例的陀螺仪解算结果的示意图；

图11为根据本发明实施例的结合电梯运动模式方位解算结果的示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

为提升方位角精度和长期定位的准确性，一方面是采用高精度高性能的MEMS陀螺仪和MEMS加速度计。另一方面，更重要的方法是对系统中出现的各种误差(包括元件误差和漂移、观测误差、算法误差等)研究更优的估计和补偿方法。基于此，本发明提出一种基于MEMS惯性导航的室内行人方位校准方法。该校准方法采用改进的Kalman与互补滤波相融合的方法，Kalman滤波中利用电子罗盘在导航系和载体系的测量偏差作为观测有效的抑制了陀螺的漂移，将其与互补滤波算法校正之后的四元数融合得出校正之后的方位角。但室内磁强计容易受到干扰，针对此问题提出了基于运动场景的行人方位校准技术。

如图2所示，本发明实施例的基于MEMS惯性导航的室内行人方位校准方法，包括如下步骤：

步骤S1，对MEMS惯性传感器采集的数据进行地磁干扰评估，判断地磁场是否受到强烈干扰，如果是，则采用室内场景模式进行方位角校准，否则采用Kalman滤波和互补滤波相融合的算法进行方位角校准。

具体的，当地磁场受到明显的干扰时，融入的磁强计数据反而会降低方位角精度。所以在进行方位角解算的过程中需要对采集数据中的环境进行磁干扰的评估，判断地磁场是否受到了强烈干扰。对采集数据中的地磁信号进行预处理，根据磁强计三个轴上磁场强度的输出矢量和与标准地磁作对比，对磁强计各个轴上的矢量和与标准地磁进行差值计算，如果计算得到的偏差超出预设阈值范围，则判断地磁场受到强烈干扰，采用室内场景模式进行方位角校准；如果计算得到的偏差未超出预设阈值范围，则判断地磁场没有受到强烈干扰，采用Kalman滤波和互补滤波相融合的算法进行方位角校准。

根据下述公式判断地磁场是否受到强烈干扰，

其中，m(i)为磁强计输出的磁场强度，m_x，m_y，m_z表示三个轴的磁场强度，m₀表示本地标准地磁场强度，

|m(i)-m₀|＜0.1*m₀ (2)

当上式条件满足时，则判断外界磁场干扰强度大，采用Kalman滤波和互补滤波相融合的算法进行方位角校准；而当上述条件不成立时，判断外界磁场干扰强度不大，采用室内场景模式进行方位角校准。

步骤S2，当判断采用Kalman滤波和互补滤波相融合的算法进行方位角校准时，执行如下步骤：

(1)进行第一次Kalman滤波，将加速度计测量值与重力场在载体坐标系下投影的差值和磁强计测量值与当地磁场强度在载体坐标系下投影的差值共同作为Kalman滤波算法中的观测值；将陀螺仪的测量值通过四阶-龙格库塔法进行解算得到不断更新的四元数将其作为Kalman滤波的状态值。将其按照的Kalman滤波算法可以得到第一次最优估计四元数q₀₀,q₁₁,q₂₂,q₃₃。

对于步骤(1)中进行第一次Kalman滤波，

在第一次Kalman滤波算法中设状态估计量X₁(k)为

X₁(k)＝[q₀,q₁,q₂,q₃]^T (3)

设第一次Kalman滤波器的观测量Z₁为

其中，a为加速度计测量值，a_s为重力场在载体坐标系下投影；m为磁强计测量值，m_s为当地磁场强度在载体坐标系下投影；为姿态矩阵参数，q₀,q₁,q₂,q₃为陀螺仪通过四阶龙格库塔法所得值。

(2)利用互补滤波算法，由电子罗盘对陀螺仪校正之后得到的校正角速度通过四阶龙格库塔法得到校正后的四元数。

对于步骤(2)，将地理坐标系下的重力和地磁分量转换到载体坐标系下然后与加速度计和磁强计在载体坐标系下测量得到的加速度和磁场强度做向量积的运算，得到图3中的Δ_θ，γ和Δ_ψ，将二者相加经过比例积分低通滤波，将电子罗盘测量姿态中的高频抖动信号滤出，然后将比例积分的输出值即图3中的W_{_offest}和陀螺仪测量的角速度进行融合，得到对陀螺仪补偿后的角速度，利用四阶龙格库塔法得到校正后的四元数，其中，Δ_θ，γ为俯仰角和横滚角的误差，Δ_ψ为航向角的误差。

(3)根据第一次Kalman滤波得到的优化四元数和互补滤波算法得到的校正四元数，进行第二次Kalman滤波，通过Kalman滤波算法的不断更新，得到最优方位角。

设第一次Kalman滤波得到的优化四元数用q₀₀,q₁₁,q₂₂,q₃₃表示，将互补滤波算法得到的校正四元数用q(1),q(2),q(3),q(4)表示作为第二次Kalman滤波算法中的观测量，下面来设计第二次Kalman滤波算法。

设第二次Kalman滤波中的状态估计量为第一次Kalman滤波之后的优化四元数

X₂(k)＝[q₀₀,q₁₁,q₂₂,q₃₃]^T (5)

设第二次Kalman滤波中的观测量为互补滤波算法得到的校正四元数：

Z₂＝[q(1),q(2),q(3),q(4)]^T (6)

将式(5)和式(6)按照上述Kalman滤波算法流程进行解算，将上述两种解算得到的四元数进行第二次卡尔曼滤波，通过Kalman滤波算法的不断更新，得到更加精确的最优方位角。

为了更好的验证上述方案的可行性进行了以下实验：

首先利用椭球拟合法对MPU9150传感器模块中的磁强计进行校准，然后将校准之后的MPU9150传感器模块绑在脚上进行数据采集，采集频率为100HZ，采集模式为行人在室内某一起点位置出发，任意行走一圈之后回到起始位置，一直重复此过程进行数据采集，采集时间为70min。图4为根据本发明实施例的单个陀螺解算(70min)的示意图。图5为根据本发明实施例的Kalman滤波和互补滤波融合解算(70min)的示意图。

从图4中可以明显的看出单个陀螺仪进行方位解算的时候会不断的随时间累积误差，不能保证方位角长时间高精度输出。虽然单独的Kalman滤波、单独的互补滤波和Kalman滤波与互补滤波相融合的校准算法可以很好的校正方位角偏差，但是利用各个算法解算的方位角精度从上述图中不易观察，本次采集的70min数据同样将每一次回到原位置的方位和最初始的方位做差，通过每一圈的方位偏差图来对这几种算法进行对比。图6至图8分别表示数据采集70min得到的互补滤波算法、Kalman滤波算法、Kalman和互补滤波相融合算法的始末位置偏差图。

通过上述算法的比较可知，使用Kalman滤波和互补滤波相融合的算法进行方位角校准优于其它两种算法，该算法可以将方位角精度保持在0.5°/h以内，从而实现长时间、高精度导航定位的目的，而单独使用互补滤波算法得出的方位角精度在2°/h以内，单独使用Kalman滤波算法得出的方位角精度在1°/h以内。

步骤S3，当判断采用室内场景模式进行方位角校准，执行如下步骤：利用将地理坐标系下的重力与加速度计在载体坐标系下测量得到的加速度做向量积的运算来校正陀螺偏差，校正后的角速度值，通过四阶龙格库塔法可直接得出最优方位角。

在地磁受到长时间严重干扰的场合，电子罗盘将无法准确确定方位信息，为此需要结合行人的运动场景约束的方法来进行方位角校准。下面以行人在电梯中的场景为例进行说明。

行人携带传感器模块在电梯中时，传感器模块中已经校准好了的磁强计会瞬间被磁化，此时再使用上述所述算法即利用磁强计去校正陀螺仪的话会引入更大的方位偏差，而考虑到电梯在运动的过程中是基于重力方向进行运动，因此在电梯运动中进行方位角校准的时候采用加速度计对陀螺进行校正来解算精确的方位，即利用将地理坐标系下的重力与加速度计在载体坐标系下测量得到的加速度做向量积的运算来校正陀螺偏差，如图9所示。

设W_{_offest}为经过K_p和K_i校正后的陀螺仪误差，则

W_{_offest}＝K_pΔ+K_i∫Δ (5)

W_校正＝w+W_{_offest} (6)

K_p为比例系数，K_i为积分系数，w为陀螺仪测量得到的角速度值，W_校正为利用本算法校正后的角速度值，通过四阶龙格库塔法可直接得出最优方位角，a＝[a_x,a_y,a_z]^T表示载体坐标系下加速度计测量得到的加速度，将地理坐标系下的重力分量通过姿态转换矩阵变换为载体坐标系下的重力分量用a_s＝[a_sx,a_sy,a_sz]^T表示，对a、a_s做向量积的运算，设运算结果为Δ，将该结果通过比例积分调节得到加速度计对陀螺进行修正的角速度。

为了验证上述方案的可行性，行人携带传感器(将传感器绑在脚上)在电梯内进行了实验，行人将传感器绑在脚上，进入电梯(电梯在向下运动)内行人保持静止状态，图10和图11分别表示在下电梯过程中行人保持静止状态利用陀螺仪解算的方位角和结合电梯运动模式的方位角校准的解算结果。表1表示行人在静止状态下两种算法的方位解算偏差。

表1

由上述表1中可以看出本发明提出的结合电梯运动约束模式的方位角校准算法可以很好的校准方位偏差。

根据本发明实施例的基于MEMS惯性导航的室内行人方位校准方法，在传感器误差补偿之后的基础上进行方位角偏差校准算法的研究。由于在惯导系统中鉴于MEMS惯性传感器精度不高，在纯惯性定位条件下，即使采取了上述误差估计补偿措施，若不定期对方位角校正，其方位角误差都不可避免地将随时间而迅速增长，难以实现方位角长时间高精度输出的目的。基于此，本发明利用Kalman滤波和互补滤波相融合的算法进行方位角的校准，一方面可以滤除地磁随机干扰的影响，另一方面也能有效消除陀螺仪方位解算的累积误差。在地磁受到长时间严重干扰的场合，电子罗盘将无法准确确定方位信息，为此本发明采用运动场景约束模式的方位角校准方法进行方位角的解算。根据磁场干扰程度大小进行方案的选择，实验结果表明，方位角精度均可以保持在1°/h之内。本算法不仅可以有效解决方位角误差发散问题，还可根据人的运动场景有效的解决磁场干扰问题，实现高精度长时间的方位输出。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。本发明的范围由所附权利要求极其等同限定。

Claims (7)

1.一种基于MEMS惯性导航的室内行人方位校准方法，其特征在于，包括如下步骤：

步骤S1，对MEMS惯性传感器采集的数据进行地磁干扰评估，判断地磁场是否受到强烈干扰，如果是，则采用室内场景模式进行方位角校准，否则采用Kalman滤波和互补滤波相融合的算法进行方位角校准；

步骤S2，当判断采用Kalman滤波和互补滤波相融合的算法进行方位角校准时，执行如下步骤：

(1)进行第一次Kalman滤波，将加速度计测量值与重力场在载体坐标系下投影的差值和磁强计测量值与当地磁场强度在载体坐标系下投影的差值共同作为Kalman滤波算法中的观测值；将陀螺仪的测量值通过四阶-龙格库塔法进行解算得到不断更新的四元数将其作为Kalman滤波的状态值；

(2)利用互补滤波算法，由电子罗盘对陀螺仪校正之后得到的校正角速度通过四阶龙格库塔法得到校正后的四元数；

(3)根据第一次Kalman滤波得到的优化四元数和互补滤波算法得到的校正四元数，进行第二次Kalman滤波，通过Kalman滤波算法的不断更新，得到最优方位角；

步骤S3，当判断采用室内场景模式进行方位角校准，执行如下步骤：利用将地理坐标系下的重力与加速度计在载体坐标系下测量得到的加速度做向量积的运算来校正陀螺偏差，校正后的角速度值，通过四阶龙格库塔法可直接得出最优方位角。

2.如权利要求1所述的基于MEMS惯性导航的室内行人方位校准方法，其特征在于，在所述步骤S1中，对采集数据中的地磁信号进行预处理，根据磁强计三个轴上磁场强度的输出矢量和与标准地磁作对比，对磁强计各个轴上的矢量和与标准地磁进行差值计算，如果计算得到的偏差超出预设阈值范围，则判断地磁场受到强烈干扰，采用室内场景模式进行方位角校准；如果计算得到的偏差未超出预设阈值范围，则判断地磁场没有受到强烈干扰，采用Kalman滤波和互补滤波相融合的算法进行方位角校准。

3.如权利要求2所述的基于MEMS惯性导航的室内行人方位校准方法，其特征在于，根据下述公式判断地磁场是否受到强烈干扰，

其中，m(i)为磁强计输出的磁场强度，m_x，m_y，m_z表示三个轴的磁场强度，m₀表示本地标准地磁场强度，

|m(i)-m₀|＜0.1*m₀

当上式条件满足时，则判断外界磁场干扰强度大，采用Kalman滤波和互补滤波相融合的算法进行方位角校准；而当上述条件不成立时，判断外界磁场干扰强度不大，采用室内场景模式进行方位角校准。

4.如权利要求1所述的基于MEMS惯性导航的室内行人方位校准方法，其特征在于，在所述步骤S2中，对于步骤(1)中进行第一次Kalman滤波，

在第一次Kalman滤波算法中设状态估计量X₁(k)为

X₁(k)＝[q₀,q₁,q₂,q₃]^T

设第一次Kalman滤波器的观测量Z₁为

其中，a为加速度计测量值，a_s为重力场在载体坐标系下投影；m为磁强计测量值，m_s为当地磁场强度在载体坐标系下投影；为姿态矩阵参数，q₀,q₁,q₂,q₃为陀螺仪通过四阶龙格库塔法所得值；

将其按照所述的Kalman滤波算法可以得到第一次最优估计四元数q₀₀,q₁₁,q₂₂,q₃₃。

5.如权利要求1所述的基于MEMS惯性导航的室内行人方位校准方法，其特征在于，在所述步骤S2中，对于步骤(2)，将地理坐标系下的重力和地磁分量转换到载体坐标系下然后与加速度计和磁强计在载体坐标系下测量得到的加速度和磁场强度做向量积的运算，得到Δ_θ，γ和Δ_ψ，将二者相加经过比例积分低通滤波，将电子罗盘测量姿态中的高频抖动信号滤出，然后将比例积分的输出值W_{_offest}和陀螺仪测量的角速度进行融合，得到对陀螺仪补偿后的角速度，利用四阶龙格库塔法得到校正后的四元数，其中，Δ_θ，γ为俯仰角和横滚角的误差，Δ_ψ为航向角的误差。

6.如权利要求1所述的基于MEMS惯性导航的室内行人方位校准方法，其特征在于，在所述步骤S2中，对于步骤(3)，进行第二次Kalman滤波，设第二次Kalman滤波中的状态估计量为第一次Kalman滤波之后的优化四元数为：

X₂(k)＝[q₀₀,q₁₁,q₂₂,q₃₃]^T

设第二次Kalman滤波中的观测量为互补滤波算法得到的校正四元数：

Z₂＝[q(1),q(2),q(3),q(4)]^T。

7.如权利要求1所述的基于MEMS惯性导航的室内行人方位校准方法，其特征在于，在所述步骤S3中，

设W_{_offest}为经过K_p和K_i校正后的陀螺仪误差，则

W_{_offest}＝K_pΔ+K_i∫Δ

W_校正＝w+W_{_offest}；

K_p为比例系数，K_i为积分系数，w为陀螺仪测量得到的角速度值，W_校正为利用本算法校正后的角速度值，通过四阶龙格库塔法可直接得出最优方位角，a＝[a_x,a_y,a_z]^T表示载体坐标系下加速度计测量得到的加速度，将地理坐标系下的重力分量通过姿态转换矩阵变换为载体坐标系下的重力分量用a_s＝[a_sx,a_sy,a_sz]^T表示，对a、a_s做向量积的运算，设运算结果为Δ，将该结果通过比例积分调节得到加速度计对陀螺进行修正的角速度。