四元数解算姿态角问题

c627933142 · 发表于 2018-10-9 20:50:13

读出MPU9250加速度和角速度后就开始折腾互补滤波和姿态解算了看了资料搞了一段代码
#define Kp 1.6f
#define Ki 0.001f
#define halfT 0.0005f
#define T 0.001f
float q0=1,q1=0,q2=0,q3=0;
float exInt=0,eyInt=0,ezInt=0;
void IMU(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx,vy,vz;
float ex,ey,ez;

norm=1/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;

float q0_last=q0;
float q1_last=q1;
float q2_last=q2;
float q3_last=q3;

q0=q0_last+(-q1_last*gx-q2_last*gy-q3_last*gz)*halfT;
q1=q1_last+( q0_last*gx+q2_last*gz-q3_last*gy)*halfT;
q2=q2_last+( q0_last*gy-q1_last*gz+q3_last*gx)*halfT;
q3=q3_last+( q0_last*gz+q1_last*gy-q2_last*gx)*halfT;

norm=1/sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
q0=q0*norm;
q1=q1*norm;
q2=q2*norm;
q3=q3*norm;
}
void Get_Angle(struct _angle *Angle)
{
Angle->roll=atan2(2.0f*(q0*q1+q2*q3),-2*q1*q1-2*q2*q2+1)*Radian_to_Angle;
Angle->pitch=asin(2.0f*(q0*q2 - q1*q3))*Radian_to_Angle;
Angle->yaw=atan2(2*(q0*q1+q2*q3),q0*q0+q1*q1-q2*q2-q3*q3)*Radian_to_Angle;
}

输出的姿态角一直是横滚和偏航-135 俯仰角0 角速度-20到+20之间跳变加速度有几万请求大佬能指点一下是不是哪里有问题啊