四元数解算姿态角问题

读出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之间跳变 加速度有几万 请求大佬能指点一下 是不是哪里有问题啊