验证网络上四元数的正确性

　　这里的四元数算法来自这里,我没有直接使用这个算法而是自己写了个程序验证其正确性.下面是代码:

(博客园 : 梦工厂2012)

// Quaternion_test.cpp : Defines the entry point for the console application.
//


/*所有方法体者经过了验证*/
#include "stdafx.h"
#include "math.h"
#define pi 3.1415926
/*四元数的元素，代表估计方向  */
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float q0_inc,q1_inc,q2_inc,q3_inc;     
/*用于对四元进行更新,角增量,不是真实的欧拉角*/
float Roll_inc,Pitch_inc,Yaw_inc;
float Roll,Pitch,Yaw;                /*真实欧拉角*/

void FromEulerAngle(float Roll_add,float Pitch_add,float Yaw_add) ;
void ToEulerAngle();
void Quaternion_nor();
void Multiply(float q0_n,float q1_n,float q2_n,float q3_n);


int main(int argc, char* argv[])
{
    Roll_inc=00.0/180*pi;
    Pitch_inc=0.0/180*pi;
    Yaw_inc=10.0/180*pi;

    printf("第一次旋转\n");
    printf("yaw_inc=%f\n",Yaw_inc*57.3);
    printf("pitch_inc=%f\n",Pitch_inc*57.3);
    printf("roll_inc=%f\n",Roll_inc*57.3);

    FromEulerAngle(Roll_inc,Pitch_inc,Yaw_inc) ;
    /*更新四元  */
    Multiply(q0_inc,q1_inc,q2_inc,q3_inc);

    printf("q0=%f\n",q0);
    printf("q1=%f\n",q1);
    printf("q2=%f\n",q2);
    printf("q3=%f\n",q3);
    
    ToEulerAngle();

    printf("yaw=%f\n",Yaw*57.3);
    printf("pitch=%f\n",Pitch*57.3);
    printf("roll=%f\n",Roll*57.3);

    printf("第二次旋转\n");
    Roll_inc=0.0/180*pi;
    Pitch_inc=10.0/180*pi;
    Yaw_inc=0.0/180*pi;

/*角度增量*/
    printf("yaw_inc=%f\n",Yaw_inc*57.3);
    printf("pitch_inc=%f\n",Pitch_inc*57.3);
    printf("roll_inc=%f\n",Roll_inc*57.3);

    /*欧拉角转四元数, 增量四元数*/
    FromEulerAngle(Roll_inc,Pitch_inc,Yaw_inc) ;

    /*更新四元数*/
    Multiply(q0_inc,q1_inc,q2_inc,q3_inc);
    printf("第二次旋转的四元数\n");
    printf("q0=%f\n",q0);
    printf("q1=%f\n",q1);
    printf("q2=%f\n",q2);
    printf("q3=%f\n",q3);
    /*转成载体欧拉角*/
    ToEulerAngle();
    printf("yaw=%f\n",Yaw*57.3);
    printf("pitch=%f\n",Pitch*57.3);
    printf("roll=%f\n",Roll*57.3);
/*第三次旋转*/


    printf("第三次旋转\n");
    Roll_inc=10.0/180*pi;
    Pitch_inc=0.0/180*pi;
    Yaw_inc=0.0/180*pi;

/*角度增量*/
    printf("yaw_inc=%f\n",Yaw_inc*57.3);
    printf("pitch_inc=%f\n",Pitch_inc*57.3);
    printf("roll_inc=%f\n",Roll_inc*57.3);

    /*欧拉角转四元数, 增量四元数*/
    FromEulerAngle(Roll_inc,Pitch_inc,Yaw_inc) ;

    /*更新四元数*/
    Multiply(q0_inc,q1_inc,q2_inc,q3_inc);
    printf("第三次旋转的四元数\n");
    printf("q0=%f\n",q0);
    printf("q1=%f\n",q1);
    printf("q2=%f\n",q2);
    printf("q3=%f\n",q3);
    /*转成载体欧拉角*/
    ToEulerAngle();
    printf("yaw=%f\n",Yaw*57.3);
    printf("pitch=%f\n",Pitch*57.3);
    printf("roll=%f\n",Roll*57.3);





    return 0;
}

/*欧拉角转四元,其它坐标系 */
/*这里是否采用小角近似???*/
void FromEulerAngle(float Roll_add,float Pitch_add,float Yaw_add)  /*这里只是机体转角近似成欧拉角   */
{

    /*在俊的姿态解算中,用的是小角近似  q=[1,Ω*t/2,Ω*t/2,Ω*t/2]T*/

    float fCosHRoll = (float)cos(Roll_add * .5f);
    float fSinHRoll = (float)sin(Roll_add * .5f);
    float fCosHPitch = (float)cos(Pitch_add * .5f);
    float fSinHPitch = (float)sin(Pitch_add * .5f);
    float fCosHYaw = (float)cos(Yaw_add * .5f);
    float fSinHYaw = (float)sin(Yaw_add * .5f);                   /*回来看看这三角函数运算用了多长时间*/
                                                                                        /*下面这个运算要根据坐标第进行修改*/
    q0_inc = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw;
    q1_inc = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw;
    q2_inc = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw;
    q3_inc = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw;
    
}
/*四元数转欧拉角*/
void ToEulerAngle()
{
    Roll=atan2(2 * (q0 * q1 + q2 * q3) , 1 - 2 * (q1 * q1 + q2 * q2)); 
    Yaw = atan2(2 * (q0 * q3 + q1 * q2) , 1 - 2 * (q2 * q2 + q3 * q3));
    Pitch = asin(2*(q0*q2-q3*q1)) ;
}
    
/*更新四元数*/
/* q_n  这里代表新来的四元数,这里指的是增量   */
void Multiply(float q0_n,float q1_n,float q2_n,float q3_n)
{
    float q0_temp=q0*q0_n      -q1*q1_n      -q2*q2_n      -q3*q3_n;
    float q1_temp=q0*q1_n      +q1*q0_n      +q2*q3_n      -q3*q2_n;
    float q2_temp=q0*q2_n      -q1*q3_n      +q2*q0_n      +q3*q1_n;
    float q3_temp=q0*q3_n      +q1*q2_n      -q2*q1_n      +q3*q0_n;
    q0=q0_temp;
    q1=q1_temp;
    q2=q2_temp;
    q3=q3_temp;

float s=sqrt(q0*q0+q1*q1+q2*q2+q3*q3); //这里重新进行规范化,避免的累积误差
q0=q0/s;
q1=q1/s;
q2=q2/s;
q3=q3/s;

}

网页中给的欧拉角的转动顺序为  yaw  pitch  roll   ,代码不是最重要的而是其验证的思想 .这里我介绍一下代码的思想 ,首先我们假设转动的顺序为 yaw pitch  roll  ,

这样的话我们首先只进行 yaw角度的旋转,pitch角度的旋转,roll角度的旋转 ,如果假设成立的话应该出现下面这结果,也就是说最终的欧拉角不相互影响:

进一步验证其正确性,其共有6种可能性,逐一进行验证,最终确定其转动顺序为 yaw pitch roll  .和之前说的一致.

我利用这个算法 对陀螺仪数据 简单平均采样,限幅处理之后,直接积分的效果:

 下面这个视频是 与加计互补滤波,可以全姿态

imu.c 与  互补滤波 四元数算法对比

 

博文为本人所写转载请表明出处 :验证网络上四元数的正确性