求教有关四元数求解姿态问题！

851285391@qq.co

近来搞姿态发现这样的问题希望有大神解答下：
 gx-=gyo_offset[0];
 gy-=gyo_offset[1];
 gz-=gyo_offset[2];

 gx*=sampletime;
 gy*=sampletime;
 gz*=sampletime;

q0+=0.5*(-q1*gx-q2*gy-q3*gz);
q1+=0.5*(q0*gx+q2*gz-q3*gy);
q2+=0.5*(q0*gy-q1*gz+q3*gx);
q3+=0.5*(q0*gz+q1*gy-q2*gx);

recipNorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 * recipNorm;
q1 = q1 * recipNorm;
q2 = q2 * recipNorm;
q3 = q3 * recipNorm;

我参考网上广泛流传的微分四元数IMU程序，想单纯用陀螺仪来解算姿态，当静止时，gx、gy、gz减去偏差后输出值很小，四元数q0不应该变化很小在0.8~0.9的范围吗？但是q0变化很大，从-0.9~+0.9的变化。。。
在程序中读俯仰角q_1=2 * q1 * q3 - 2 * q0 * q2;静止时它是从-0.9~+0.9大范围变化，很不正常，这究竟怎么回事呢？

正点原子

帮顶.....