无人机项目跟踪记录四十一-----四元素（1）

无人机相关代码中与四元素相关的第一个函数是static void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz)，很多地方不明白，理解用到的方法是：

1、网上搜索此函数名；

2、查找电子罗盘的测量原理；

3、查找别人写的有关姿态解算的博客；

现对此函数做详细分析如下：

    float initialRoll, initialPitch;             //定义横滚、俯仰初始值
    float cosRoll, sinRoll, cosPitch, sinPitch;  //定义横滚俯仰的正弦与余弦值
    float magX, magY;                            //定义坐标变换后磁力计的值
    float initialHdg, cosHeading, sinHeading;    //定义方位角初始值以及其正弦余弦值

上面是初始化部分，没有什么好说的。

    initialRoll = atan2(-ay, -az);                  //用正切求解横滚角
    initialPitch = atan2(ax, -az);                  //用正切求解俯仰角

这两句是利用电子罗盘加速度计读取的三轴加速度值求解横滚与俯仰角，至于符号，个人理解似乎与电子罗盘安装的位置角度有关，这块有点疑问。可以参考以下两篇文章，基本都可以理解了。

https://blog.csdn.net/u011052048/article/details/105675967

https://blog.csdn.net/shawn_shao/article/details/80695917

    cosRoll = cosf(initialRoll);                    //横滚余弦值
    sinRoll = sinf(initialRoll);                    //横滚正弦值
    cosPitch = cosf(initialPitch);                  //俯仰余弦值
    sinPitch = sinf(initialPitch);                  //俯仰正弦值

    magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;   //求解磁力计旋转后的x方向的值

    magY = my * cosRoll - mz * sinRoll;                                         //求解磁力计旋转后的y方向的值

同样上面链接中的博客中有详细论述，思路是电子罗盘有两个寄存器，一个是加速度计，一个是磁力计。通过矩阵变换原理求解出从机体系到地理系的变换矩阵，然后求解出旋转后的x与y方向的磁力值。

此处有一个疑问是为何通过加速计求得的俯仰和横滚角，就与偏航角无关了，直接令它为零了呢？

    cosRoll = cosf(initialRoll * 0.5f);                                          //三维坐标空间中四元素的函数表示
    sinRoll = sinf(initialRoll * 0.5f);                                          //三维坐标空间中四元素的函数表示

    cosPitch = cosf(initialPitch * 0.5f);                                        //三维坐标空间中四元素的函数表示
    sinPitch = sinf(initialPitch * 0.5f);                                        //三维坐标空间中四元素的函数表示

    cosHeading = cosf(initialHdg * 0.5f);                                        //三维坐标空间中四元素的函数表示
    sinHeading = sinf(initialHdg * 0.5f);                                        //三维坐标空间中四元素的函数表示

上面是四元素在三维坐标系中用欧拉角的表示，可以在网上搜素四元素的数学表示，以及四元素的基本知识。

    q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;      //四元素与欧拉角的变换关系
    q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;      //四元素与欧拉角的变换关系
    q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;      //四元素与欧拉角的变换关系
    q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;      //四元素与欧拉角的变换关系

可以参看，四元素与欧拉角的变换推导：https://zhuanlan.zhihu.com/p/103622849?from_voters_page=true

四元素坐标向量的坐标旋转相乘的时候，看做向量相乘，可以加入i,j,k，x轴方向的向量是i，y轴是j，z轴是k。推导时候注意 ij=k,ji=-k，....等等，满足右手定则，三个向量相乘（直接乘开不是点乘）后整理实部就是q0，i的部分是q1，以此类推，就可以推导出以上关系式。

    // auxillary variables to reduce number of repeated operations, for 1st pass
    q0q0 = q0 * q0;                                                              //辅助计算
    q0q1 = q0 * q1;                          
    q0q2 = q0 * q2;
    q0q3 = q0 * q3;
    q1q1 = q1 * q1;
    q1q2 = q1 * q2;
    q1q3 = q1 * q3;
    q2q2 = q2 * q2;
    q2q3 = q2 * q3;
    q3q3 = q3 * q3;

以上代码为辅助计算，方便四元素矩阵计算的应用，没有啥具体意义。