四元数的理解和应用

四元数是简单的 超复数。  复数是由 实数加上虚数单位 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;        
}