下面整篇文章都是转载的。

算法输入：物体的初始姿态，3轴陀螺仪不同时刻的Yaw，Pitch，Roll的角速度；
算法输出：物体的当前姿态。
具体算法：
1. 初始姿态的四元数(w,x,y,z)=(1,0,0,0) 命名为A
2. 读取3轴陀螺仪当前时刻的Yaw,Pitch,Roll角速度，乘以上次计算以来的间隔时间，得到上一时刻以来(Yaw,Pitch,Roll)的变化量，命名为欧拉角b
3. b是Tait-Bryan angle定义的欧拉角，将其转为四元数B
4. A=A×B，做四元数乘法，即可得到当前姿态对应的新的四元数A
5.重复2～4部，即可连续更新姿态
6.将四元数A重新转换为Tait-Bryan angle形式的欧拉角a，就可以以直观的形式查看当前姿态
 

核心算法1，欧拉角转四元数
void Quaternion::FromEulerAngle(const EulerAngle &ea)
{
        float fCosHRoll = cos(ea.fRoll * .5f);
        float fSinHRoll = sin(ea.fRoll * .5f);
        float fCosHPitch = cos(ea.fPitch * .5f);
        float fSinHPitch = sin(ea.fPitch * .5f);
        float fCosHYaw = cos(ea.fYaw * .5f);
        float fSinHYaw = sin(ea.fYaw * .5f);
        w = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw;
        x = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw;
        y = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw;
        z = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw;
}

核心算法2，四元数转欧拉角

EulerAngle Quaternion::ToEulerAngle() const
{
        EulerAngle ea;

        ea.fRoll  = atan2(2 * (w * z + x * y) , 1 - 2 * (z * z + x * x));
        ea.fPitch = asin(CLAMP(2 * (w * x - y * z) , -1.0f , 1.0f));
        ea.fYaw   = atan2(2 * (w * y + z * x) , 1 - 2 * (x * x + y * y));
        return ea;
}

核心算法3，四元数乘法
Quaternion Quaternion::Multiply(const Quaternion &b)
{
        Quaternion c;
        c.w=w*b.w        -x*b.x        -y*b.y        -z*b.z;
        c.x=w*b.x        +x*b.w        +y*b.z        -z*b.y;
        c.y=w*b.y        -x*b.z        +y*b.w        +z*b.x;
        c.z=w*b.z        +x*b.y        -y*b.x        +z*b.w;
        c.Normalize();
        return c;
}

次要的规范化算法：
void  Quaternion::Normalize(){
        float s=getS();
        w/=s;
        x/=s;
        y/=s;
        z/=s;
}
float Quaternion::getS(){
        return sqrt(w*w+x*x+y*y+z*z);
}

我的loop函数，算法的集成部分：

Quaternion nowQ;
void loop() {
  int intx, inty,intz;
  float  pitch,roll,yaw;

  gyro.ReadGyroOutCalibrated_Radian(&pitch, &roll, &yaw);

  EulerAngle dt;
  dt.fRoll=roll;
  dt.fPitch=pitch;
  dt.fYaw=-yaw;

  Quaternion dQ;
  dQ.FromEulerAngle(dt);
  nowQ=nowQ.Multiply(dQ);


  count++;
  if (count>1000){
    EulerAngle nowG=nowQ.ToEulerAngle();

    Serial.print(nowG.fRoll/3.1415926535*180,11);//横滚
    Serial.print(",");
    Serial.print(nowG.fPitch/3.1415926535*180,11);//俯仰
    Serial.print(",");
    Serial.print(nowG.fYaw/3.1415926535*180,11);//偏航
    Serial.print(",");
    Serial.print(nowQ.getS(),11);//偏航
    Serial.println();
    count=0;
  }
}