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