请问MPU9250姿态融合但航向角不正确该怎么办

我取了MPU9250的陀螺仪、加速度和磁力计数据，采用互补滤波算法进行数据融合，俯仰角和翻滚角的数据正确，但是俯仰角数据不正确，基本上没有变动。在绕Z轴转动时，磁力计数据全是正的值，数据融合代码如下。还望有大神指教！！！
float recipNorm;
float vx, vy, vz , wx, wy, wz;
float ex, ey, ez;
float qa, qb, qc;
float hx, hy, hz, bx,by, bz;
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))   
{
  recipNorm = invSqrt(ax * ax + ay * ay + az * az);
  ax *= recipNorm;
  ay *= recipNorm;
  az *= recipNorm;        
    recipNorm = invSqrt(mx * mx + my * my + mz * mz);         
    mx = mx * recipNorm;
    my = my * recipNorm;
    mz = mz * recipNorm;
    hx = 2*mx*(0.5f - q2*q2 - q3*q3) + 2*my*(q1*q2 - q0*q3) + 2*mz*(q1*q3 + q0*q2);
    hy = 2*mx*(q1*q2 + q0*q3) + 2*my*(0.5f - q1*q1 - q3*q3) + 2*mz*(q2*q3 - q0*q1);
    hz = 2*mx*(q1*q3 - q0*q2) + 2*my*(q2*q3 + q0*q1) + 2*mz*(0.5f - q1*q1 - q2*q2);
  
    bx = sqrt((hx*hx) + (hy*hy));
    bz = hz;     
  
    vx = 2*(q1*q3 - q0*q2);
    vy = 2*(q0*q1 + q2*q3);
    vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
    wx = 2*bx*(0.5 - q2*q2 - q3*q3) + 2*bz*(q1*q3 - q0*q2);
    wy = 2*bx*(q1*q2 - q0*q3) + 2*bz*(q0*q1 + q2*q3);
    wz = 2*bx*(q0*q2 + q1*q3) + 2*bz*(0.5 - q1*q1 - q2*q2);  
  
    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);  
  if(twoKi > 0.0f) {                              
   integralFBx += twoKi * ex * sample_period;   
   integralFBy += twoKi * ey * sample_period;
   integralFBz += twoKi * ez * sample_period;
   gx += integralFBx;
   gy += integralFBy;
   gz += integralFBz;
  }
  else {
   integralFBx = 0.0f;
   integralFBy = 0.0f;
   integralFBz = 0.0f;
  }
  gx += twoKp * ex;  
  gy += twoKp * ey;
  gz += twoKp * ez;
}
gx *= (0.5f * sample_period);  
gy *= (0.5f * sample_period);
gz *= (0.5f * sample_period);
qa = q0;
qb = q1;
qc = q2;                          
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
Eular_X = atan2(2.0f*(q2*q3 + q0*q1),1.0f - 2.0f*(q1*q1 + q2*q2));
Eular_Y = asin(-2.0f*(q1*q3 - q0*q2));
Eular_Z = atan2(2.0f*(q1*q2 + q0*q3),1.0f - 2.0f*(q2*q2 + q3*q3));