MPU6050--DMP （二）四元数法

四元数为目前应用最广泛的姿态表示方法之一。四维空间的矢量能够以四元 数形式表示，因此四元数也被叫做超复数。

（1）四元数的定义 ：

四元数的表示形式可以写成：

上述式子中，q0 表示为实部，i、 j、 k, 表示为虚部。其中，虚部之间的关系为(根据右手定则)

（2）四元数的基本运算法则：

               1、 交换律、结合律可应用于四元数加法运算过程中，表示为

                2、四元数的乘法公式以原始形式和四元数代数形式表示如下(矩阵形式)：

推导过程如下：

（3）、计算姿态矩阵：

                得到姿态变换 的 四 元数乘形式 将 四 元数乘形式转换成 矩 阵形式 ， 便可得到用 四 元数表示 的 姿态矩阵。 其变换如下(也就是矩阵的乘法，非常简单) ：

根据这个结果以及上一篇的结果即可得到最终的pitch、yaw和roll三个角度的解算公式。