加速度计+陀螺仪+磁力计+互补滤波+DMP---以MPU6050为例
一、MPU6050
介绍
MPU6050内置了运动融合引擎，用于手持和桌面的应用程序、游戏控制器、体感遥控以及其他消费电子设备。它内置一个三轴 MEMS 陀螺仪、一个三轴 MEMS 加速度计、一个数字运动处理引擎（DMP）以及用于第三方的数字传感器接口的辅助 I2C 端口（常用于扩展磁力计）。当辅助 I2C 端口连接到一个三轴磁力计，MPU6050 能提供一个完整的九轴融合输出到其主 I2C 端口。
MPU6050 拥有16位模/数转换器（ADC），将三轴陀螺仪及三轴加速度计数据转化为数字量输出。为了精确跟踪快速和慢速运动，MPU6050 支持用户可编程的陀螺仪满量程范围有：±250、±500、±1000 与 ±2000（单位为 °/s 或 dps），支持用户可编程的加速度计满量程范围有：±2G、±4G、±8G 与 ±16G。同时 MPU6050 内置了一个可编程的低通滤波器，可用于传感器数据的滤波。
MPU6050 数据传输可通过最高至 400kHz 的 I2C 总线完成，它的封装尺寸为 4mm * 4mm * 0.9mm（QFN）。其他的特性包括内置的温度传感器以及在整个工作温度范围内的振荡频率仅有 ±1% 波动的振荡器，并具有高达 10000 GHz 的碰撞容忍度。
MPU6050 的电源供应十分灵活，电源电压 VDD 供电范围为 2.375~3.46 V。此外，提供了 I2C 接口的逻辑参考电平 VLOGIC，它可以是 1.8 V ± 5% 或 VDD 的电压。MPU6050 的三轴陀螺仪正常工作电流为 3.6 mA，待机电流低至 5 μA；三轴加速度计正常工作电流为 500 μA，低功耗模式下工作电流最低为 10 μA。通过片上的数字运动处理引擎 DMP可减少复杂的融合运算负荷，由DMP完成四元数运算，微控制器只需要简单的读取和计算，即可得到融合姿态。使能六轴传感器以及 DMP 仅需 3.9 mA 的工作电流。
要玩转 MPU6050，首先驱动 I2C 总线，然后初始化 MPU6050，最后从 MPU6050 读取数据。MPU6050的地址有两个0x68（ADO引脚接GND或不接）、0x69（ADO接VCC）。
参数
1、通信方式：IIC
2、工作电压：2.3~3.46V
3、工作模式：主模式、旁路模式
4、内部资源：陀螺仪、加速度计、DMP、从IIC
5、使用：一般使用陀螺仪和加速度计采用互补滤波算法就可以计算倾角，在四轴上常使用DMP利用四元数法获取融合姿态。
模块资源
A、一个三轴加速度计可测量三轴加速度；
B、一个三轴陀螺仪可测量三轴角速度；
C、16位AD模块用于采集传感器数值；
D、一个DMP模块，用于解算数据，输出四元数；
E、一个从IIC模块（AUX），用于与其他传感器模块通信；
F、一个主IIC模块，与控制器通信。
二、加速度计
A、工作原理
一个加速度计我们可以把它想作一个圆球在一个方盒子中。
我们假定这个盒子不在重力场中或者其他任何会影响球的位置的场中，球处于盒子的正中央。你可以想象盒子在外太空中，远离任何天体，一切东西都处于无重力状态。在上面的图中你可以看到我们给每个轴分配了一对墙（我们移除了Y+以此来观察里面的情况）。设想每面墙都能感测压力。如果我们突然把盒子向左移动（加速度为1g=9.8m/s^2），那么球会撞上X-墙。然后我们检测球撞击墙面产生的压力，X轴输出值为-1g。
请注意加速度计检测到的力的方向与它本身加速度的方向是相反的。这种力量通常被称为惯性力或假想力 。在这个模型中你你应该学到加速度计是通过间接测量力对一个墙面的作用力来测量加速度的，在实际应用中，可能通过弹簧等装置来测量力。这个力可以是加速度引起的，但在下面的例子中，我们会发现它不一定是加速度引起的。
如果我们把模型放在地球上，球会落在Z-墙面上并对其施加一个1g的力，见下图：
在这种情况下盒子没有移动但我们任然读取到Z轴有-1g的值。球在墙壁上施加的压力是由引力造成的。在理论上，它可以是不同类型的力量 - 例如，你可以想象我们的球是铁质的，将一个磁铁放在盒子旁边那球就会撞上另一面墙。引用这个例子只是为了说明加速度计的本质是检测力而非加速度。只是加速度所引起的惯性力正好能被加速度计的检测装置所捕获。
虽然这个模型并非一个MEMS传感器的真实构造，但它用来解决与加速度计相关的问题相当有效。实际上有些类似传感器中有金属小球，它们称作倾角开关，但是它们的功能更弱，只能检测设备是否在一定程度内倾斜，却不能得到倾斜的程度。
到目前为止，我们已经分析了单轴的加速度计输出，这是使用单轴加速度计所能得到的。三轴加速度计的真正价值在于它们能够检测全部三个轴的惯性力。让我们回到盒子模型，并将盒子向右旋转45度。现在球会与两个面接触：Z-和X-，见下图：
以上模型可以看作加速度计的通俗理解，实际上的加速度计有很多类型，但是原理差不多，下图是MMA7361的简化换能器物理模型，使用的就是电容型模型，在存在加速度时会使电容中间的极板发生位移，导致电容电平变化。
B、倾角计算原理
事实上一个加速度计就快可以计算角度，但是单纯使用加速度计求角度，会有很大的噪声，且容易受到干扰，只适用于静态，所以不推荐，最好还是用陀螺仪+加速度计的方式获取角度。当然在要求不高的场合还是可以考虑只用加速度计的。
如果芯片水平静置，X、Y方向的重力分量为0g，而Z轴方向的重力分量为g。如下图所示，X=0；Y=0 Z=g
如果各边与水平方向有一些夹角，则其图像如图2所示，
X轴方向的加速度大小为Ax，其与水平线的夹角为α1，与重力加速度g的夹角α；
Y轴方向的加速度为Ay，与水平线的加速度为 β1，与重力加速度g的夹角为β；
Z轴方向的加速度为Az，与水平线的加速度为 γ1，与重力加速度g的夹角为γ。
基于图2中的夹角概念，它们的关系为
α = 90度- α1， β = 90度- β1 ， γ = 90度- γ1。
g在各轴方向上的分量为：
Ax = gcosα，
Ay = gcosβ ，
Az = gcosγ；
将3中数据代入得：
Ax = gcosα = gcos(90度- α1) =gsinα1 ，
Ay =gsinβ1 ，
Az = gsin γ1.
如图3所示。(其中各垂直虚线的大小为：g * g = Ax * Ax + gcosα1 * gcosα1， 则
gcosα1 = squr(g * g - Ax * Ax) ，
gcosβ1 = squr (g * g - Ay * Ay )，
gcosγ1 = squr(g * g - Az * Az )）。
根据立体几何中，g相当于立方体的对角线，Ax、Ay、Az相当于三条边，如图4所示，虚线大小等于 Ay * Ay+Az * Az，所以根据勾股定理Ax * Ax + Ay * Ay + Az * Az = g * g
由4和5可以知道，(以X轴为例) sinα1 = Ax/g， cosα1 = squr(g * g - Ax * Ax) / g ，
tanα1 =Ax / squr(g * g - Ax * Ax) = Ax / squr(Ay * Ay + Az * Az) 。
tanβ1 = Ay / squr(Ax * Ax+Az * Az) ,
tanγ1 = Az / squr(Ax * Ax +Ay * Ay)。
最后得出加速度传感器值与角度值(弧度)的关系为：
tanα1 = Ax / squr(Ay * Ay + Az * Az) ，
tanβ1 = Ay / squr(Ax * Ax+Az * Az) ，
tanγ1 = Az / squr(Ax * Ax +Ay * Ay)。
其中 α1 、β1 、γ1分别是X、Y、Z轴和水平线的弧度值(反三角函数计算的值是弧度)，Ax 、Ay、Az是三个轴上的加速度值。那么弧度值分别为：
α1 = arctan(Ax / squr(Ay * Ay + Az * Az))
β1= arctan(Ay / squr(Ax * Ax+Az * Az))
γ1= arctan( Az / squr(Ax * Ax +Ay * Ay))
接下来就得使用数据公式：弧度= θπR/180 。 这样算得θ = 弧度 * 180/πR,其中R取1。最后得到的各轴的角度值分别为：
θx = α1 * 180/π = [arctan(Ax / squr(Ay * Ay + Az * Az))] * 180/π
θy =β1 * 180/π = [arctan(Ay / squr(Ax * Ax+Az * Az))] * 180/π
θz =γ1 * 180/π = [arctan( Az / squr(Ax * Ax +Ay * Ay))] * 180/π
三、陀螺仪
陀螺仪（Gyroscope、GYRO-Sensor）也叫地感器，传统结构是内部有个陀螺，如下图所示（三轴陀螺），三轴陀螺仪的工作原理是通过测量三维坐标系内陀螺转子的垂直轴与设备之间的夹角，并计算角速度，通过夹角和角速度来判别物体在三维空间的运动状态。三轴陀螺仪可以同时测定上、下、左、右、前、后等6个方向（合成方向同样可分解为三轴坐标），最终可判断出设备的移动轨迹和加速度。
在如今的电子模块里使用的是电子陀螺仪，其工作原理利用了科里奥利加速度.
陀螺仪的内部原理是这样的：对固定电极施加电压，并交替改变电压，让一个质量块做振荡式来回运动，当旋转时，会产生科里奥利加速度，此时就可以对其进行测量。 角速率由科氏加速度测量结果决定：
- 科氏加速度 = 2 × (w × 质量块速度)
- w是施加的角速率(w = 2 πf)
科氏加速度与谐振器具有相同的频率和相位，因此可以抵消低速外部振动 ，施加变化的电压来回移动器件，此时器件只有水平运动没有垂直运动。如果施加旋转，可以看到器件会上下移动，外部指将感知该运动，从而就能拾取到与旋转相关的信号。
上面的动画，只是抽象展示了陀螺仪的工作原理，而真实的陀螺仪内部构造是下面这个样子
将加速度计和陀螺仪的数据读取出来后还需要进行融合算法才能得到角度，而融合算法 种类更多，有一阶互补滤波、二阶互补滤波、卡尔曼滤波、四元数法。
四、磁力计
电子罗盘是一种重要的导航工具，能实时提供移动物体的航向和姿态。随着半导体工艺的进步和手机操作系统的发展，集成了越来越多传感器的智能手机变得功能强大，很多手机上都实现了电子罗盘的功能。而基于电子罗盘的应用（如Android的Skymap）在各个软件平台上也流行起来。
要实现电子罗盘功能，需要一个检测磁场的三轴磁力传感器和一个三轴加速度传感器。随着微机械工艺的成熟，意法半导体推出将三轴磁力计和三轴加速计集成在一个封装里的二合一传感器模块LSM303DLH，方便用户在短时间内设计出成本低、性能高的电子罗盘。本文以LSM303DLH为例讨论该器件的工作原理、技术参数和电子罗盘的实现方法。
1. 地磁场和航向角的背景知识
如图1所示，地球的磁场象一个条形磁体一样由磁南极指向磁北极。在磁极点处磁场和当地的水平面垂直，在赤道磁场和当地的水平面平行，所以在北半球磁场方向倾斜指向地面。用来衡量磁感应强度大小的单位是Tesla或者Gauss（1Tesla=10000Gauss）。随着地理位置的不同，通常地磁场的强度是0.4-0.6 Gauss。需要注意的是，磁北极和地理上的北极并不重合，通常他们之间有11度左右的夹角。
2．ST集成磁力计和加速计的传感器模块LSM303DLH
2.1 磁力计工作原理
在LSM303DLH中磁力计采用各向异性磁致电阻（Anisotropic Magneto-Resistance）材料来检测空间中磁感应强度的大小。这种具有晶体结构的合金材料对外界的磁场很敏感，磁场的强弱变化会导致AMR自身电阻值发生变化。
在制造过程中，将一个强磁场加在AMR上使其在某一方向上磁化，建立起一个主磁域，与主磁域垂直的轴被称为该AMR的敏感轴，如图3所示。为了使测量结果以线性的方式变化，AMR材料上的金属导线呈45º角倾斜排列，电流从这些导线上流过，如图4所示。由初始的强磁场在AMR材料上建立起来的主磁域和电流的方向有45º的夹角。
当有外界磁场Ha时，AMR上主磁域方向就会发生变化而不再是初始的方向了，那么磁场方向和电流的夹角θ也会发生变化，如图5所示。对于AMR材料来说，θ角的变化会引起AMR自身阻值的变化，并且呈线性关系，如图6所示。
当R1=R2=R3=R4=R，在外界磁场的作用下电阻变化为∆R时，电桥输出?V正比于?R。这就是磁力计的工作原理。
2.2 置位/复位(Set/Reset)电路
由于受到外界环境的影响，LSM303DLH中AMR上的主磁域方向不会永久保持不变。LSM303DLH内置有置位/复位电路，通过内部的金属线圈周期性的产生电流脉冲，恢复初始的主磁域，如图8所示。需要注意的是，置位脉冲和复位脉冲产生的效果是一样的，只是方向不同而已。
置位/复位电路给LSM303DLH带来很多优点：
1） 即使遇到外界强磁场的干扰，在干扰消失后LSM303DLH也能恢复正常工作而不需要用户再次进行校正。
2） 即使长时间工作也能保持初始磁化方向实现精确测量，不会因为芯片温度变化或内部噪音增大而影响测量精度。
3） 消除由于温漂引起的电桥偏差。
2.3 LSM303DLH的性能参数
LSM303DLH集成三轴磁力计和三轴加速计，采用数字接口。磁力计的测量范围从1.3 Gauss到8.1 Gauss共分7档，用户可以自由选择。并且在20 Gauss以内的磁场环境下都能够保持一致的测量效果和相同的敏感度。它的分辨率可以达到8 mGauss并且内部采用12位ADC，以保证对磁场强度的精确测量。和采用霍尔效应原理的磁力计相比，LSM303DLH的功耗低，精度高，线性度好，并且不需要温度补偿。
LSM303DLH具有自动检测功能。当控制寄存器A被置位时，芯片内部的自测电路会产生一个约为地磁场大小的激励信号并输出。用户可以通过输出数据来判断芯片是否正常工作。
作为高集成度的传感器模组，除了磁力计以外LSM303DLH还集成一颗高性能的加速计。加速计同样采用12位ADC，可以达到1mg的测量精度。加速计可运行于低功耗模式，并有睡眠/唤醒功能，可大大降低功耗。同时，加速计还集成了6轴方向检测，两路可编程中断接口。
3. ST电子罗盘方案介绍
一个传统的电子罗盘系统至少需要一个三轴的磁力计以测量磁场数据，一个三轴加速计以测量罗盘倾角，通过信号条理和数据采集部分将三维空间中的重力分布和磁场数据传送给处理器。处理器通过磁场数据计算出方位角，通过重力数据进行倾斜补偿。这样处理后输出的方位角不受电子罗盘空间姿态的影响，如图9所示。
LSM303DLH的典型应用如图10所示。它需要的周边器件很少，连接也很简单，磁力计和加速计各自有一条I2C总线和处理器通信。如果客户的I/O接口电平为1.8V，Vdd_dig_M、Vdd_IO_A和Vdd_I2C_Bus均可接1.8V供电，Vdd使用2.5V以上供电即可；如果客户接口电平为2.6V，除了Vdd_dig_M要求1.8V以外，其他皆可以用2.6V。在上文中提到，LSM303DLH需要置位/复位电路以维持AMR的主磁域。C1和C2为置位/复位电路的外部匹配电容，由于对置位脉冲和复位脉冲有一定的要求，建议用户不要随意修改C1和C2的大小。
4. 铁磁场干扰及校准
电子指南针主要是通过感知地球磁场的存在来计算磁北极的方向。然而由于地球磁场在一般情况下只有微弱的0.5高斯，而一个普通的手机喇叭当相距2厘米时仍会有大约4高斯的磁场，一个手机马达在相距2厘米时会有大约6高斯的磁场，这一特点使得针对电子设备表面地球磁场的测量很容易受到电子设备本身的干扰。
磁场干扰是指由于具有磁性物质或者可以影响局部磁场强度的物质存在，使得磁传感器所放置位置上的地球磁场发生了偏差。如图11所示，在磁传感器的XYZ 坐标系中，绿色的圆表示地球磁场矢量绕z轴圆周转动过程中在XY平面内的投影轨迹，再没有外界任何磁场干扰的情况下，此轨迹将会是一个标准的以O(0,0)为中心的圆。当存在外界磁场干扰的情况时，测量得到的磁场强度矢量α将为该点地球磁场β与干扰磁场γ的矢量和。记作：
4.1 平面校准方法
针对XY轴的校准，将配备有磁传感器的设备在XY平面内自转，如图11，等价于将地球磁场矢量绕着过点O(γx,γy)垂直于XY平面的法线旋转， 而红色的圆为磁场矢量在旋转过程中在XY平面内投影的轨迹。这可以找到圆心的位置为((Xmax + Xmin)/2, (Ymax + Ymin)/2). 同样将设备在XZ平面内旋转可以得到地球磁场在XZ平面上的轨迹圆，这可以求出三维空间中的磁场干扰矢量γ(γx, γy, γz).
4.2 立体8字校准方法
一般情况下，当带有传感器的设备在空中各个方向旋转时，测量值组成的空间几何结构实际上是一个圆球，所有的采样点都落在这个球的表面上，如图13所示，这一点同两维平面内投影得到的圆类似。
这种情况下，可以通过足够的样本点求出圆心O(γx, γy, γz), 即固定磁场干扰矢量的大小及方向。公式如下：
8字校准法要求用户使用需要校准的设备在空中做8字晃动，原则上尽量多的让设备法线方向指向空间的所有8个象限，如图14所示。
4.2 十面校准方法
同样，通过以下10面校准方法，也可以达到校准的目的。
对于一个物体在空中的姿态，导航系统里早已有定义，如图17所示，Android中也采用了这个定义。Pitch(Φ)定义为x轴和水平面的夹角，图示方向为正方向；Roll(θ)定义为y轴和水平面的夹角，图示方向为正方向。由Pitch角引起的航向角的误差如图18所示。可以看出，在x轴方向10度的倾斜角就可以引起航向角最大7-8度的误差。
手机在空中的倾斜姿态如图19所示，通过3轴加速度传感器检测出三个轴上重力加速度的分量，再通过式2可以计算出Pitch和Roll。
式3可以将磁力计测得的三轴数据（XM，YM ，ZM）通过Pitch和Roll转化为式1中计算航向角需要的Hy和Hx。之后再利用式1计算出航向角。
转自：http://www.dzsc.com/data/2010-11-29/87454.html
五、传感器的校准
所谓校准其实就是计算零偏值，而后传感器读取值减去零偏即完成校准。
1）陀螺仪：测量的是角速度，将传感器静止时角速度应当为0，所以静止放置传感器，此时的陀螺仪数值即为零偏；
2）加速度计：测量的是三轴与重力的夹角，Z轴向上静止放置时，XY读数为0，Z读数为 1g 翻过来为 -1g（AD值与量程有关，1g对应量程最大值）。对于XY轴校准，将传感器Z轴向上静止放置，XY轴的读数为零偏；对于Z轴，先将Z轴向上读取数值（1g），再将Z轴向下读取数值（-1g），两个数值相加 除2 即为零偏。也可以使用椭球校准法，这种方法不需要区分XYZ
3）磁力计：测量的是XYZ轴与地磁场的夹角，以Z轴为例，传感器向上和向下的Z轴数据应当大小相等，方向相反。校准使用球面校准法、椭球校准法。
PS：
1.使用MPU6050得出的偏航角Yaw具有很大的漂移，温度低时最为严重，如果要使用Yaw角度建议使用MPU9250，9250内有3轴磁力计可以补偿漂移，Yaw角度数据较为稳定。
2.因为加速度计无法感知z轴上的运动，把三轴加速度计放在空间中，水平自旋，它三轴输出数据是不变的。所以，没有电子罗盘，偏航角Yaw就是z轴陀螺仪的积分，肯定会漂移的。
3.磁力计容易受到干扰，虽然它能够修正XYZ轴的角度，但是一般只用它修正Z轴的角度，用于消除漂移。
4.校准后除了观察各传感器数值变化规律外，还应当观察姿态角规律，其中Pitch变化在 -180 ~ 180 连续变化，Roll变化在 -90 ~ 90 连续变化,Yaw变化在 -180 ~ 180 连续变化。融合后传感器Y轴为器件中轴，Y轴正方向指向磁北时 Yaw = 0，向左偏为加，向右为减。Y轴与地平面的夹角为Pitch，Y轴正方向与地平面平行时 Pitch = 0 ，Y轴向上翘为加，向下翘为减。绕Y轴翻滚为Roll，器件水平放置时 Roll = 0，向右翻滚为加。
六、互补滤波
多个传感器，以2个为例，分别是传感器A、B，两者各有长处短处，将AB的短处克服，长处互补，即A的长处补B的短处，B的长处补A的短处，此种方法为互补滤波。最常见的是陀螺仪和加速度计的互补。
常用例
加速度计，输出加速度大小，由其测量原理导致对高频信号敏感，在震动环境中干扰较大。
陀螺仪，输出角速度，对其积分得出角度，但在积分作用下导致低频干扰和漂移。
磁力计，输出机体与地磁场夹角，低频特性好，易受磁场干扰。
在四轴中，加速度计/磁力计对振动敏感，取其值计算倾角误差大，而陀螺仪积分的角度受高频振动影响小，但随时间增长有漂移。互补滤波就是在短时间内以陀螺仪为主，定时用加速度计均值校正陀螺仪；长时间以加速度计为主，加大它的比重，这就是互补；由于陀螺仪高频信号好，低频存在干扰，故将陀螺仪信号通过一个高通滤波器，同样加速度计信号通过一个低通滤波器，这就是滤波，当然条件不足时也可以软件滤波；滤波后再互补。
互补公式（信号已滤波）
陀螺仪积分角度 += 加速度*dt；
融合角度 = A * 陀螺仪积分角度 + B * 加速度角度；
（A + B = 1）
（互补是给他们不同的权重，加权求和）
以MPU6050为例的程序
在读取数据后需要进行一下步骤:
A、读取数据；
B、去除零偏；
C、先期处理；
D、融合算法。
这里 先期处理 包含多种形式，我总结了二种在下面程序中标注出来了；融合算法 一般有DMP、互补滤波、卡尔曼滤波，下面使用的是一阶互补滤波。
计算角度我理解是以加速度为主，利用加速度计的坐标与地平面坐标的夹角得到具体角度，随后由于单纯的加速度计数据干扰大，所以引入陀螺仪数据作为辅助，令角度数据克服干扰变得稳定。
//第一种方法：
//读取数据
//去除零偏
//单位转换
//加速度计计算角度
//互补滤波1
void MPU_HubuLvbo()
{
float Q =0.02;
float ax_angle=0,ay_angle=0,az_angle=0;
float gx_rate=0,gy_rate=0,gz_rate=0;
float acc_x=0,acc_y=0,acc_z=0;
MPU_AD();//读取数据
MMA7361_x = MMA7361_x - MMA7361_x_off;//去除零偏
MMA7361_z = MMA7361_z - MMA7361_z_off;//去除零偏
//MMA7361_x = MMA7361_x / 16384.0;
//MMA7361_z = MMA7361_z / 16384.0;
//加速度计的读取值除以灵敏度可以把单位转换为g，16384可以从数据手册里得到，它和量程有关。但是由于atan2(x,y)=arctan(y/x);所以在y/x中16384被约掉了，所以灵敏度转换可以省略。
ax_angle = atan2(MMA7361_x,MMA7361_z)*180/3.14;
//使用加速度计计算角度，反三角函数的数值单位是弧度，所以180/3.14是转换成°为单位
ENC03_x = ENC03_x - ENC03_x_off;//去除零偏
gx_rate = ENC03_x/131.0;
//陀螺仪的读取值除以灵敏度可以把单位转换为°/s，131可以从数据手册里得到，它和量程有关。
angle_x = Q*(angle_x + gx_rate*0.2) + (1-Q)*(ax_angle); //互补滤波
}
//第二种方法：
//读取数据
//去除零偏
//单位转换
//加速度计计算角度
//互补滤波2
void MPU_HubuLvbo()
{
float Q =0.02;
float ax_angle=0,ay_angle=0,az_angle=0;
float gx_rate=0,gy_rate=0,gz_rate=0;
float acc_x=0,acc_y=0,acc_z=0;
float T=0.23,DT=0.2;//DT是采样周期
MPU_AD();//读取数据
MMA7361_x = MMA7361_x - MMA7361_x_off;//去除零偏
//MMA7361_x = MMA7361_x / 16384.0;
//加速度计的读取值除以灵敏度可以把单位转换为g，16384可以从数据手册里得到，它和量程有关。但是在除法中16384被约掉了，所以这里可以省略。
ax11_angle = MMA7361_x*180/(MMA7361_x_MAX - MMA7361_x_MIN);//区间映射 -90°~90°
ENC03_x = ENC03_x - ENC03_x_off;//去除零偏
gx_rate = ENC03_x/131.0;
//陀螺仪的读取值除以灵敏度可以把单位转换为°/s，131可以从数据手册里得到，它和量程有关。
angle_x += (gx_rate + (ax_angle - angle_x)/T)*DT;//互补滤波
}
上面程序，第一个是基础的互补滤波公式，第二个也是互补滤波公式，两者在读取加速度计和陀螺仪数据后均进行了先期处理而后才使用互补滤波算法。
第一种方式的波形如下：
上图是传感器绕Y轴旋转整圈X轴与水平面的夹角的变化，红色是加速度计计算的角度值ax11_angle，棕色是融合角度数据angle_x 。可以看出，角度能够从180°~-180°连续变化，但是在180°和-180°的临界面会有数值的突然变化。这种方式陀螺仪作用较小，融合角度几乎完全逼近加速度计计算的角度值。
第二种方式的波形如下
上图是传感器绕Y轴旋转整圈X轴与水平面的夹角的变化，红色是加速度计计算的角度值ax11_angle，棕色是融合角度数据angle_x 。可以看出，角度能够从90°~-90°连续变化。这种方式陀螺仪作用较大，抗干扰能力强，但是数据逼近加速度计计算的角度值的程度减小。
第二种方法的说明
第二种滤波方式来自比赛时的学习文档 “4、直立平衡车模参考设计方案（2011）” 文档不上传了，具体算法框图如下：
VOLTAGE_GRAVITY：加速度计采集值；
GRAVITY_OFFSET：加速度计零偏；
GRAVITY_ANGLE_RATIO：加速度计归一化系数，
GRAVITY_ANGLE_RATIO=180/(MMA7361_x_MAX - MMA7361_x_MIN)
VOLTAGE_GYRO：陀螺仪采集值；
GYROSCOPE_OFFSET：陀螺仪零偏；
GYROSCOPE_ANGLE_RATIO：陀螺仪系数，
GYROSCOPE_ANGLE_RATIO=131；
GRAVITY_ADJUST_TIME_CONSTANT：加速度计时间常数，其值=1/Tg，Tg越大角度输出跟踪越慢，但可以有效抑制加速度计上的噪声，Tg过大会放大陀螺仪输出误差，一般取 Tg=1~4.
GYROSCOPE_ANGLE_SIGMA_FREQUENCY:积分时间常数，是原始数据采集的时间间隔的倒数。
滤波的最终目的和效果
1、以陀螺仪的特性消除加速度计在测量角度时的噪声和误差，实际上就是用加速度计测量角度，然而这个角度容易被干扰，所以用陀螺仪的角度对其进行优化。
2、最终可以得到较为稳定的，变化较为平滑的加速度计角度，该角度以加速度计的测量角度值为主；
3、通过滤波后加速度计角度对滤波前加速度计角度的追踪对比，可看出滤波后的角度对震动反应变慢，对突然的变化反应具有一定延迟，整体变化平滑；
4、单纯的互补滤波得到的3轴角度，是XYZ轴相对于重力加速度的（即垂直向下的方向的）夹角，与常说的姿态角俯仰角Pitch、偏航角Yaw、翻滚角Roll不一样。
七、DMP
DMP就是对三轴加速度计和三轴陀螺仪的ADC数据进行融合计算，得出四元数，再利用四元数计算姿态角。想要实现DMP一般有2种方式：硬件DMP和软件DMP。
硬件DMP
MPU6050芯片内部有一个DMP模块，该模块能够利用三轴加速度计和三轴陀螺仪的ADC数据，计算四元数，而用户只需要读取四元数，并转换成姿态角就可以了。这样的方式节省了主控芯片的资源，比较可靠、方便。但是MPU6050内部的DMP模块是没有对外公开的，既代码不开源，官方给的代码是针对STM8 MSP430芯片的，幸运的是网上大佬比较多，我们有机会把官方代码移植到STM32中。
以下是移植库代码教程。
》》》》”小马哥STM32课程系列直播-如何两个月做出自己的暴力空心杯小四轴”
链接：http://www.moore8.com/courses/1406
后面的移植笔记不是本人的，是在网上找到的，放到这里是方便随时查看。
原文链接：https://blog.csdn.net/qq646497210/article/details/83096361
记录一下MPU6050的dmp代码移植的过程
1、准备四个东西
1）mpu6050硬件
2）mpu6050寄存器手册，
3）stm32f10x单片机
4）dmp固件库(我的是5.1版的)
PS:固件库只需要提取下面7个文件即可，其他文件没啥用
“inv_mpu_dmp_motion_driver.h”
“inv_mpu_dmp_motion_driver.c”
“inv_mpu.h”
“inv_mpu.c”
“dmpmap.h”
“dmpKey.h”（前6个在同一个目录下）
“motion_driver_test.c“
把前6个文件通过keil导入文件的方式，导入的keil中。第7个文件不是代码文件，而是适用于msp430的main函数文件，我们把第七个文件当作参照，来自己写一份关于stm32的main文件即可.
2、观察它文件内容的结构
1）整体文件分析
在”inv_mpu.c” “inv_mpu_dmp_motion_driver.c”这两个文件里面全是大片大片的英文，但是仔细看，你会发现，这么多的英文，其实结构很清晰，都是单个的模块的函数内容定义的集合以及一些条件预定义而已，并不是什么杂乱无章的东西，。打开“motion_driver_test.c“，你也会发现也是大量的函数定义以及条件预定义和一个main函数，而这个main函数是针对MSP430的一个代码，刚好是用于读取实时的欧拉角代码，首先我们对main函数进行分析，我们知道一个main函数一般分为两部分，各个内外设的初始化，和while循环内的无限操作函数，对于我们的要求也是也一样，我们无非也就是需要两个代码，mpu6050的初始化，以及无限的读出欧拉角，刚好和main函数对应上了，所以开始分析代码，因为main上自带英文注释，咱先把它通过谷歌翻译在”inv_mpu.c” “inv_mpu_dmp_motion_driver.c”这两个文件里面全是大片大片的英文，但是仔细看，你会发现，这么多的英文，其实结构很清晰，都是单个的模块的函数内容定义的集合以及一些条件预定义而已，并不是什么杂乱无章的东西，。打开“motion_driver_test.c“，你也会发现也是大量的函数定义以及条件预定义和一个main函数，而这个main函数是针对MSP430的一个代码，刚好是用于读取实时的欧拉角代码，首先我们对main函数进行分析，我们知道一个main函数一般分为两部分，各个内外设的初始化，和while循环内的无限操作函数，对于我们的要求也是也一样，我们无非也就是需要两个代码，mpu6050的初始化，以及无限的读出欧拉角，刚好和main函数对应上了，所以开始分析代码，因为main上自带英文注释，咱先把它通过谷歌翻译
2）main文件分析
初始化：{
- 初始化MSP430芯片
- 初始化mpu6050
- 检测返回值，如果返回值>0,重启MSP430
- 开启陀螺仪和加速计
- 加载mpu6050内部的陀螺仪和加速计的FIFO-根据内部参数配置
- 设置mpu6050内部采样率
- 返回刚刚配置的内部的一系列的值
- 数组置数函数-c语言特有，将hal数组内的所有值都置为0
- Hal.sesors=ACCEL_ON | GYRO_ON，标志-加速计和陀螺仪都开了
- 上报四元数
- 加载DMP功能
- 将陀螺仪和加速度方向矩阵推入DMP
- 手势回调函数-和我们的意图无关手势回调函数-和我们的意图无关
- 安卓回调函数-和我们的意图无关
- 选择dmp要加载的功能-和这些传入参数的值有关
- 加载已选功能
- 设置FIFO缓存的速率
- 开启DMP
- hal.dmp_on = 1 标志-DMP开启
- 开启MPS430的中断
}
While(1){
1.USB相关函数-和上位机有关，和我们的意图无关-
2.获得时间戳=当时的时间，和我们的意图无关-
3.运动中断函数，和我们的意图无关-
4.控制MSP430芯片休眠函数，和我们的意图无关-
5.查询DMP开了没和是否有数据更新了(两个都要满足)-
6.向DMP的fifo读取并且计算四元数
7.再检测FIFO内是否还存在有剩余的数据-如果没有，数据更新标志位清零
8.返回四元数
}
3、翻译”inv_mpu.c”和“motion_driver_test.c“的函数大概内容
1、“inv_mpu.c”
static int set_int_enable(unsigned char enable) 模块中断使能函数
int mpu_reg_dump(void) 测试打印函数
int mpu_read_reg(unsigned char reg, unsigned char *data) 3.向芯片读寄存器值，除了MEMERY和FIFO
int mpu_init(void) MPU6050的初始化
int mpu_lp_accel_mode(unsigned char rate) 进入低功耗模式
int mpu_get_gyro_reg(short *data, unsigned long *timestamp) 获取新的原始陀螺仪数据
int mpu_get_accel_reg(short *data, unsigned long *timestamp获取新的原始加速度数据
int mpu_get_temperature(long *data, unsigned long *timestamp) 获取新的温度数据
int mpu_set_accel_bias(const long *accel_bias) 偏差配置函数
int mpu_reset_fifo(void) 重置FIFO函数
int mpu_get_gyro_fsr(unsigned short *fsr) 获得陀螺仪全尺寸范围函数
int mpu_set_gyro_fsr(unsigned short fsr) 设置陀螺仪全尺寸范围函数
int mpu_get_accel_fsr(unsigned char *fsr) 获得加速度全尺寸范围函数
int mpu_set_accel_fsr(unsigned char fsr) 配置加速度全尺寸范围函数
int mpu_get_lpf(unsigned short *lpf) .获得DLPF范围函数
int mpu_set_lpf(unsigned short lpf) 配置DLPF范围函数
int mpu_get_sample_rate(unsigned short *rate) 获得采样频率范围函数
int mpu_set_sample_rate(unsigned short rate) 配置采样频率范围函数
int mpu_get_compass_sample_rate(unsigned short *rate) 获得罗盘采样频率范围函数
int mpu_set_compass_sample_rate(unsigned short rate) 配置罗盘采样频率范围函数
int mpu_get_gyro_sens(float *sens) 获得陀螺仪灵敏度比例因子函数
int mpu_get_accel_sens(unsigned short *sens) 获得加速计灵敏度比例因子函数
int mpu_get_fifo_config(unsigned char *sensors) 获得开启的FIFO通道函数
int mpu_configure_fifo(unsigned char sensors) 配置开启FIFO通道函数
int mpu_get_power_state(unsigned char *power_on) 获得芯片工作状态
int mpu_set_sensors(unsigned char sensors) 配置传感器的时钟和工作状态函数
int mpu_get_int_status(short *status).获得中断状态函数
int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp,unsigned char *sensors, unsigned char *more) 获得FIFO数据函数
int mpu_read_fifo_stream(unsigned short length, unsigned char *data,unsigned char *more) 获得FIFO数据长度函数
int mpu_set_bypass(unsigned char bypass_on) 设置旁路模式函数
int mpu_set_int_level(unsigned char active_low) 设置中断优先级函数
int mpu_set_int_latched(unsigned char enable) 设置中断锁存函数-
设置自检函数
static int get_st_biases(long *gyro, long *accel, unsigned char hw_test) 获取所有的偏差值函数
int mpu_run_self_test(long *gyro, long *accel) 行自检值函数
int mpu_write_mem(unsigned short mem_addr, unsigned short length,unsigned char *data) 向DMP写记忆函数
int mpu_read_mem(unsigned short mem_addr, unsigned short length,unsigned char *data) 向DMP读记忆函数
int mpu_load_firmware(unsigned short length, const unsigned char *firmware,unsigned short start_addr, unsigned short sample_rate) 加载并验证DMP映像函数
int mpu_set_dmp_state(unsigned char enable) DMP状态控制函数
int mpu_get_dmp_state(unsigned char *enabled) DMP状态读取函数
2、“motion_driver_test.c“
static __inline unsigned short inv_row_2_scale(const signed char row) 矩阵转换方向标量函数
static void setup_gyro(void) 陀螺仪配置函数
void STM32F103_Reset(void) 外部控制芯片复位函数，我们是stm32，所以重写
static __inline void run_self_test(void) 自检函数，用于坐标原点标定
static void gyro_data_ready_cb(void) 数据获得后的状态回调函数
4、”inv_mpu.c”和“inv_mpu_dmp_motion_driver.c“的文件格式的修改
5、根据“motion_driver_test.c“创建兼容STM32F10X的代码
1.DMP初始化函数:
u8 DMP_Init(void)
{
int result;
unsigned char accel_fsr;
unsigned short gyro_rate, gyro_fsr;
unsigned long timestamp;
result = mpu_init(); //1.6050初始化,成功=0，失败=1
if (result)
{
STM32F103_Reset(); //2.如果失败，重新复位stm32
return 8;
}
if(mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //3.配置陀螺仪和加速计传感器的时钟和工作状态函数
{return 1;}
if(mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //4.配置陀螺仪和加速计开启FIFO通道函数
{return 2;}
if(mpu_set_sample_rate(DEFAULT_MPU_HZ)) //5.配置默认的采样率
{return 3;}
mpu_get_sample_rate(&gyro_rate); //6.获得陀螺仪采样频率范围函数
mpu_get_gyro_fsr(&gyro_fsr); //7.获得陀螺仪全尺寸范围函数
mpu_get_accel_fsr(&accel_fsr); //8.获得加速计全尺寸范围函数
memset(&hal, 0, sizeof(hal)); //9.数组填数函数
hal.sensors = ACCEL_ON | GYRO_ON; //10.标志位-"开启传感器"设置为加速计和陀螺仪
if(dmp_load_motion_driver_firmware()) //11.加载并验证DMP映像函数
{return 4;}
if(dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //12.推送陀螺仪和加速度计的方向矩阵到DMP
{return 5;}
hal.dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL; //13.DMP的功能选项标志位设置，用来告诉DMP要开启的功能
if(dmp_enable_feature(hal.dmp_features)) //14.使能上述功能
{return 6;}
if(dmp_set_fifo_rate(DEFAULT_MPU_HZ)) //15.配置DMP的FIFO速率
{return 7;}
mpu_set_dmp_state(1); //16.开启DMP
hal.dmp_on = 1; //17.标志位-"DMP状态"为开启
run_self_test(); //18.DMP的自检，就是标定现在的状态为坐标原点
return 0;
}
2.DMP_欧拉角读取
short gyro[3], accel[3], sensors;
unsigned char more;
long quat[4];
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
float pitch,roll,yaw;
u8 DMP_DATA_UPDATA(void)
{
unsigned long sensor_timestamp;
gyro_data_ready_cb(); //数据采集结束标志位
if (hal.new_gyro && hal.dmp_on) //用于计算四元数函数
{
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))
{return 1;}
if (!more)
hal.new_gyro = 0;
if(sensors & INV_WXYZ_QUAT)
{
q0=quat[0]/q30;
q1=quat[1]/q30;
q2=quat[2]/q30;
q3=quat[3]/q30;
pitch=asin((-2)*q1*q3+2*q0*q2)*57.3;
roll=atan2(2*q2*q3+2*q0*q1,(-2)*q1*q1-2*q2*q2+1)*57.3;
yaw=atan2(2*(q1*q2+q0*q3),q0*q0+q1*q1-q2*q2-q3*q3)*57.3;
}
}
return 0;
}
6、注意事项：
模拟i2c的内容:
附件内有模拟I2C的代码，因为i2c内的延迟函数是通过while（time–）实现的，所以延迟长度可以通过time的值控制，为什么要考虑这个呢，因为在测试的时候发现单片机会 在“u8 DMP_Init(void) “函数中的 if(dmp_load_motion_driver_firmware()) /11.加载并验证DMP映像函数等待要很久，而这个函数的本质就是不断的通过I2C向6050收发数据，而我当时的I2C代码因为怕时钟线跳变时有延迟，所以给他每次跳变都加了很大的延迟，所以导致对于这个6050的这个函数也会延迟恒久，但是当我把软件延迟的变量变小，发现一下这个函数就通过了
关于mpu6050返回四元数的途径:
我们知道获取外部的数据有两种方式，一种是单片机一直通过询问标志位的方式去获得外部数据，另外一种是外部给单片机引脚一个电平跳变方式，来告诉单片机读取数据，也就是外部中断，而且第二中种效率高，但是对于6050，如果设置的dmp，他的默认中断触发是50us的高电平(应该是的)，us对于单片机来说可能会出bug，所以选择巡查的方式来读出数据，所以在测试中，我用的是滴答定时器在固定的时间间隔来读取数据，因为6050产出的数据速度远远大于每次单片机去读取的时间，所以不用担心会读出空数据，但是，会有一个问题，就是如果单片机的读取时间过慢，比如说1s读一次，那么就会死机，这个好像是因为fifo的会阻塞问题把。具体的不清楚，
编译结束时，warning和error都要关注，有时候warning也会让程序出bug
static _inline 关键字无法识别 要改成static __inline
mpu6050的功能报错，因为它没有 accel_cfg2，lp_accel_odr，accel_intel 这三个功能，所以在inv_mpu.c内的struct gyro_reg_s 中把它注释掉
硬件库的移植是一个极其痛苦的过程，稍有不慎就是一大堆报错，还有可能移植不成功。所以推荐直接用正点原子给的MPU6050的例程来改吧，它把移植都已经做好了，我们要做的就是再把这个移植到自己的工程，然后参考它的主函数写自己的程序，当然主控必须是STM32F10系列，不然只能自己哭着移植官方库了。另外需要注意的是，硬件库中有一段代码是在开机时令MPU6050进行自检，用于判断芯片是否损坏以及设定原点，此时需要把MPU6050正面向上水平放置，这样才能通过自检，不然会报错的。MPU6050硬件DMP库会把每次开机时的MPU状态视作原点，想要修改这一现象需要把“inv.mpu.c”文件中的“run_self_test”函数改成如下
u8 run_self_test(void)
{
int result;
//char test_packet[4] = {0};
long gyro[3], accel[3];
result = mpu_run_self_test(gyro, accel);
if (result == 0x3)
{
/* Test passed. We can trust the gyro data here, so let's push it down
* to the DMP.
*/
float sens;
unsigned short accel_sens;
mpu_get_gyro_sens(&sens);
gyro[0] = (long)(gyro[0] * sens);
gyro[1] = (long)(gyro[1] * sens);
gyro[2] = (long)(gyro[2] * sens);
dmp_set_gyro_bias(gyro);
mpu_get_accel_sens(&accel_sens);
accel_sens = 0；//添加词句完成修改，MPU不自动设定原点
accel[0] *= accel_sens;
accel[1] *= accel_sens;
accel[2] *= accel_sens;
dmp_set_accel_bias(accel);
return 0;
}else return 1;
}
软件DMP
网上一直流行着一种软件DMP算法，具体内容放下面了，具体为啥这样做我是没看懂，代码倒是试了一下，也是没感觉好用，甚至感觉我得出的数据是错的 … …，具体为啥没时间去找了，先把内容贴出来吧，以后有机会再尝试下。
先来看看如何用欧拉角描述一次平面旋转(坐标变换)：
MPU6050的四元数解算姿态方法
设坐标系绕旋转α角后得到坐标系，在空间中有一个矢量在坐标系中的投影为，在内的投影为由于旋转绕进行，所以Z坐标未变，即有。
转换成矩阵形式表示为：
整理一下
所以从旋转到可以写成上面仅仅是绕一根轴的旋转，如果三维空间中的欧拉角旋转要转三次:
上面得到了一个表示旋转的方向余弦矩阵。
不过要想用欧拉角解算姿态，其实我们套用欧拉角微分方程就行了：
上式中左侧，是本次更新后的欧拉角，对应row、pit、yaw。右侧，是上个周期测算出来的角度，三个角速度由直接安装在四轴飞行器的三轴陀螺仪在这个周期转动的角度，单位为弧度，计算间隔时T陀螺角速度，比如0.02秒0.01弧度/秒=0.0002弧度。间因此求解这个微分方程就能解算出当前的欧拉角。
前面介绍了什么是欧拉角，而且欧拉角微分方程解算姿态关系简单明了，概念直观容易理解，那么我们为什么不用欧拉角来表示旋转而要引入四元数呢？
一方面是因为欧拉角微分方程中包含了大量的三角运算，这给实时解算带来了一定的困难。而且当俯仰角为90度时方程式会出现神奇的“GimbalLock”。所以欧拉角方法只适用于水平姿态变化不大的情况，而不适用于全姿态飞行器的姿态确定。
四元数法只求解四个未知量的线性微分方程组，计算量小，易于操作，是比较实用的工程方法。
我们知道在平面(x,y)中的旋转可以用复数来表示，同样的三维中的旋转可以用单位四元数来描述。我们来定义一个四元数：
我们可以把它写成,其中,。那么是矢量，表示三维空间中的旋转轴。w是标量，表示旋转角度。那么就是绕轴旋转w度，所以一个四元数可以表示一个完整的旋转。只有单位四元数才可以表示旋转，至于为什么，因为这就是四元数表示旋转的约束条件。
而刚才用欧拉角描述的方向余弦矩阵用四元数描述则为：
所以在软件解算中，我们要首先把加速度计采集到的值(三维向量)转化为单位向量，即向量除以模，传入参数是陀螺仪x、y、z值和加速度计x、y、z值：
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
norm = sqrt(axax + ayay + azaz);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
下面把四元数换算成方向余弦中的第三行的三个元素。刚好vx、vy、vz 。其实就是上一次的欧拉角（四元数）的机体坐标参考系换算出来的重力的单位向量。
vx = 2(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
axyz是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向量。axyz是测量得到的重力向量，vxyz是陀螺积分后的姿态来推算出的重力向量，它们都是机体坐标参照系上的重力向量。那它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，exyz就是两个重力向量的叉积。这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。（你可以自己拿东西想象一下）由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
ex=（ayvz-azvy）;
ey=（azvx-axvz）;
ez=（axvy-ayvx）;
用叉积误差来做PI修正陀螺零偏
exInt = exInt + exKi;
eyInt = eyInt + eyKi;
ezInt = ezInt + ezKi; // adjusted gyroscope measurements
gx = gx + Kpex + exInt;
gy = gy + Kpey + eyInt;
gz = gz + Kpez + ezInt;
四元数微分方程，其中T为测量周期，为陀螺仪角速度，以下都是已知量，这里使用了一阶龙哥库塔求解四元数微分方程：
q0 = q0 + (-q1gx - q2gy - q3gz)halfT;
q1 = q1 + (q0gx + q2gz - q3gy)halfT;
q2 = q2 + (q0gy - q1gz + q3gx)halfT;
q3 = q3 + (q0gz + q1gy - q2gx)halfT;
最后根据四元数方向余弦阵和欧拉角的转换关系，把四元数转换成欧拉角：
所以有：
ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2q2 - 2 * q3 q3 + 1)* 57.3; // yaw
ANGLE.Y= asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
ANGLE.X= atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
完整程序
//变量定义
#define Kp1 00.0f//比例增益支配率收敛到加速度计/磁强计
#define Ki 0.002f//积分增益支配率的陀螺仪偏见的衔接
#define halfT 0.001f//采样周期的一半
float q0=1，q1=0，q2=0，q3=0;//四元数的元素，代表估计方向
float exInt=0，eyInt=0，ezInt=0;//按比例缩小积分误差
float Yaw，Pitch，Roll;//偏航角，俯仰角，翻滚角
voidIMUupdate（float gx，float gy，float gz，float ax，float ay，float az）
{
float norm;
float vx，vy，vz;
float ex，ey，ez;
//测量正常化
norm=sqrt（ax*ax+ay*ay+az*az）;
ax=ax/norm;//单位化
ay=ay/norm;
az=az/norm;
//估计方向的重力
vx=2*（q1*q3-q0*q2）;
vy=2*（q0*q1+q2*q3）;
vz=q0*q0-q1*q1-q2*q2+q3*q3;
//错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
ex=（ay*vz-az*vy）;
ey=（az*vx-ax*vz）;
ez=（ax*vy-ay*vx）;
//积分误差比例积分增益
exInt=exInt+ex*Ki;
eyInt=eyInt+ey*Ki;
ezInt=ezInt+ez*Ki;
//调整后的陀螺仪测量
gx=gx+Kp*ex+exInt;
gy=gy+Kp*ey+eyInt;
gz=gz+Kp*ez+ezInt;
//整合四元数率和正常化
q0=q0+（-q1*gx-q2*gy-q3*gz）*halfT;
q1=q1+（q0*gx+q2*gz-q3*gy）*halfT;
q2=q2+（q0*gy-q1*gz+q3*gx）*halfT;
q3=q3+（q0*gz+q1*gy-q2*gx）*halfT;
//正常化四元
norm=sqrt（q0*q0+q1*q1+q2*q2+q3*q3）;
q0=q0/norm;
q1=q1/norm;
q2=q2/norm;
q3=q3/norm;
Pitch=asin（-2*q1*q3+2*q0*q2）*57.3;//pitch，转换为度数
Roll=atan2（2*q2*q3+2*q0*q1，-2*q1*q1-2*q2*q2+1）*57.3;//rollv
Yaw=atan2（2*（q1*q2+q0*q3），q0*q0+q1*q1-q2*q2-q3*q3）*57.3;//Yaw
}