C语言实现姿态四元数转旋转矢量

四元数

在将三维矢量代数推广至乘法和除法运算的研究中，爱尔兰数学家、物理学家哈密顿于1843年创建了四元数（quaternion）和四元数代数。四元数是指由一个实数单位1和三个虚数单位i，j，k组成并具有下列形式实元的数。

单位1，i， j，k可以看作四维空间（用符号H来表示)中的一组单位矢量。任何四元数都可看作该空间中的一个点或向量。

旋转矢量

力学中刚体的有限次转动是不可交换的。转动的不可交换性决定了转动不是矢量，即两次以上的不同轴转动不能相加。对一个空间方向随时间变化的角速度矢量进行积分是没有物理意义的。而旋转矢量破解了这个难题。一个坐标系到另一个坐标系的变换可以通过多次转动来完成，也可以通过绕一个定义在参考坐标系中的矢量的单次转动来实现。该矢量称作等效旋转矢量(rotation vector）是一个三元素的向量，旋转矢量的方向给出了转动轴的方向，它的模长为转动角度的大小。又称为轴角（axis-angle)。

源码

/* Convert  attitude quaternion to Rotation vector ---------------------------------------------------------
* args   : double *q        I   attitude quaternion vector (4 x 1)
*          double *r