一种用于行人方位估计的自适应增益互补滤波方法及系统与流程

本公开涉及滤波数据处理技术领域，特别是涉及一种用于行人方位估计的自适应增益互补滤波方法及系统。

背景技术：

最近，人体运动捕捉系统(运动追踪)的发展受到临床医学的高度刺激，其中步态分析是运动捕捉的主要应用。通过这些系统获得的高分辨率、定量数据可用于更好地了解多种疾病的原因，从而形成有效的治疗方法，比如对于失去运动功能的门诊患者或中风患者的康复。持续监测患者在自由生活环境中的人体运动可提供更有价值的反馈来指导临床治疗。然而，超越实验室环境并获得人体活动的准确测量，尤其是在自由生活环境下，这是极具挑战性的。目前，商业人体运动捕捉系统拥有出色捕捉和重建系统，但需要外部设备(如相机)，并且不能为用户提供日常环境下的重要信息。

通常，人体可以被建模为由关节连接的多节段的连杆系统，其被称为运动链。如果可以确定相对于每个节段的方位，则可以计算出基于该运动链的人体的整体运动。因此，关节角的准确估计在人体运动分析中具有重要的作用。可以通过附着于人体的惯性传感器模块来测量各个节段的方向。这种传感器模块通常由三轴加速度计，三轴陀螺仪和三轴磁力计组成，尺寸紧凑，可称为可穿戴惯性传感器。

然而，使用惯性传感器估计方向存在两个主要的挑战。首先，直接对三轴陀螺仪的角速度进行积分，是一个挑战；因为惯性传感器固有的物理限制会对方向的估计精度产生负面影响，惯性传感器在输出数据的同时会产生噪声，并在长期跟踪中迅速引起漂移误差累积。长期跟踪过程中积累的漂移误差阻碍了其在人体运动捕捉评估中的有效应用。其次，作为用于减轻方向漂移的辅助传感器：三轴加速度计和磁力计，它们分别被用作垂直(地球重力)和水平(地球磁场)参考。然而，加速度计在动态运动条件下对身体的多余加速度敏感(除重力之外)，而磁力计测量很容易受到局部地球磁场变化的影响，例如由电器或由黑色金属材料制成的物体引起的地球磁场方向的变化，从而影响所需的方向估计。此外，加速度计和磁力计的输出也包含测量误差。

技术实现要素：

为了解决现有技术的不足，本公开实施例子提供了一种用于行人方位估计的自适应增益互补滤波方法，将来自于高斯-牛顿优化算法的收敛率和来自于陀螺仪的发散率进行有效的互补，它不仅具有一次迭代计算的优点，减少计算负荷，而且还可获得陀螺仪测量误差的精确方向。

本说明书实施例子公开了一种用于行人方位估计的自适应增益互补滤波方法，通过以下技术方案实现：

包括：

数据采集：利用集成三轴加速度计、三轴陀螺仪、三轴磁力计的惯性传感器获得行人载体的三轴的加速度、角速度和磁场强度；

基于陀螺仪的方向估计：将三轴的陀螺仪测量的数据使用四元数表示，利用四元数的导数描述传感器坐标系相对于地球坐标系下的方向变化率，在已知初始条件下，通过对四元数的导数的积分获得传感器坐标系相对于对地球坐标系下的方向；

利用高斯-牛顿优化方法计算加速度计和磁力计测量值的方向；

自适应滤波器的时变参数被设为每个方向的权值，增益是根据基于高斯-牛顿优化算法的低通频率的收敛率和基于陀螺仪的高通频率的发散率进行自适应调节，获得陀螺仪最后的方向估计。

本说明书实施例子公开了一种用于行人方位估计的自适应增益互补滤波系统，通过以下技术方案实现：

包括：

数据采集单元，被配置为：利用集成三轴加速度计、三轴陀螺仪、三轴磁力计的惯性传感器获得行人载体的三轴的加速度、角速度和磁场强度；

基于陀螺仪的方向估计单元，被配置为：将三轴的陀螺仪测量的数据使用四元数表示，利用四元数的导数描述传感器坐标系相对于地球坐标系下的方向变化率，在已知初始条件下，通过对四元数的导数的积分获得传感器坐标系相对于对地球坐标系下的方向；

加速度计和磁力计测量值的方向计算单元，被配置为：利用高斯-牛顿优化方法计算加速度计和磁力计测量值的方向；

陀螺仪最后的方向估计单元，被配置为：自适应滤波器的时变参数被设为每个方向的权值，增益是根据基于高斯-牛顿优化算法的低通频率的收敛率和基于陀螺仪的高通频率的发散率进行自适应调节，获得陀螺仪最后的方向估计。

本说明书实施例子公开了一种用于行人方位估计的自适应增益互补滤波器，所述滤波器被配置为执行以下内容：

基于陀螺仪的方向估计：将三轴的陀螺仪测量的数据使用四元数表示，利用四元数的导数描述传感器坐标系相对于地球坐标系下的方向变化率，在已知初始条件下，通过对四元数的导数的积分获得传感器坐标系相对于地球坐标系的方向；

利用高斯-牛顿优化方法计算加速度计和磁力计测量值的方向；

自适应滤波器的时变参数被设为每个方向的权值，增益是根据基于高斯-牛顿优化算法的低通频率的收敛率和基于陀螺仪的高通频率的发散率进行自适应调节，获得陀螺仪最后的方向估计。

与现有技术相比，本公开的有益效果是：

本公开的技术方案将来自于高斯-牛顿优化算法的收敛率和来自于陀螺仪的发散率进行有效的互补，它不仅具有一次迭代计算的优点，而且计算负荷小，从而可以实时地获得陀螺仪测量误差的精确方向。

附图说明

构成本公开的一部分的说明书附图用来提供对本公开的进一步理解，本公开的示意性实施例及其说明用于解释本公开，并不构成对本公开的不当限定。

图1为本公开实施好例子的自适应增益互补滤波器算法的主要架构图；

图2为本公开实施好例子的传感器坐标系和地球坐标系之间的转换关系。本专利中传感器坐标系记为s，地球坐标系记为e；

图3为本公开实施好例子的自适应增益互补滤波器算法的具体公式化框图，它是图一的具体公式化形式；

图4为本公开实施好例子的含有磁铁干扰情况下，三个轴的磁干扰情况。在摆动磁铁的时刻，三轴的磁干扰情况明显；

图5为本公开实施好例子所提出的适应增益互补滤波器算法和medgwick方法在磁干扰情况下的方向估计的比较；

图6为本公开实施好例子的突然快速运动下的加速度数据变化值；

图7为本公开实施好例子的突然快速运动下的方向估计；

图8为本公开实施好例子的陀螺仪x，y和z轴偏差的估计结果。

具体实施方式

应该指出，以下详细说明都是例示性的，旨在对本公开提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本公开所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本公开的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

术语解释部分:高斯-牛顿优化算法(gauss-newtonoptimizationalgorithm,gna)。

实施例子一

该实施例子公开了一种用于行人方位估计的自适应增益互补滤波方法，该方法能够1)准确估计陀螺仪测量误差以校正陀螺仪的瞬时测量值；2)处理磁场失真；3)减少计算负荷和降低传感器的误差累积。

在该实施例子中，自适应增益互补滤波器算法，它是以高斯-牛顿优化算法(gna)的收敛率和陀螺仪发散率为基础，关键技术主要包括：(1)数据采集；(2)四元数表示；(3)陀螺积分；(4)矢量观测；(5)互补滤波器；(6)补偿方案；(7)滤波增益。

图1中，自适应增益互补滤波方法主要由四部分组成：1)陀螺积分2)矢量观察3)互补滤波器4)补偿方案；对传感器进行校正，由于本传感器中集成了三轴的加速度计，三轴的磁力计和三轴的陀螺仪，在传感器出厂时，已经校正过，因此不需要重新校正。首先是陀螺积分，对从陀螺仪中输出的角速度进行一次积分，输出结果作为3)互补滤波器的输入；其次是矢量观察，先将从加速度计中输出的加速度和从磁力计中输出的磁力强度作为4)补偿方案的输入，进而进行测量矢量的选择，然后再将输出结果作为高斯-牛顿方法的输入，高斯-牛顿方法的输出作为3)互补滤波器的另外一个输入，最终从互补滤波器中输出当前时刻的最优方位估计。

在该实施例子中，关于数据采集部分，使用lpms-b2惯性传感器进行数据采集，它集成三轴加速度计、三轴陀螺仪、三轴磁力计，欧拉角的取值范围：滚转角：(±180°)，俯仰角：(±90°)，偏航角(±180°)，通过蓝牙与上位机软件lpmscontro连接，可实时地获得三轴的加速度、角速度和磁场强度，实时地计算载体的姿态方向，线性加速度等信息。所测的数据的最终目的是对数据进行分析处理，估计行人的方位。本实验的数据采集频率为50hz。通过lpmscontro软件获得实时的加速度、角速度和磁力数据。

在一实施例子中，惯性传感器可根据需要放置于人的手臂上。

在该实施例子中，关于四元数表示，本实施例子中用到了两个坐标系：载体坐标系(记为s)和地球坐标系(记为e)，在实际解算的过程中，需要将载体坐标系的坐标转换到地球坐标系，因此需要知道两个坐标系的转换关系。当载体的坐标系方向相对于绝对坐标系指定时，可以确定载体在空间中的方向，绝对坐标系通常称为地球坐标系e(xe轴指向局部磁北和ze轴指向重力的相反方向)，如图2所示。为了方便以下叙述，这里将各个变量的名称列出：

s：固联于传感器的下传感器坐标系；

e：地球坐标系；

g：重力矢量；

^sa：传感器坐标系下加速度的测量值；

^sω：传感器坐标系下角速度的测量值；

^sm：传感器坐标系下磁力的测量值；

t时刻，传感器坐标系s到地球坐标系e下基于四元数的方向；

t时刻，陀螺仪数据得出的方向估计；

t时刻从加速度计和磁力计得出的方向估计；

t时刻来经过互补滤波融合之后得出的方向估计。

为了方便表示这两个坐标系之间的旋转，常用的三种旋转表示分别为：欧拉角，旋转矩阵和四元数。欧拉角法虽参数较少，但存在极点，而旋转矩阵法具有更多的参数且计算效率较低。因此本实施例子中使用四元数法来表示这两个坐标系之间的旋转。

具体的，四元数可以被认为是一个具有四个分量的向量，可以由标量s和普通向量u组成，可以通过四元数来计算关于单位矢量v＝[vx,vy,vz]相对于角度θ的旋转。如果这个单位矢量v是在s坐标系下定义，对应的在e坐标系下的位置可以通过在s坐标系下围绕这个单位矢量v旋转θ得到，所以s坐标系和e坐标系之间的相对方向可以用单位四元数表示。

如上所述，坐标系e可以通过在传感器坐标系s下绕单位向量^sv旋转θ得到(上标s表明这个矢量是定义在坐标系s中)，坐标系s相对于坐标系e的方向可以用单位四元数表示。所以这个单位矢量是定义在e坐标系下，对应关系如公式(3)

是的共轭，实际上等于因此，

在该实施例子中，基于陀螺仪的方向估计，此处基于陀螺仪方向的估计是根据公式(5)，(6)，(7)得出来的。初始系统中，我们默认t＝0时刻，地球坐标系和传感器坐标系是重合的，地球坐标系是固定的，而传感器坐标系是变化的，而在实际计算的时候，需要将传感器坐标系转换到地球坐标系，进行解算。

在传感器坐标系s下，三轴的陀螺仪测量的数据用^sω表示，^sω可被重新定义为一个含有4个元素的行向量：^sω＝[0ωxωyωz]，使用四元数表示。四元数的导数用来描述传感器坐标系相对于地球坐标系下的方向变化率，可以使用公式(5)计算：

其中，ω(^sω)是四元数^sω＝[0ωxωyωz]的斜对称矩阵，

因此，在已知初始条件的前提下，在时刻t+δt传感器坐标系相对于地球坐标系下的方向可以通过对积分获得，如公式(7)所列。

其中，δt是采样时间，为时刻t从陀螺仪数据得出的方向估计。这里只是将三轴的陀螺仪测量的数据使用四元数表示，利用四元数的导数描述传感器坐标系相对于地球坐标系下的方向变化率，在已知初始条件下，通过对四元数的导数的积分获得传感器坐标系相对于地球坐标系下的方向，具体求解如公式(7)。

在该实施例子中，将高斯-牛顿方法应用到矢量观测(加速度计和磁力计测得的三轴加速度数据和磁力数据)，由于加速度计和磁力计可以测量相对于地球的绝对方向，因此可以使用一种称为快速收敛方法来计算其测量值的方向，这种方法被称为高斯-牛顿优化方法。在地球坐标系下它们的归一化参考方向(加速度计^eza＝[0001]，磁力计并且初始时刻^smt＝0/||^smt＝0||，可根据当前磁力计的测量值和上一时刻的最优值自适应地改变。如图3中的组1.2，当磁力干扰发生时，它的归一化测量值可以由(8)得出：

通过以下步骤将高斯-牛顿方法建模计算出单位四元数

寻找：

最小化：

其中：

满足条件:

其中f是价值函数，将其定义为误差函数ε＝[εaεm]^t的平方，ε由加速度误差εa和磁力计误差εm组成。通过之前定义的^eza，^ezm,t，^sat，^smt和四元数乘法得出：

考虑到自然存在的动态运动和磁场干扰现象，以及传感器固有的误差，加速度计和陀螺仪的误差因数可以用ρa和ρm表示。误差的目标函数可以重写为

2)快高斯-牛顿方法

本专利提出一种新的快速高斯-牛顿方法，只需要一次迭代运算。根据上述公式化问题，传统的高斯-牛顿方法包括以下优化步骤：

其中k是迭代次数，j(k)是∈的雅克比行列式，可通过公式(10)计算得出。k表示的6×4常数矩阵，ρa由矩阵的前三行组成，ρm是由其余的行组成。运算符.*代表的是矩阵的点乘。公式(9)描述的是在已知初始值的前提下使用高斯-牛顿方法针对多次迭代运算得到的的一次方向估计结果。公式(9)可以进一步被优化为公式(11)。

实际上是在时间t+δt最后的优化方向，表示为(下标a表示的是依靠加速度计和磁力计估计的方向)，是指在之前时刻t估计的方向，被记为(下标f表示的是在互补滤波器融合之后的方向估计)，λk将在每次迭代中更改为最优值。由(12)中所示的λt+δt控制的收敛速率等于或大于方向的变化率，因此，每个时间段计算一次迭代是可接受的。λt+δt的最优值保证了的收敛速度，它受到通过陀螺仪测量数据计算的方向变化率的限制，从而避免了由于不必要的大步长而造成的过度调节。因此公式(11)可以简单写成仅含有一次迭代计算的公式(12)，λt+δt的优化值可以通过公式(13)计算得出。

其中，α是考虑到加速度计和磁力计存在噪声时λ的增加值。

在该实施例子中，关于自适应增益互补滤波器，互补滤波器主要针对两个具有互补光谱特征的噪声源设计，本实施例子中将低通滤波器处理的加速度及磁力信号和高通滤波器处理的陀螺仪信号组合起来得到最终的最优速率。通常情况下，用于测量噪声的传统互补滤波器的增益是一个常数，本实施例子提出的自适应滤波器结合了互补滤波器和卡尔曼滤波器的优点，时变参数和1-kt被设为每个方向的权值，并且能够得到鲁棒性的结果。最优化时变参数和增益可以提高自适应滤波器在估计方向时候的收敛速度和稳定性，由公式(14)表示，增益kt是根据基于高斯-牛顿优化算法的低通频率的收敛率和基于陀螺仪的高通频率的发散率β进行自适应调节。如图3中的组3所示。

因此，可获得最后的方向估计：

其中，为估计的方向速率，可以用来自于陀螺仪的方向变化率陀螺仪测量误差的幅值(通常为零均值误差)β和来自于加速度计和磁力计的方向误差变化率表示。

在一实施例子中，关于补偿方案：

1)陀螺仪的零偏漂移补偿：由于长时间的跟踪，惯性传感器会产生累积漂移误差，该实施例子用表示陀螺仪零偏漂移补偿。角速率误差^sωe，角速率偏差^sωb，角速度^sωc经过每次陀螺仪补偿之后，得到归一化后的方向如图3组2所示。

其中ζ表示为消除非零均值之后陀螺仪测量误差的收敛速度，通过数次实验可以得出它近似于β。

2)加速度计和磁力计补偿：为了在传感器测量值中选择最可靠的矢量作为滤波器的输入，需要对加速度计和磁强计的测量值进行补偿。例如，在快速运动的情况下^sat测量值和临时磁干扰的情况下^smt测量值都不能作为可靠的数据信息。因此，在这些情况下，^sω为更可信的测量信息。

通过以上的这些概念，输入矢量^sa(重力相关)和^sm(磁力相关)在高斯-牛顿优化算法中可以通过以下步骤进行补偿(见图3组1.1)：(a)参考矢量^ezm在时刻t可以在地球磁场中自适应变化。(b)参考矢量选为^eza或^eza,t；(c)自适应调整滤波器增益，不仅可以对快速运动和磁干扰的情况进行补偿，也可以对陀螺仪积分造成的累积方向误差进行补偿。

在一实施例子中，关于滤波增益：

滤波器增益β解释了所有零-均值陀螺仪测量误差，前面步骤中的陀螺仪的零偏漂移补偿中提到了角速率误差^sωe，角速率偏差^sωb。这些误差通常来自传感器噪声、信号混叠、量化误差、校准误差、传感器错位和频率响应特性。滤波器增益ζ表示收敛速度，尤其是去除非零均值陀螺仪测量误差，也表示为四元数导数的大小。这两个误差都表示陀螺仪的偏差。本实施例子使用和来定义β和ζ，表示为三轴(x,y,z)的陀螺仪测量平均误差的估计值，表示的是为三轴的陀螺仪的零偏漂移的估计速率。和的值可以通过数次实验确定。根据公式(5)描述的关系，β和ζ可以通过公式(18)和公式(19)分别确定，其中是任意的单位四元数。

图4为含有磁铁干扰情况下，三个轴的磁干扰情况。在摆动磁铁的时刻，三轴的磁干扰情况明显。图5为本公开技术方案所提出的自适应增益互补滤波器算法和medgwick方法在在磁干扰情况下的方向估计的比较，madgwick方法是一种较为成熟的九轴融合的算法，本公开技术方案通过与该方法进行比较。主要从偏航角，俯仰角和横滚角的角度进行分析比较，从图中可以看出，在无运动的磁干扰下，我们所提出的方法所获得的三个角度的摆动几乎接近于0，而medgwick方法的角度变化较为明显。图6为突然快速运动下的加速度数据变化值。图7为突然快速运动下的方向估计。在磁性均匀环境中突然快速运动，与vicon系统(vicon是一个动作捕捉系统，该系统的准确性好，精度高)的测量结果相比，即使在这种情况下，我们所提出的算法提供了相当准确的估计性能。然而，使用madgwick方法，相应的估计角度远离实际值，并且在返回初始状态后有明显的漂移。图8为陀螺仪x，y和z轴偏差的估计结果，它是对具有磁干扰的零偏漂移的补偿，从这些结果可以看出，即使在磁干扰中，本专利所提出的自适应增益互补滤波方法也可以成功地估计陀螺仪的偏差，而madgwick的方法得出了错误的估计。

本公开提出了一种用于行人方位估计的自适应增益互补滤波方法及系统，它解决了依靠惯导在长期跟踪过程中引起漂移误差累积问题，以及受周围磁场影响而引起的误差和磁场失真问题。它不仅具有一次迭代计算的优点，减少计算负荷，准确估计陀螺仪测量误差以校正陀螺仪的瞬时测量值，而且还可获得陀螺仪测量误差的精确方向。自适应增益互补滤波方法，它是以高斯-牛顿优化算法(gna)的收敛率和陀螺仪发散率为基础，将收敛率和来自于陀螺仪的发散率进行有效的互补，整个过程由：陀螺积分，对方向进行估计；将高斯-牛顿方法应用到矢量观测；自适应增益互补滤波器；补偿方案四部分组成，该方法的特点包括陀螺偏差的精确估计、陀螺瞬时测量误差的校正、快速运动和磁畸变条件下的鲁棒估计。实验结果验证了该方法的有效性，表明该方法比几种常用的定位估计方法具有更好的定位精度。

实施例子二

该实施例子公开了一种用于行人方位估计的自适应增益互补滤波系统，包括：

数据采集单元，被配置为：利用集成三轴加速度计、三轴陀螺仪、三轴磁力计的惯性传感器获得行人载体的三轴的加速度、角速度和磁场强度；

基于陀螺仪的方向估计单元，被配置为：将三轴的陀螺仪测量的数据使用四元数表示，利用四元数的导数描述传感器坐标系相对于地球坐标系下的方向变化率，在已知初始条件下，通过对四元数的导数的积分获得传感器坐标系相对于对地球坐标系下的方向；

加速度计和磁力计测量值的方向计算单元，被配置为：利用高斯-牛顿优化方法计算加速度计和磁力计测量值的方向；

陀螺仪最后的方向估计单元，被配置为：自适应滤波器的时变参数被设为每个方向的权值，增益是根据基于高斯-牛顿优化算法的低通频率的收敛率和基于陀螺仪的高通频率的发散率进行自适应调节，获得陀螺仪最后的方向估计。

该实施例子还包括：陀螺仪的零偏漂移补偿单元，由于惯性传感器因长时间跟踪而产生漂移误差累计，使用表示陀螺仪零偏漂移补偿。

加速度计和磁力计补偿单元：为了在传感器测量值中选择最可靠的矢量作为滤波器的输入，需要对加速度计和磁力计的测量值进行补偿。

实施例子三

该实施例子公开了一种用于行人方位估计的自适应增益互补滤波器，所述滤波器被配置为执行以下内容：

基于陀螺仪的方向估计：将三轴的陀螺仪测量的数据使用四元数表示，利用四元数的导数描述传感器坐标系相对于地球坐标系下的方向变化率，在已知初始条件下，通过对四元数的导数的积分获得传感器坐标系相对于地球坐标系下的方向；

利用高斯-牛顿优化方法计算加速度计和磁力计测量值的方向；

自适应滤波器的时变参数被设为每个方向的权值，增益是根据基于高斯-牛顿优化算法的低通频率的收敛率和基于陀螺仪的高通频率的发散率进行自适应调节，获得陀螺仪最后的方向估计。

实施例子四

该实施例子公开了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现以下步骤：

数据采集：利用集成三轴加速度计、三轴陀螺仪、三轴磁力计的惯性传感器获得行人载体的三轴的加速度、角速度和磁场强度；

基于陀螺仪的方向估计：将三轴的陀螺仪测量的数据使用四元数表示，利用四元数的导数描述传感器坐标系相对于地球坐标系下的方向变化率，在已知初始条件下，通过对四元数的导数的积分获得传感器坐标系相对于地球坐标系下的方向；

利用高斯-牛顿优化方法计算加速度计和磁力计测量值的方向；

自适应滤波器的时变参数被设为每个方向的权值，增益是根据基于高斯-牛顿优化算法的低通频率的收敛率和基于陀螺仪的高通频率的发散率进行自适应调节，获得陀螺仪最后的方向估计。

还包括：

陀螺仪的零偏漂移补偿，由于长时间的跟踪，惯性传感器会产生累积漂移误差，该实施例子用表示陀螺仪零偏漂移补偿。

加速度计和磁力计补偿：为了在传感器测量值中选择最可靠的矢量作为滤波器的输入，需要对加速度计和磁强计的测量值进行补偿。

关于该实施例子的相关步骤的技术内容可参见实施例子一中的具体技术内容，此处不再赘述。

实施例子五

该实施例子公开了一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现以下步骤：

数据采集：利用集成三轴加速度计、三轴陀螺仪、三轴磁力计的惯性传感器获得行人载体的三轴的加速度、角速度和磁场强度；

基于陀螺仪的方向估计：将三轴的陀螺仪测量的数据使用四元数表示，利用四元数的导数描述传感器坐标系相对于地球坐标系下的方向变化率，在已知初始条件下，通过对四元数的导数的积分获得传感器坐标系相对于地球坐标系下的方向；

利用高斯-牛顿优化方法计算加速度计和磁力计测量值的方向；

自适应滤波器的时变参数被设为每个方向的权值，增益是根据基于高斯-牛顿优化算法的低通频率的收敛率和基于陀螺仪的高通频率的发散率进行自适应调节，获得陀螺仪最后的方向估计。

还包括：

陀螺仪的零偏漂移补偿，由于长时间的跟踪，惯性传感器会产生累积漂移误差，该实施例子用表示陀螺仪零偏漂移补偿。

加速度计和磁力计补偿：为了在传感器测量值中选择最可靠的矢量作为滤波器的输入，需要对加速度计和磁强计的测量值进行补偿。

关于该实施例子的相关步骤的技术内容可参见实施例子一中的具体技术内容，此处不再赘述。

以上所述仅为本公开的优选实施例而已，并不用于限制本公开，对于本领域的技术人员来说，本公开可以有各种更改和变化。凡在本公开的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本公开的保护范围之内。