新人求问四元数欧拉角计算问题

FreeStyler

现在我能正确的读到MPU6050各寄存器的数据，也能将他们处理成正确的加速度，角速度真值，然后用这个函数，输入参数是三轴加速度，角速度真值，但是得到的欧拉角波形是这样的

，显然不对，请高手分析一下是什么原因？
void IMU_update(int gx, int gy, int gz, int ax, int ay, int az)//gx£¬gy£¬gzÊÇÈýÖá½ÇËٶȣ¬ax,ay,azÊÇÈýÖá¼ÓËÙ¶È
{
  float norm;
  float hx, hy, hz, bx, bz;
  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;

  norm = sqrt(ax*ax + ay*ay + az*az);           
  ax = ax / norm;
  ay = ay / norm;
  az = az / norm;
           
  vx = 2*(q1q3 - q0q2);                                                                               
  vy = 2*(q0q1 + q2q3);
  vz = q0q0 - q1q1 - q2q2 + q3q3 ;

  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 = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);               
  q0 = q0 / norm;
  q1 = q1 / norm;
  q2 = q2 / norm;
  q3 = q3 / norm;
       

Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3+1)* 57.3;//yaw
Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
  Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
       
}

fastchaser

p，i，T参数都调好了吗？

FreeStyler

fastchaser 发表于 2015-3-16 19:49
p，i，T参数都调好了吗？

halfT用的是6050姿态解算时间的一半（初始化的时候设置125HZ，时间是0.008s，halfT取的0.004），Kp和Ki与这个有关么？如果有关的话应该如何整定？麻烦了！

fastchaser

FreeStyler 发表于 2015-3-17 09:03
halfT用的是6050姿态解算时间的一半（初始化的时候设置125HZ，时间是0.008s，halfT取的0.004），Kp和Ki与 ...

看不懂你的图   不过这个姿态解算 算法需要注意的就是p，i参数，halfT周期参数，以及载体坐标系的坐标轴定义方向了  没其他问题。 p  i 调参 我也没想到好办法呢  等大神解答。