本帖最后由 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度，我用这个方法偏差同等条件下一两度了，但是速度方面就不是很清楚了，恳请各位大侠指导一下。
长期融合使用的是物理实验用的线性回归方程近似 |
楼主
标题置顶
标题高亮
