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;
}
大家来看看这个算法,还有哪些问题,我觉得在姿态提取处是不是这样处理是不合理的