MPU6050 四元数求得欧拉角 为什么会漂移
当固定MPU6050 一定角度时，融合后的Anglex，Angley会匀速变大或变小，大约10变化2度，平放时候为0不会发生变化。void IMUupdate( float gx, float gy, float gz, float ax, float ay, float az )
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
float q0q0 = q0 * q0;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
float q0q3 = q0 * q3;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q3q3 = q3 * q3;
float q0y_q0y = q0_y * q0_y;
float q1y_q1y = q1_y * q1_y;
float q2y_q2y = q2_y * q2_y;
float q3y_q3y = q3_y * q3_y;
float q1y_q2y = q1_y * q2_y;
float q0y_q3y = q0_y * q3_y;
/* Yaw轴四元素的微分方程 */
q0_y = q0_y + (-q1_y * gx - q2_y * gy - q3_y * gz) * halfT;
q1_y = q1_y + (q0_y * gx + q2_y * gz - q3_y * gy) * halfT;
q2_y = q2_y + (q0_y * gy - q1_y * gz + q3_y * gx) * halfT;
q3_y = q3_y + (q0_y * gz + q1_y * gy - q2_y * gx) * halfT;
/* 规范化yaw */
norm = sqrt( q0y_q0y + q1y_q1y + q2y_q2y + q3y_q3y );
q0_y = q0_y / norm;
q1_y = q1_y / norm;
q2_y = q2_y / norm;
q3_y = q3_y / norm;
/* 规范化加速度计值 */
norm = sqrt( ax * ax + ay * ay + az * az );
ax = ax / norm;
ay = ay / norm;
az = az / norm;
/* 估计重力方向和流量/变迁 */
vx = 2 * (q1q3 - q0q2);
vy = 2 * (q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
/* 向量外积再相减得到差分就是误差 */
ex = (ay * vz - az * vy);
ey = (az * vx - ax * vz);
ez = (ax * vy - ay * vx);
/* 对误差进行PI计算 */
exInt = exInt + ex * Ki;
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;
/* 校正陀螺仪 */
gx = gx + Kp * ex + exInt;
gy = gy + Kp * ey + eyInt;
gz = gz + Kp * ez + ezInt;
/* 四元素的微分方程 */
q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;
q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;
/* 规范化Pitch、Roll轴四元数 */
norm = sqrt( q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3 );
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
Anglex = atan2( 2 * q2q3 + 2 * q0q1, -2 * q1q1 - 2 * q2q2 + 1 ) * 57.2957795f;
Angley = asin( -2 * q1q3 + 2 * q0q2 ) * 57.2957795f;
Anglez = atan2( 2 * q1y_q2y + 2 * q0y_q3y, -2 * q2y_q2y - 2 * q3y_q3y + 1 ) * 57.2957795f;
}
你先想想两个问题：
1. 为什么会有Y方向的四元数？
2.下面的这个程序是否有错？
/* 四元素的微分方程 */
q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;
q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT; zouyf12 发表于 2016-5-24 23:04
你先想想两个问题：
1. 为什么会有Y方向的四元数？
我参考的这个帖子：http://www.amobbs.com/thread-5492189-1-1.html /* 四元素的微分方程 */这一块程序没看出错误来。
y轴先忽略，没作用
1. 以下程序，计算q1时的q0已经不是原来的q0了。
q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
2. gx = gx + Kp * ex + exInt;
gy = gy + Kp * ey + eyInt;
gz = gz + Kp * ez + ezInt;
Kp 是否有赋值？ 有的话加大看看效果。
页:
[1]