为什么我的ROOL跟PATCH只能在180度范围?????

只看该作者 · 2013-11-3 17:40

我根据四元数解算ROOL和PATCH,但是角度只在180度范围内,还有YAW只是瞬时角度,动一下,就变回0了!!
求大神解救!!!
下面附上代码:
/*
四元数---------------------------------------------------------------------------------
*/
void IMUupdate(float axi, float ayi, float gzi, float gxi, float gyi, float azi) {
      float norm;
      float vx, vy, vz;
      float ex, ey, ez;
            float q0 = 1, q1 = 0, q2 = 0, q3 = 0;       // 四元数的元素，代表估计方向
            float exInt = 0, eyInt = 0, ezInt = 0;       // 按比例缩小积分误差
            float gx=0,gy=0,gz=0,ax=0,ay=0,az=0;       //全局变量
//                float t11,t12,t13,t21,t22,t23,t31,t32,t33;

      ax=ax*kfa+kfan*axi;
      ay=ay*kfa+kfan*ayi;
      az=az*kfa+kfan*azi;

      gx=gx*kfg+kfgn*gxi;
      gy=gy*kfg+kfgn*gyi;
      gz=gz*kfg+kfgn*gzi;

      // 测量正常化
      norm = sqrtf(ax*ax + ay*ay + az*az);
      ax = ax / norm;
      ay = ay / norm;
      az = az / norm;

      // 估计方向的重力
      vx = 2*(q1*q3 - q0*q2);
      vy = 2*(q0*q1 + q2*q3);
      vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

      // 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
      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)*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;

      // 正常化四元
      norm = sqrtf(q0*q0 + q1*q1 + q2*q2 + q3*q3);
      q0 = q0 / norm;
      q1 = q1 / norm;
      q2 = q2 / norm;
      q3 = q3 / norm;

         Pitch  =  asin(-2 * q1 * q3 + 2 * q0* q2)*57.3; // 俯仰
      Rool = -atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)*57.3; // 翻滚
            Yaw = -atan2(2 * (q1*q2+q0*q3),q0*q0+q1*q1-q2*q2-q3*q3)*10;       //偏航
}