我自己写了一个姿态更新算法，大侠们能帮我看看吗

只看该作者 · 2015-4-24 18:14

本帖最后由 superstarzhu 于 2015-5-4 15:35 编辑

h ttp://v.youku.com/v_show/id_XOTQ3MTgyODc2.html 先放个视频，用这个算法升空的（地磁还没弄，弄完我再发，现在没加地磁也基本buhuitaizhuan，还是有点扭来扭去，我还没买遥控用电脑串口控制的所以最后摔了（为什么连网址都不给我发，又不能发视频）

这是直接在百度百科上搜的线性回归方程的算法，我就是用这条式子算的，对照下面很容易就能看出我在算什么
typedef struct fp_vector_tag
{
float x,y,z;
}fp_vector;

float invSqrt(float x)
{
float xhalf = 0.5f * x;
int i = *(int*)&x;       // get bits for floating value
i =  0x5f375a86 - (i>>1); // gives initial guess
x = *(float*)&i;          // convert bits back to float
x = x * (1.5f - xhalf*x*x); // Newton step
return x;
}

void rotateS(fp_vector *v, float deltaRoll, float deltaPitch, float deltaYaw)  //这里可以用小角度近似取代这个算法
{
fp_vector v_tmp = *v;

// This does a  "proper" matrix rotation using gyro deltas without small-angle approximation
float mat[3][3];
float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, sinzcosx, coszsinx, sinzsinx;


cosx = cosf(deltaRoll);
sinx = sinf(deltaRoll);
cosy = cosf(deltaPitch);
siny = sinf(deltaPitch);
cosz = cosf(deltaYaw);
sinz = sinf(deltaYaw);

coszcosx = cosz * cosx;
sinzcosx = sinz * cosx;
coszsinx = sinx * cosz;
sinzsinx = sinx * sinz;

mat[0][0] = cosz * cosy;
mat[0][1] = -cosy * sinz;
mat[0][2] = siny;
mat[1][0] = sinzcosx + (coszsinx * siny);
mat[1][1] = coszcosx - (sinzsinx * siny);
mat[1][2] = -sinx * cosy;
mat[2][0] = (sinzsinx) - (coszcosx * siny);
mat[2][1] = (coszsinx) + (sinzcosx * siny);
mat[2][2] = cosy * cosx;

v->x = v_tmp.x * mat[0][0] + v_tmp.y * mat[1][0] + v_tmp.z * mat[2][0];
v->y = v_tmp.x * mat[0][1] + v_tmp.y * mat[1][1] + v_tmp.z * mat[2][1];
v->z = v_tmp.x * mat[0][2] + v_tmp.y * mat[1][2] + v_tmp.z * mat[2][2];
}

#define T 0.001f  //姿态更新率1000hz
#define Gravity_LTC_Time 200 //Gravity long term correction time  //这个是指定执行多少次更新算法时执行一次长期融合，理论上近似线性区域内越长效果越好，但是太长下面的1方+到n方就会爆了，各位大虾们看看有木有什么优化的方法
volatile float Gravity_EC_FACTOR=0.0003f;  //Error correction factor  //加速度计短期修正陀螺仪的偏差系数，直接设置成0都可以
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
      static fp_vector old_gyro_data;
      static fp_vector gravity_vec={0.0f,0.0f,-1.0f}; //vector that indicates the gravity
      fp_vector error_vec;
      float norm;

      static unsigned int IMU_times=0; //
      static int accel_LT_x=0,accel_LT_y=0,accel_LT_z=0;
      static int accel_LT_tx=0,accel_LT_ty=0,accel_LT_tz=0;
      static const float av_t=(Gravity_LTC_Time-1)/2.0f;
      static const float nt=Gravity_LTC_Time*av_t;
      static const float x_divede = Gravity_LTC_Time * (1.0f+Gravity_LTC_Time) * (2.0f*Gravity_LTC_Time+1.0f) / 6.0f - Gravity_LTC_Time*av_t*av_t;
      float LT_x,LT_y,LT_z;
      float av_x,av_y,av_z;

      accel_LT_x+=ax; accel_LT_y+=ay; accel_LT_z+=az;
      accel_LT_tx+=IMU_times*ax;
      accel_LT_ty+=IMU_times*ay;
      accel_LT_tz+=IMU_times*az;

      norm=invSqrt(ax*ax+ay*ay+az*az);
      ax*=norm;
      ay*=norm;
      az*=norm;

      if(++IMU_times<Gravity_LTC_Time) //短期融合
      {
            // error_vec.x = ( gravity_vec.y * az - gravity_vec.z * ay ) - old_gyro_data.x;
            //error_vec.y = ( gravity_vec.z * ax - gravity_vec.x * az ) - old_gyro_data.y;
            //error_vec.z = ( gravity_vec.x * ay - gravity_vec.y * ax ) - old_gyro_data.z;//这些其实不要也可以，有了长期融合短期内就可以完全相信陀螺仪的积分

            //old_gyro_data.x += Gravity_EC_FACTOR*error_vec.x;
            // old_gyro_data.y += Gravity_EC_FACTOR*error_vec.y;
            // old_gyro_data.z += Gravity_EC_FACTOR*error_vec.z;
            rotateS( &gravity_vec , old_gyro_data.x , old_gyro_data.y , 0.0f );
      }
      else //长期融合
      {
            IMU_times=0;
            //长期融合使用的是线性回归方程的办法，下面的运算都是把方程算出来
            av_x=((float)accel_LT_x)/Gravity_LTC_Time;
            av_y=((float)accel_LT_y)/Gravity_LTC_Time;
            av_z=((float)accel_LT_z)/Gravity_LTC_Time;

            LT_x=(accel_LT_tx-nt*av_x)/x_divede;
            LT_y=(accel_LT_ty-nt*av_y)/x_divede;
            LT_z=(accel_LT_tz-nt*av_z)/x_divede;

            LT_x=LT_x*(Gravity_LTC_Time-1-av_t)+av_x;
            LT_y=LT_y*(Gravity_LTC_Time-1-av_t)+av_y;
            LT_z=LT_z*(Gravity_LTC_Time-1-av_t)+av_z;
            //到这里之前方程已经算出来了，接下来就是把方程的最后一点的坐标算出来，并把它认为是完全可信的姿态
            norm=invSqrt(LT_x*LT_x+LT_y*LT_y+LT_z*LT_z);
            gravity_vec.x=LT_x*norm;
            gravity_vec.y=LT_y*norm;
            gravity_vec.z=LT_z*norm;

            accel_LT_x=accel_LT_y=accel_LT_z=0;
            accel_LT_tx=accel_LT_ty=accel_LT_tz=0;
      }
            Yaw+=old_gyro_data.z;
      Pitch=atan2f(gravity_vec.x,gravity_vec.z);
      Roll=atan2f(gravity_vec.y,gravity_vec.z);

      old_gyro_data.x=gx*T;
      old_gyro_data.y=gy*T;
      old_gyro_data.z=gz*T;

}

就是这些了，用旋转矩阵做的，我用的新西达电机kv2200的震动比较大我用Mag那人的那个算法偏差会上10度，我用这个方法偏差同等条件下一两度了，但是速度方面就不是很清楚了，恳请各位大侠指导一下。
长期融合使用的是物理实验用的线性回归方程近似

只看该作者 · 2015-4-24 18:18

还没有写详细的注释大家见谅，短期融合每个算法的思路好像都差不多，长期融合我还没看过别人的。我这里的思路是在一段时间内获取一系列加速度的离散点，然后通过线性回归方程算出理论上这条直线的最后一点在哪里，然后将这点直接认为是准确姿态，当然也可以再判断姿态偏差再做旋转，不过我感觉降低速度而且没什么必要。大虾们给给意见哇，四元数我还没看懂。。。