MPU9250姿态融合，航向角不正确基本没有变动

zhaidandan · 发表于 2018-7-31 09:57:53

我取了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));