互补滤波错误

言出必折

#define Kp 0.8f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.001f                       // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.005f                   // half the sample period采样周期的一半

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error




//定义系统的欧拉角
typedef struct
{
        float ROLL;
        float PITCH;
        float YAW;
}ANGLE;

ANGLE Q_ANGLE;




/*******************************************************************************
快速计算 1/Sqrt(x)，源自雷神3的一段代码，神奇的0x5f3759df！比正常的代码快4倍        
*******************************************************************************/
float invSqrt(float x)
{
        float halfx = 0.5f * x;
        float y = x;
        long i = *(long*)&y;
        i = 0x5f3759df - (i>>1);
        y = *(float*)&i;
        y = y * (1.5f - (halfx * y * y));
        return y;
}


void IMUupdate1(float gx, float gy, float gz, float ax, float ay, float 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;
       


        if(ax*ay*az==0)
                return;
       
  norm = invSqrt(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 ;
        OLED_ShowNum(0,50,q0q2,2,16);
  // 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 + ex * Ki;                                                                  //对误差进行积分
  eyInt = eyInt + ey * Ki;
  ezInt = ezInt + ez * Ki;

  // adjusted gyroscope measurements
  gx = gx + Kp*ex + exInt;                                                                                                   //将误差PI后补偿到陀螺仪，即补偿零点漂移
  gy = gy + Kp*ey + eyInt;
  //gz = gz + Kp*ez+ ezInt;                                                                                           //这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减
       
       
       
  //四元素的微分方程
  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 = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  q0 = q0 / norm;
  q1 = q1 / norm;
  q2 = q2 / norm;
  q3 = q3 / norm;
       
        //四元数转化成欧拉角
  //Q_ANGLE.YAW =  0.002 * MPU6050_GYRO_LAST.Z*Gyro_Gain;  // yaw GYRO_I.Z;  atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3+1)* 57.3;
        Q_ANGLE.YAW = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3+1)* 57.3;//yaw
        Q_ANGLE.PITCH = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
  Q_ANGLE.ROLL = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
       
//        *yaw=Q_ANGLE.YAW ;
//        *pitch=Q_ANGLE.PITCH;
//        *roll=Q_ANGLE.ROLL;
       
}
大神们看看我的互补哪错了

言出必折

感觉他的四元很快归零了，但没找到错了哪

flyleaf

帮顶，看到这些就头大