大家对四轴的YAW是怎么处理的

镖叔 · 发表于 2015-1-22 18:28:47

我看很多资料多数是四元数处理。例子如下
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
//int16_t Xr,Yr;
float vx, vy, vz;// wx, wy, wz;
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;

if(ax*ay*az==0)
return;

norm = Q_rsqrt(ax*ax + ay*ay + az*az); //acc?????é????
ax = ax *norm;
ay = ay * norm;
az = az * norm;

// estimated direction of gravity and flux (v and w) ????????·??ò???÷??/±??¨
vx = 2*(q1q3 - q0q2); //????????xyz??±í??
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;

// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) ; //?ò?????????à????????·??????ó??
ey = (az*vx - ax*vz) ;
ez = (ax*vy - ay*vx) ;

exInt = exInt + VariableParameter(ex) * ex * Ki; //???ó????????·?
eyInt = eyInt + VariableParameter(ey) * ey * Ki;
ezInt = ezInt + VariableParameter(ez) * ez * Ki;
// adjusted gyroscope measurements

gx = gx + Kp * VariableParameter(ex) * ex + exInt;
gy = gy + Kp * VariableParameter(ey) * ey + eyInt;
gz = gz + Kp * VariableParameter(ez) * ez + ezInt;

// integrate quaternion rate and normalise //??????????·?·???
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

// normalise quaternion
norm = Q_rsqrt(q0q0 + q1q1 + q2q2 + q3q3);
q0 = q0 * norm;
q1 = q1 * norm;
q2 = q2 * norm;
q3 = q3 * norm;

qa0 = q0;
qa1 = q1;
qa2 = q2;
qa3 = q3;

angle.roll = atan2(2*q2q3 + 2*q0q1, -2*q1q1 - 2*q2q2 + 1); // roll
angle.pitch = asin(-2*q1q3 + 2*q0q2); // pitch

/* ???????????????????????? */
/*???? http://baike.baidu.com/view/1239157.htm?fr=aladdin */

//Xr = X_HMC * COS(angle.pitch/AtR) + Y_HMC * SIN(-angle.pitch/AtR) * SIN(-angle.roll/AtR) - Z_HMC * COS(angle.roll/AtR) * SIN(-angle.pitch/AtR);
//Yr = Y_HMC * COS(angle.roll/AtR) + Z_HMC * SIN(-angle.roll/AtR);

//angle.yaw = atan2((double)Yr,(double)Xr) * RtA; // yaw
angle.yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * RtA;
angle.roll *= RtA;
angle.pitch *= RtA;

}

但是我发现YAW角度一只在漂移，平均每5秒偏向0.1度左右，我查过很多资料说是MPU6050的一个硬件缺陷，应该用地磁来补偿误差例如HWM5883l,大家的看法的看法是怎么样的？