|
|
楼主 |
发表于 2015-1-29 21:59:58
|
显示全部楼层
本帖最后由 cw628 于 2015-1-29 22:01 编辑
我还没到估计速度这里，还在重力方向的加速度估计就有问题，我用的这个旋转方法：（*delta这个是姿态角（单位是弧度），*v这个是加速度计测到的3轴的值） ，v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];这个作为计算的输出的重力方向的加速度
void rotateV(struct fp_vector *v, float *delta)
{
struct 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(delta[ROLL]);
sinx = sinf(delta[ROLL]);
cosy = cosf(delta[PITCH]);
siny = sinf(delta[PITCH]);
cosz = cosf(delta[YAW]);
sinz = sinf(delta[YAW]);
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];
} |
|
发表于 2015-1-28 23:45:36
