四元数是简单的
超复数。
复数是由
实数加上虚数单位 i 组成,其中i^2 = -1。 相似地,
四元数都是由实数加上三个虚数单位 i、j、k 组成,而且它们有如下的关系: i^2 = j^2 = k^2 = -1, i^0 = j^0 = k^0 = 1 , 每个四元数都是 1、i、j 和 k 的线性组合,即是四元数一般可表示为a + bk+ cj + di,其中a、b、c 、d是实数。
对于i、j、k本身的几何意义可以理解为一种旋转,其中i旋转代表X轴与Y轴相交平面中X轴正向向Y轴正向的旋转,j旋转代表Z轴与X轴相交平面中Z轴正向向X轴正向的旋转,k旋转代表Y轴与Z轴相交平面中Y轴正向向Z轴正向的旋转,-i、-j、-k分别代表i、j、k旋转的反向旋转。(见右图)
四元数拉。直接用老外Madgwick的IMU就可以。超级简单
unsigned char BS004_IMU_Update(float ax,float ay,float az,float gx,float gy,float gz)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
//
//:四元数乘法运算
float q0q0 = q0 * q0;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
float q1q1 = q1 * q1;
float q1q3 = q1 * q3;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q3q3 = q3 * q3;
//
//:归一化处理
norm = sqrt(ax*ax + ay*ay + az*az);
if(norm==0) return 0;
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);
//
//:比例运算
exInt = exInt + ex*bs004_quad_Ki;
eyInt = eyInt + ey*bs004_quad_Ki;
ezInt = ezInt + ez*bs004_quad_Ki;
//
//:陀螺仪融合
gx = gx + bs004_quad_Kp*ex + exInt;
gy = gy + bs004_quad_Kp*ey + eyInt;
gz = gz + bs004_quad_Kp*ez + ezInt;
//
//:整合四元数率
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*bs004_quad_halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*bs004_quad_halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*bs004_quad_halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*bs004_quad_halfT;
//
//:归一化处理
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
if(norm==0) return 0;
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
//
//:欧拉角转换
bs004_imu_roll=asin(-2*q1q3 + 2*q0q2)*57.30f;
bs004_imu_pitch=atan2(2*q2q3 + 2*q0q1, -2*q1q1-2*q2q2 + 1)*57.30f;
bs004_imu_yaw=bs004_imu_yaw-gz*bs004_mpu6050_gyro_scale;
//
return 1;
}