// 载体坐标下的重力加速度常量，单位化。//用旋转矩阵将世界坐标系的单位化重力矢量(0,0,1）不是（0,0，-1）,mpu6050 只感应非重力加速度)转换到机载坐标系中。
//机载坐标下的重力矢量 旋转矩阵（坐标系转换矩阵的逆矩阵也就是转置矩阵，因为欧拉角解得的旋转矩阵必是正交阵） 世界坐标下的重力矢量
// x cos(T)cos(K) cos(T)sin(C) -sin(C) 0
// [ y ] = [ sin(F)sin(T)cos(K)-cos(F)sin(K) sin(F)sin(T)sin(K)+cos(F)cos(K) sin(F)cos(T) ] * [ 0 ]
// z cos(F)sin(T)cos(K)+sin(F)sin(K) cos(F)sin(T)sin(K)-sin(F)cos(K) cos(F)cos(T) 1
//K 是 yaw,T 是 pitch,F 是 roll,旋转顺序为 ZYX
// w^2+x^2-y^2-z^2 2*(x*y+w*z) 2*(x*z-w*y)
//上式中的旋转矩阵用四元数表示即为 ：[ 2*(x*y-w*z) w^2-x^2+y^2-z^2 2*(y*z+w*x) ]
// 2*(x*z+w*y) 2*（y*z-w*x） w^2-x^2-y^2+z^2
//
float x_ac = x_q*z_q_2 - w_q*y_q_2;// 2*(x*z-w*y) =ax*az(1-cos(alpha))-ay*sin(alpha)
float y_ac = y_q*z_q_2 + w_q*x_q_2;// 2*(y*z+w*x) =az*ay(1-cos(alpha))+ax*sin(alpha)
float z_ac = 1 - x_q*x_q_2 - y_q*y_q_2;// w^2+x^2-y^2-z^2 =1-2*x^2-2*y^2 = cos(alpha)+(1-cos(alpha)*z^2)
//
// 测量值与常量的叉积。//测量值叉乘常量值,并以此向量表示误差角度大小与转轴方向，用于修正陀螺仪积分角度
float x_ca = y_aa * z_ac - z_aa * y_ac;
float y_ca = z_aa * x_ac - x_aa * z_ac;
float z_ca = x_aa * y_ac - y_aa * x_ac;
//
// 构造增量旋转。//可看成分别绕 xyz 轴的三次旋转的叠加。sin(delta/2)近似为 delta/2,cos(delta/2)近似为 0
float delta_x = gyr[0] * interval / 2 + x_ca * FACTOR;//绕 x 轴旋转角度的一半，记 d_x 看作绕 x 轴的一次旋转：w=1,x=d_x,y=0,z=0
float delta_y = gyr[1] * interval / 2 + y_ca * FACTOR;//绕 y 轴旋转角度的一半，记 d_y 看作绕 y 轴的一次旋转：w=1,x=0,y=d_y,z=0
float delta_z = gyr[2] * interval / 2 + z_ca * FACTOR;//绕 z 轴旋转角度的一半，记 d_z 看作绕 z 轴的一次旋转：w=1,x=0,y=0,z=d_z
//三次旋转叠加为一次旋转，即三个四元数相乘
//四元数乘法公式：q3=q1*q2
|
楼主
标题置顶
标题高亮
