四元数解算的姿态不稳定

使用四元数解算姿态时遇到这两个问题：1.不加入磁力计时，初始时解算出的姿态角挺稳定的，都是0，不过偏航角会漂移，后来加了磁力计，数据倒是不飘了，就是初始时解算出来的姿态角会衰减振荡，经过十几秒之后才能稳定（放置的初始偏航角很小时，振荡时间很短，但是放置存在一定偏航角时，振荡时间长），想问下怎么解决这个振荡问题；2.加入磁力计，当放置的偏航角比较小的时候，解算出来滚转俯仰基本为0，正常，当放置偏航角为正负160这么大时，计算的滚转和俯仰角大约10左右，应该是0的呀；
有人说是由于把磁力计融入到了roll和pitch,下面是我使用的解算过程，大家帮忙看下问题出在哪里：
CPU_INT08U Att_Solution (float *angle_output,float gx, float gy, float gz,
                                    float ax, float ay, float az, float mx, float my, float mz)
{   
        float  norm;
        float  hx, hy, hz, bx, bz;
        float  wx, wy, wz;
        float  vx, vy, vz;
        float  ex, ey, ez;
        
        float  q0q0 = q0*q0;
        float  q0q1 = q0*q1;
        float  q0q2 = q0*q2;
        float  q0q3 = q0*q3;
        float  q1q1 = q1*q1;
        float  q1q2 = q1*q2;
        float  q1q3 = q1*q3;
        float  q2q2 = q2*q2;
        float  q2q3 = q2*q3;
        float  q3q3 = q3*q3;
        /* 对加速度在体轴系下归一化 */
        norm = sqrt(ax*ax + ay*ay + az*az);
        if(norm == 0)        return 0;            
        ax = ax / norm;
        ay = ay / norm;
        az = az / norm;
        norm = sqrt(mx*mx + my*my + mz*mz);   
        if(norm == 0)        return 0;   
        mx = mx / norm;
        my = my / norm;
        mz = mz / norm;
        /* compute reference direction of magnetic field */
        hx = 2*mx*(0.5f-q2q2-q3q3)+2*my*(q1q2-q0q3)+2*mz*(q1q3+q0q2);
        hy = 2*mx*(q2q2+q3q3)+2*my*(0.5f-q1q1-q3q3)+2*mz*(q2q3-q0q1);
        hz = 2*mx*(q1q3-q0q2)+2*my*(q2q3+q0q1)+2*mz*(0.5f-q1q1-q2q2);
        bx = sqrt((hx*hx)+(hy*hy));
        bz = hz;
        /* 将地理坐标系下重力加速度旋转到体轴系下 */     
        vx = 2*(q1q3 - q0q2);
        vy = 2*(q0q1 + q2q3);
        vz = q0q0 - q1q1 - q2q2 + q3q3 ;   
        wx = 2*bx*(0.5f-q2q2-q3q3)+2*bz*(q1q3-q0q2);
        wy = 2*bx*(q1q2-q0q3)+2*bz*(q0q1+q2q3);
        wz = 2*bx*(q0q2+q1q3)+2*bz*(0.5f-q1q1-q2q2);
        /* 叉乘求取误差分量 */
        ex = (ay*vz - az*vy) + (my*wz - mz*wy);                                               
        ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
        ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
//        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;
        /* 一阶龙格库塔法更新四元数 */
        q0 = q0 + (-q1*gx - q2*gy - q3*gz)*0.002f/2.0f;
        q1 = q1 + (q0*gx + q2*gz - q3*gy)*0.002f/2.0f;
        q2 = q2 + (q0*gy - q1*gz + q3*gx)*0.002f/2.0f;
        q3 = q3 + (q0*gz + q1*gy - q2*gx)*0.002f/2.0f;
        /* 四元数归一化 */
        norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 = q0 / norm;
        q1 = q1 / norm;
        q2 = q2 / norm;
        q3 = q3 / norm;
        /* 四元数转化为欧拉角 */
        angle_output[0] = atan2(2.0f * (q2q3 + q0q1), q0q0-q1q1-q2q2+q3q3) * 57.3f;
        angle_output[1] = asin(2.0f * (q0q2 - q1q3)) * 57.3f;
        angle_output[2] = atan2(2.0f * (q1q2 + q0q3), q0q0+q1q1-q2q2-q3q3) * 57.3f;
        
        return 1;
}