//陀螺仪x轴为前进方向
init_Roll = atan2(init_ay, init_az);
init_Pitch = -asin(init_ax); //init_Pitch = asin(ax / 1);
init_Yaw = -atan2(init_mx*cos(init_Roll) + init_my*sin(init_Roll)*sin(init_Pitch) + init_mz*sin(init_Roll)*cos(init_Pitch),
init_my*cos(init_Pitch) - init_mz*sin(init_Pitch)); //atan2(mx, my);
q0 = cos(0.5*init_Roll)*cos(0.5*init_Pitch)*cos(0.5*init_Yaw) + sin(0.5*init_Roll)*sin(0.5*init_Pitch)*sin(0.5*init_Yaw); //w
q1 = sin(0.5*init_Roll)*cos(0.5*init_Pitch)*cos(0.5*init_Yaw) - cos(0.5*init_Roll)*sin(0.5*init_Pitch)*sin(0.5*init_Yaw); //x 绕x轴旋转是roll
q2 = cos(0.5*init_Roll)*sin(0.5*init_Pitch)*cos(0.5*init_Yaw) + sin(0.5*init_Roll)*cos(0.5*init_Pitch)*sin(0.5*init_Yaw); //y 绕y轴旋转是pitch
q3 = cos(0.5*init_Roll)*cos(0.5*init_Pitch)*sin(0.5*init_Yaw) - sin(0.5*init_Roll)*sin(0.5*init_Pitch)*cos(0.5*init_Yaw); //z 绕z轴旋转是Yaw
研究了两天。。。谁能告诉我这怎么来的吗??????。。。 |
|
楼主
