CN105068654A - 基于can总线和惯性传感器的动作捕获***及方法 - Google Patents

Abstract

本发明提出了一种基于CAN总线和惯性传感节点的动作捕获***及方法，用于实时跟踪捕获人体的运动信息，***包括惯性传感节点组合、数据汇聚节点和中心计算机。惯性传感节点组包含18个惯性传感节点，实时采集各个骨骼点部位的加速度，角速度和地磁信息数据，并利用互补融合滤波算法进行姿态解算，从而求得描述当前骨骼点三维空间姿态的四元数以及欧拉角等数据。数据汇聚节点连接这18个惯性传感节点，用来分时采集这18个惯性传感节点解算出的各个骨骼点的三维空间姿态数据，并将采集到的数据通过无线wifi模块发送给中心计算机。中心计算机的主要任务是完成采集数据汇聚节点传输来的运动数据接收和利用运动数据驱动虚拟3D人物模型的功能。

Description

基于CAN总线和惯性传感器的动作捕获***及方法

技术领域

本发明涉及人体运动学、传感器技术、健康医疗等领域，尤其涉及一种基于CAN总线和惯性传感器的动作捕获***。

背景技术

动作捕获技术是一种新型的人机交互技术，其在康复医疗、影视、动画、游戏等领域应用前景广阔。利用人体动作捕获技术，在康复医疗上，可以辅助因疾病或者交通意外后导致肢体或其他特定身体部位失去运动机能的患者进行康复治疗；此外，动作捕获技术还可以应用影视制作上，可以简化电影制作的难度，提高影视人物的逼真程度，如电影《阿凡达》中的虚拟人物就是利用动作捕获技术制作而成的。动作捕获技术应用于游戏中能够引入体感交互技术，使玩家能够利用身体的运动数据与虚拟游戏环境进行互动。

如专利CN104461013A所述，基于wifi的动作捕获***由17个惯性传感节点构成的姿态信息获取***，wifi无线路由和中心计算机三个部分组成。其中惯性传感节点分别安装在人体的多个部位，用来实时采集各部位的加速度、角速度和地磁等信息，并利用融合算法得到他的姿态角、四元数以及欧拉角等姿态信息。然后将采集到的信息和融合后的信息通过wifi方式由wifi无线路由器发送到中心计算机，中心计算机融合各个惯性传感节点的姿态信息数据完成人体动作的重构。

基于SPI的动作捕获***也是由分布在全身各主要关节点的15～17个惯性传感节点，数据汇聚节点还有中心计算机三个部分组成。其中数据汇聚节点上分别利用一根片选信号线连接再惯性传感节点上，用来分时采集安装在全身各主要关节点上的三维空间姿态数据。采集完各个节点的三维空间姿态数据后将这些数据汇总封装成一个运动数据帧，并发送给中心计算机，中心计算机利用各个传感节点的姿态信息数据完成人体动作的重构。

上述方案主要存在的缺陷有以下几点：

(1)使用无线方案每个惯性传感节点都必须配备一个锂电池单独供电，单一个节点或几个节点的电池电量不足无法工作时，整个***变无法使用。

(2)使用SPI方案的动作捕获***每个惯性传感节点都必须连接一个片选信号线，这将直接导致随着惯性传感节点的数量增加，连接在动作捕获***上数据汇聚节点的连线增加，整个***导线分布复杂；

(3)使用无线方案，每个节点尺寸和重量都不容易再减小，会在一定程度上妨碍运动。

发明内容

为解决上述技术问题，本发明提出了一种基于CAN总线和惯性传感器的动作捕获***，采用部分有线和部分无线的方式进行数据传输，在保证影响人体运动最小的前提下提高数据的传输速率、***的稳定性和使用的便利性。

由于人体的运动信息构成复杂，需要放置多个惯性传感节点，这样就对整个***的数据传输速率有了更高的要求，为了避免传统方法造成各个惯性传感节点数据传输速率不稳定，有的方法连线复杂、外观不雅等缺点，本发明提供了一种基于CAN总线的多路数据传输方案，以确保整个动作捕获***的惯性传感节点的数据传输速率稳定，接线方法简单，外观时尚等诸多优点，有效地解决了当前技术带来的不便。

具体而言，本发明提供了一种基于CAN总线和惯性传感器的动作捕获***：所述***包括惯性传感节点组、数据汇聚节点和中心计算机；其特征在于：

所述惯性传感节点组包括18个惯性传感节点，分别分布在人体主要骨骼点处，用于实时采集人体主要骨骼点的三维空间姿态数据，描述三维空间姿态的数据主要包括四元数或欧拉角；所述数据汇聚节点，是惯性传感节点姿态解算数据的汇聚中心，它完成对18个惯性传感节点的采集工作，并将采集到的姿态数据封装成运动数据帧并通过无线wifi模块将数据发送给中心计算机。

优选的，所述惯性传感节点组的每个惯性传感节点都包含电源模块、三轴加速度传感器、三轴陀螺仪传感器、三轴地磁仪传感器和CAN总线协议处理模块；其中，所述三轴加速度传感器、三轴陀螺仪传感器和三轴地磁仪传感器都使用I2C通信协议与嵌入式微处理器相连接；所述嵌入式微处理器实时采集上述九轴惯性传感器输出的加速度数据、角速度数据和地磁数据，并将采集到的数据进行滤波，计算，并套用互补融合滤波算法得出比较准确的描述当前惯性传感节点三维空间姿态信息的四元数和欧拉角数据。

优选的，所述互补融合滤波方法进行姿态解算的步骤为：

步骤一、***上电，初始化三轴加速度传感器，三轴地磁仪传感器和三轴陀螺仪传感器，并启动定时器，嵌入式微处理器实时采集九轴惯性传感器的数据并将采集到的数据进行简单的滤波去除噪声；

步骤二、利用三轴陀螺仪传感器输出的角速度数据和定时器定时时间T，对角速度数据进行积分运动，得出经过时间T，当前时刻相对于上一时刻惯性传感节点三维空间旋转过的角度值；

步骤三、利用三轴地磁仪传感器和三轴加速度传感器，利用牛顿最速下降法推导出三维空间旋转过的角度值；

步骤四、利用互补融合滤波算法融合三轴陀螺仪传感器计算出的三维空间姿态数据和三轴加速度地磁仪传感器计算出的三维空间姿态数据，采用的互补滤波器函数为：

$F\left(t\right)=F_1\left(t\right)+F_2\left(t\right)=\frac{\mathrm{\τ}}{\mathrm{\τ}+dt}gyro\left(t\right)+(1-\frac{\mathrm{\τ}}{\mathrm{\τ}+dt})accel\_mag\left(t\right)$

其中gyro(t)为陀螺仪传感器姿态解算后的值，accel_mag(t)为加速度地磁仪传感器姿态解算后的值，dt是采样时间，τ是常数。使用的滤波器函数为：

F(t)＝0.99gyro(t)+0.01accel_mag(t)

优选的，所述18个惯性传感节点主要分布于人体的主要骨骼点处，用于实时采集、计算各个主要骨骼点的三维空间位置，以便于中心计算机进行动作重构；其中，所述18个惯性传感节点分别佩戴在人体的18个部位，该18个部位分别是：头部、左手掌、左下臂、左上臂、左肩、左脚掌、左小腿、左大腿、右手掌、右下臂、右上臂、右肩、右脚掌、右小腿、右大腿、脖子、上背和下背；所述18个惯性传感节点能够精确的捕获人体的各个主要关节点的运动数据，能够解决全身的动作捕获问题。

优选的，所述惯性传感节点包括电源管理模块、九轴惯性传感器模块和CAN总线数据传输模块；所述电源管理模块用于提供稳定的3.3v电压给惯性传感节点供电，并且能够单独决定惯性传感节点的通断电功能；所述九轴惯性传感器模块包括三轴加速度传感器、三轴陀螺仪传感器和三轴地磁仪传感器，用于实时采集人体各个关键骨骼点的三维加速度、三维角速度和地磁方向的数据，利用这些数据计算出当前惯性传感节点的三维空间姿态信息；所述CAN总线数据传输模块是用于采集18个惯性传感节点姿态解算后的数据并汇总到数据汇聚节点上采用的总线通信协议；其中，所述18个CAN总线分成三组，每组各包含6个惯性传感节点，每组CAN总线节点分别连接再数据汇聚传输节点的CAN总线上。

优选的，所述数据汇聚节点包括三条CAN总线通信线路，每条CAN总线通信线路包含上下两端CAN总线接口，上端CAN总线接口用于连接人体上身肢体的惯性传感节点，下端的CAN总线接口用于连接人体下身肢体的惯性传感节点；所述三条CAN总线通信线路能够有效的提高各个惯性传感节点的姿态上传速率，因为CAN总线通信协议的节点通信速率会随着挂载在总线上的节点数量的增加而降低，采用三条CAN总线通信线路的方式，有效的降低了惯性传感节点挂载在单条CAN总线上的惯性传感节点的数量，从而确保了惯性传感节点的数据通信速率。所述数据汇聚节点还包括无线wifi模块，无线wifi通信模块接收中心计算机的控制指令并根据相应的控制指令采集18个惯性传感节点的数据并上传给中心计算机。

优选的，所述中心计算机采集数据汇聚节点发送的运动数据帧，根据各个节点的三维空间位置欧拉角和abdomen节点的三维空间位置数据利用人体动作的重构方法进行重构。所述人体动作重构方法的步骤为：

步骤一：依据人体的骨骼点的附属关系，建立人体骨骼点结构层次模型，本***建立了三条人体骨骼点层次结构模型；

步骤二、绘制人体关节点，初始化人体关节点的位置和方向，并根据关节点的方向绘制人体的骨骼并利用球面坐标系将骨骼旋转到合适的位置；

步骤三、利用实时采集到的运动数据帧来驱动人体各个关节点，并利用球面坐标旋转公式将依次旋转各个骨骼点。

同时，本发明还提出了一种基于CAN总线和惯性传感节点的动作捕获方法，其基于如权利要求1-9中的任一所述的***实现，该方法包括以下步骤：

(1)给连接在CAN总线上的惯性传感节点组上电，完成18个惯性传感节点的***初始化工作；

(2)给数据汇聚节点上电，完成数据汇聚节点与中心计算机的连接；

(3)判断是否启动动作捕获***，若是则进入第(4)步骤，否则等待；

(4)惯性传感节点采集加速度、角速度和地磁等数据，进行初次滤波，然后根据互补融合滤波算法对当前的惯性传感节点进行三维空间姿态解算，并存储到内存空间；

(5)判断是否接收到CAN总线数据发送命令，如果是则将解算后的三维空间姿态数据四元数或欧拉角发送给数据汇聚节点，否则进入第(4)步骤；

(6)数据汇聚节点循环对18个惯性传感节点进行数据采集，当接收到18个惯性传感节点的三维空间姿态数据后将这18组数据封装成运动数据帧通过wifi模块发送给中心计算机；

(7)中心计算机判断是否接受到数据，如果接受到数据则进入步骤(8)，否则进入第(6)步骤；

(8)数据采集过程结束，中心计算机通过人体动作重构方法对发送过来的运动数据帧进行解析。

本发明具有如下有益效果：

(1)本***传感器灵敏度高、姿态解算速率快、数据传输速率可靠，这意味着虚拟3D人物运动重构稳定可靠。

(2)本设计采用三条CAN总线的方式，连线简单，布线方便，外观时尚，给用户的束缚感较小。

(3)***使用无线wifi模块将人体的运动数据发送出去，这样能够直接供给PC电脑，手机，平板等多种设备使用。

(4)惯性传感节点设计尺寸小，从而保证了佩戴的方便，并且使用环境简单，穿戴上电使用即可，从而使普通非专业用户也能轻易使用本***。

(5)采用18个惯性传感节点，相比较于15～17个惯性传感节点的动作捕获设备，他能够采集左肩、右肩和脖子的运动信息，能够更好的描述人体全身的运动数据，能够给用户呈现出更好的重构效果。

附图说明

图1是本发明中人体惯性传感节点分布图；

图2是本发明中***惯性传感节点总线与数据传输节点连接图；

图3是本发明中惯性传感节点结构框图；

图4是本发明中数据汇聚节点结构框图。

具体实施方式

为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合附图及具体实施例进行详细描述。本领域技术人员应当知晓，下述具体实施例或具体实施方式，是本发明为进一步解释具体的发明内容而列举的一系列优化的设置方式，而该些设置方式之间均是可以相互结合或者相互关联使用的，除非在本发明明确提出了其中某些或某一具体实施例或实施方式无法与其他的实施例或实施方式进行关联设置或共同使用。同时，下述的具体实施例或实施方式仅作为最优化的设置方式，而不作为限定本发明的保护范围的理解。

实施例1

如图1所示，一种基于CAN总线和惯性传感器的动作捕获***是由18个惯性传感节点、数据汇聚节点和中心计算机组成的。这18个惯性传感节点分别放置在如图1所说的标记的骨骼点处，数据汇聚节点放置在人体的腰部位置。数据汇聚节点通过采集这18个惯性传感节点的姿态解算后的数据通过无线wifi模块将数据发送给中心计算机。

如图2所示，18个惯性传感节点分为3组，每组包含6个节点，分别归属Bus1，Bus2和Bus3。其中Bus1、Bus2和Bus3也分为两段：上段和下端，这样设计能够使得***整体连线最少且布线方便，简单。18个惯性传感节点分别放置在使用者的头部、左手掌、左下臂、左上臂、左肩、左脚掌、左小腿、左大腿、右手掌、右下臂、右上臂、右肩、右脚掌、右小腿、右大腿、脖子、上背和下背等部位上。

如图3所示，惯性传感节点包括主控模块，外置存储模块，电源模块、CAN总线协议处理模块和九轴惯性传感模块。其中电源模块提供了一个稳定3.3V电压的功能；外置存储模块用来存储九轴惯性传感器节点的校准数据以及节点的编号数据；主控模块包含嵌入式微处理器，时钟电路和复位电路；CAN总线模块包含CAN总线电平转换电路，CAN总线电平转换电路与嵌入式微处理器的CAN总线协议处理单元相连接；九轴惯性传感器包含三轴加速度传感器、三轴陀螺仪传感器和三轴地磁仪传感器，这九轴惯性传感器通过i2c协议与嵌入式微处理器连接。

如图4所示，数据汇聚节点包括无线wifi发送模块、主控模块、三条CAN总线协议处理模块和电源模块组成，其中无线wifi发送模块采用的是USR-wifi模块，通过串口与主控模块连；三条CAN总线协议模块主要包含两种模式，CANbus1和CANbus3采用外置CAN总线协议处理芯片和CAN总线电平转换芯片组合，CANbus2采用嵌入式微处理器集成CAN总线协议处理单元和CAN总线电平转换芯片组成。其中CANbus1和CANbus3采用的外置CAN总线协议处理芯片分别使用SPI总线与嵌入式微处理器相连。

实施例2

一种基于CAN总线和惯性传感器的动作捕获***的***，包括惯性传感节点组、数据汇聚节点和中心计算机。惯性传感节点组的主要任务是完成九轴惯性传感节点的三轴加速度、三轴陀螺仪和三轴地磁仪传感器的数据采集、滤波和姿态解算，姿态解算后获得描述当前节点三维空间姿态信息的四元数或欧拉角描述。得到的四元数或欧拉角数据通过CAN总线的形式汇聚到数据汇聚节点，并通过无线wifi模块将数据封装成运动数据帧发送给中心计算机。中心计算机首先开始建立虚拟3D人物，初始化动作与人体动作保持一致，当接收到运动数据帧之后实时动画演示用户的实时动作。

基于CAN总线和惯性传感器的动作捕获***的工作步骤如下：

(1)给连接在CAN总线上的惯性传感节点组上电，完成18个惯性传感节点的***初始化工作；

(2)给数据汇聚节点上电，完成数据汇聚节点与中心计算机的连接；

(3)判断是否启动动作捕获***，若是则进入第(4)步骤，否则等待；

(4)惯性传感节点采集加速度、角速度和地磁等数据，进行初次滤波，然后根据互补融合滤波算法对当前的惯性传感节点进行三维空间姿态解算，并存储到内存空间；

(5)判断是否接收到CAN总线数据发送命令，如果是则将解算后的三维空间姿态数据四元数或欧拉角发送给数据汇聚节点，否则进入第(4)步骤；

(6)数据汇聚节点循环对18个惯性传感节点进行数据采集，当接收到18个惯性传感节点的三维空间姿态数据后将这18组数据封装成运动数据帧通过wifi模块发送给中心计算机；

(7)中心计算机判断是否接受到数据，如果接受到数据则进入步骤(8)，否则进入第(6)步骤；

(8)数据采集过程结束，中心计算机通过人体动作重构方法对发送过来的运动数据帧进行解析。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (11)

1.一种基于CAN总线和惯性传感节点的动作捕获***，所述***包括惯性传感节点组、数据汇聚节点和中心计算机，其特征在于：

所述惯性传感节点组包括一预定数量的惯性传感节点，分别设置在人体主要骨骼点上，用于实时采集人体主要骨骼点的三维空间姿态数据，描述三维空间姿态的数据主要包括四元数或欧拉角；

所述数据汇聚节点，是惯性传感节点姿态解算数据的汇聚中心，它完成对上述预定数量的惯性传感节点的采集工作，并将采集到的姿态数据封装成运动数据帧并通过无线WIFI模块将数据发送给中心计算机。

2.如权利要求1所述的动作捕获***，其特征在于：

所述惯性传感节点组的每个惯性传感节点都包含电源模块、三轴加速度传感器、三轴陀螺仪传感器、三轴地磁仪传感器和CAN总线协议处理模块；其中，

所述三轴加速度传感器、三轴陀螺仪传感器和三轴地磁仪传感器都使用I2C通信协议与嵌入式微处理器相连接；

所述嵌入式微处理器实时采集上述九轴惯性传感器输出的加速度数据、角速度数据和地磁数据，并将采集到的数据进行滤波，计算，并套用互补融合滤波算法得出比较准确的描述当前惯性传感节点三维空间姿态信息的四元数和欧拉角数据。

3.如权利要求1所述的动作捕获***，其特征在于：

所述互补融合滤波算法进行姿态解算的步骤为：

步骤一：***上电，初始化三轴加速度传感器，三轴地磁仪传感器和三轴陀螺仪传感器，并启动定时器，嵌入式微处理器实时采集九轴惯性传感器的数据并将采集到的数据进行简单的滤波去除噪声；

步骤二：利用三轴陀螺仪传感器输出的角速度数据和定时器定时时间T，对角速度数据进行积分运动，得出经过时间T，当前时刻相对于上一时刻惯性传感节点三维空间旋转过的角度值；

步骤三：利用三轴地磁仪传感器和三轴加速度传感器，利用牛顿最速下降法推导出三维空间旋转过的角度值；

步骤四：利用互补融合滤波算法融合三轴陀螺仪传感器计算出的三维空间姿态数据和三轴加速度地磁仪传感器计算出的三维空间姿态数据，采用的互补滤波器函数为：

$F\left(t\right)=F_1\left(t\right)+F_2\left(t\right)=\frac{\mathrm{\τ}}{\mathrm{\τ}+dt}gyro\left(t\right)+(1-\frac{\mathrm{\τ}}{\mathrm{\τ}+dt})accel\_mag\left(t\right)$

其中gyro(t)为陀螺仪传感器姿态解算后的值，accel_mag(t)为加速度地磁仪传感器姿态解算后的值，dt是采样时间，τ是常数；

使用的滤波器函数为：

F(t)＝0.99gyro(t)+0.01accel_mag(t)。

4.如权利要求1-3中任一所述的动作捕获***，其特征在于：

所述惯性传感节点包括电源管理模块、九轴惯性传感器模块和CAN总线数据传输模块；

所述电源管理模块用于提供稳定的电压给惯性传感节点供电，并且能够单独决定惯性传感节点的通断电功能；

所述九轴惯性传感器模块包括三轴加速度传感器、三轴陀螺仪传感器和三轴地磁仪传感器，用于实时采集人体各个关键骨骼点的三维加速度、三维角速度和地磁方向的数据，利用这些数据计算出当前惯性传感节点的三维空间姿态信息；

所述CAN总线数据传输模块是用于采集的预定数量的惯性传感节点姿态解算后的数据并汇总到数据汇聚节点上采用的总线通信协议；

其中，所述预定数量的CAN总线分成三组，每组各包含相同数量的惯性传感节点，每组CAN总线节点分别连接，再将数据汇聚传输节点的CAN总线上。

5.如权利要求1所述的动作捕获***，其特征在于：

所述数据汇聚节点包括三条CAN总线通信线路，每条CAN总线通信线路包含上下两端CAN总线接口，上端CAN总线接口用于连接人体上身肢体的惯性传感节点，下端的CAN总线接口用于连接人体下身肢体的惯性传感节点；

所述数据汇聚节点还包括无线WIFI通信模块，无线WIFI通信模块接收中心计算机的控制指令并根据相应的控制指令采集预定数量的惯性传感节点数据并上传给中心计算机。

6.如权利要求1所述的动作捕获***，其特征在于：

所述中心计算机采集数据汇聚节点发送的运动数据帧，根据各个节点的三维空间位置欧拉角和abdomen节点的三维空间位置数据利用人体动作的重构方法进行重构。

7.如权利要求1-6中任一所述的动作捕获***，其特征在于：

所述预定数量的惯性传感节点主要分布于人体的主要骨骼点处，用于实时采集、计算各个主要骨骼点的三维空间位置，以便于中心计算机进行动作重构。

8.如权利要求1-6中任一所述的动作捕获***，其特征在于：

所述预定数量为18,所述的18个惯性传感节点分别佩戴在人体的18个部位，该18个部位分别是：头部、左手掌、左下臂、左上臂、左肩、左脚掌、左小腿、左大腿、右手掌、右下臂、右上臂、右肩、右脚掌、右小腿、右大腿、脖子、上背和下背。

9.一种基于CAN总线和惯性传感节点的动作捕获方法，其基于如权利要求1-8中的任一所述的***实现，该方法包括以下步骤：

(1)给连接在CAN总线上的惯性传感节点组上电，完成预定数量的惯性传感节点的***初始化工作；

(2)给数据汇聚节点上电，完成数据汇聚节点与中心计算机的连接；

(3)判断是否启动动作捕获***，若是则进入第(4)步骤，否则等待；

(4)惯性传感节点采集加速度、角速度和地磁等数据，进行初次滤波，然后根据互补融合滤波算法对当前的惯性传感节点进行三维空间姿态解算，并存储到内存空间；

(5)判断是否接收到CAN总线数据发送命令，如果是则将解算后的三维空间姿态数据四元数或欧拉角发送给数据汇聚节点，否则进入第(4)步骤；

(6)数据汇聚节点循环对上述预定数量的惯性传感节点进行数据采集，当接收到惯性传感节点的三维空间姿态数据后将数据封装成运动数据帧通过WIFI模块发送给中心计算机；

(7)中心计算机判断是否接受到数据，如果接受到数据则进入步骤(8)，否则进入第(6)步骤；

(8)数据采集过程结束，中心计算机通过人体动作重构方法对发送过来的运动数据帧进行解析。

10.权利要求9所述的动作捕获方法，其特征在于：

所述人体动作重构方法的步骤为：

步骤一：依据人体的骨骼点的附属关系，建立人体骨骼点结构层次模型，本***建立了三条人体骨骼点层次结构模型；

步骤二：绘制人体关节点，初始化人体关节点的位置和方向，并根据关节点的方向绘制人体的骨骼并利用球面坐标系将骨骼旋转到合适的位置；

步骤三：利用实时采集到的运动数据帧来驱动人体各个关节点，并利用球面坐标旋转公式将依次旋转各个骨骼点。

11.权利要求9或10所述的动作捕获方法，其特征在于：

所述互补融合滤波算法进行姿态解算的步骤为：

步骤一：***上电，初始化三轴加速度传感器，三轴地磁仪传感器和三轴陀螺仪传感器，并启动定时器，嵌入式微处理器实时采集九轴惯性传感器的数据并将采集到的数据进行简单的滤波去除噪声；

步骤二：利用三轴陀螺仪传感器输出的角速度数据和定时器定时时间T，对角速度数据进行积分运动，得出经过时间T，当前时刻相对于上一时刻惯性传感节点三维空间旋转过的角度值；

步骤三：利用三轴地磁仪传感器和三轴加速度传感器，利用牛顿最速下降法推导出三维空间旋转过的角度值；

步骤四：利用互补融合滤波算法融合三轴陀螺仪传感器计算出的三维空间姿态数据和三轴加速度地磁仪传感器计算出的三维空间姿态数据，采用的互补滤波器函数为：

$F\left(t\right)=F_1\left(t\right)+F_2\left(t\right)=\frac{\mathrm{\τ}}{\mathrm{\τ}+dt}gyro\left(t\right)+(1-\frac{\mathrm{\τ}}{\mathrm{\τ}+dt})accel\_mag\left(t\right)$

其中gyro(t)为陀螺仪传感器姿态解算后的值，accel_mag(t)为加速度地磁仪传感器姿态解算后的值，dt是采样时间，τ是常数；

使用的滤波器函数为：

F(t)＝0.99gyro(t)+0.01accel_mag(t)。