void init_Q(void)
{////////pitch  roll  yaw
q0=cos(roll/2)*cos(pitch/2)*cos(yaw/2)+sin(roll/2)*sin(yaw/2)*sin(yaw/2);

}
void angle()  //姿态矩阵
{
float DX,DX1,DX2,DX3,DY,DY1,DY2,DY3,DZ,DZ1,DZ2,DZ3;
float model,h0,h1,h2,h3;  //h表示增量四元数
float q0,q1,q2,q3;          //q表示更新前的四元数
float model_d,d0,d1,d2,d3;   //d表示更新后的四元数
float T11,T12,T13,T21,T22,T23,T31,T32,T33;

//三子样采样优化 D表示角增量
DX=DX1+DX2+DX3+(9/20)*(DY1*DZ3+DZ1*DY3)+(27/40)*(DY2*(DZ3-DZ1)+DX2*(DX3-DX1));

DY=DY1+DY2+DY3+(9/20)*(DY1*DY3+DX1*DZ3)+(27/40)*(DZ2*(DX3-DX1)+DY2*(DY3-DY1));

DZ=DZ1+DZ2+DZ3+(9/20)*(DZ1*DZ3+DY1*DX3)+(27/40)*(DX2*(DY3-DY1)+DY2*(DZ3-DZ1));

//计算增量四元数h
model=sqrt(DX*DX+DY*DY+DZ*DZ);
if(model==0)
{
  h0=1;  
  h1=0;
  h2=0;
  h3=0;
}
else
{
  h0=cos(model/2);
  h1=(DX/model)*sin(model/2);
  h2=(DY/model)*sin(model/2);    
  h3=(DZ/model)*sin(model/2);    
}

//四元数更新
d0=q0*h0-q1*h1-q2*h2-q3*h3;
///////////////////////////
d1=q1*h0+q0*h1-q3*h2+q2*h3;      
d2=q2*h0+q3*h1+q0*h2-q1*h3;    
d3=q3*h0-q2*h1+q1*h2+q0*h3;
//进行规范化
model_d=sqrt(d0*d0+d1*d1+d2*d2+d3*d3);
d0=d0/model_d;
d1=d1/model_d;
d2=d2/model_d;
d3=d3/model_d;
//将更新和规范化之后的四元数d赋给q,以便下一次的更新
q0=d0;
q1=d1;
q2=d2;
q3=d3;

//将四元数转化成方向余弦矩阵
T11=q0*q0+q1*q1-q2*q2-q3*q3;
T12=2*(q1*q2-q0*q3);
T13=2*(q1*q3+q0*q2);

T21=2*(q1*q2+q0*q3);
T22=q0*q0-q1*q1+q2*q2-q3*q3;
T23=2*(q2*q3-q0*q1);

T31=2*(q1*q3-q0*q2);
T32=2*(q2*q3+q0*q1);
T33=q0*q0-q1*q1-q2*q2+q3*q3;

//提取姿态角
pitch=asin(T32);
if(T22==0&&T12>0)
yaw=pi/2;
if(T22==0&&T12<0)
yaw=-pi/2;
if(T22>0&&T12>0)
yaw=atan(-T31/T33);
if(T22>0&&T12<0)
yaw=atan(-T31/T33);
if(T22<0&&T12>0)
yaw=atan(-T31/T33)+2*pi;
if(T22<0&&T12<0)
yaw=atan(-T31/T33)-2*pi;
}