/*********************** Start Quaternion_CF **************************************************/
问题：为什么不用欧拉角来表示旋转而要引入四元数呢????????
前面介绍了什么是欧拉角，而且欧拉角微分方程解算姿态关系简单明了，
概念直观容易理解，那么我们为什么不用欧拉角来表示旋转而要引入四元数呢？
一方面是因为欧拉角微分方程中包含了大量的三角运算，这给实时解算带
来了一定的困难。而且当俯仰角为90度时方程式会出现神奇的“GimbalLock”。
所以欧拉角方法只适用于水平姿态变化不大的情况，而不适用于全姿态飞行器的姿态确定。
四元数法只求解四个未知量的线性微分方程组，计算量小，易于操作，是比较实用的工程方法。

四元数是一种超复数。如把四元数的集合考虑成多维实数空间的话，四元数就代表
k i j 着一个四维空间，相对于复数为二维空间。
简而言之，四元数包含了刚体旋转的所有信息，而在四旋翼飞行器的姿态解算中，
往往使用的是四元数微分方程对四元数进行更新

define Kp 2.0f //加速度权重，越大则向加速度测量值收敛越快

define Ki 0.001f //误差积分增益

//四元数更新姿态：http://www.crazepony.com/wiki/ ... .html
//四元数：四元数可以理解为一个实数和一个向量的组合，也可以理解为四维的向量。

void ANO_IMU::Quaternion_CF(Vector3f gyro,Vector3f acc, float deltaT)
{
Vector3f V_gravity, V_error, V_error_I;

//1.重力加速度归一化

/加速度计数据归一化，把加速度计的三维向量转换为单位向量，
因为是单位矢量到参考性的投影，所以要把加速度计数据单位化，其实
归一化改变的只是这三个向量的长度，也就是只改变了相同的倍数，方
向并没有改变，也是为了与单位四元数对应/

acc.normalize();

//2.四元数换算成方向余弦中的第三行的三个元素
// estimated direction of gravity and flux (v and w)----->估计重力方向和流量/变迁,提取四元数的等效余弦矩阵中的重力分量

/*
1.将当前姿态的重力在三个轴上的分量分离出来,把四元数换算成方向余弦中的第三行的三个元素，根据余弦矩阵和欧拉角的定义，就是地理坐标系(参考坐标系)的Z轴的重力向量。
2.注意是地理坐标系(n系)到载体坐标系(b系)的,不要弄反了。 如果书上是b系到n系,转置即可。

3.惯性测量器件测量的都是关于b系的值，为了方便，我们一般将b系转换到n系进行导航参数求解。但是这里并不需要这样做，因为这里是对陀螺仪进行补偿。

4.此处的vx,vy,vz就是上一次的欧拉角(四元数)的机体坐标参考系换算出来的重力的单位向量
vx = 2(q1q3 - q0q2);----------------->参考系Z轴与载体系 x 轴之间方向余弦向量
vy = 2(q0q1 + q2q3);----------------->参考系Z轴与载体系 y 轴之间方向余弦向量
vz = q0q0 - q1q1 - q2q2 + q3q3;----->参考系Z轴与载体系 z 轴之间方向余弦向量
*/
Q.vector_gravity(V_gravity);

//3.向量叉积得出姿态误差
/*
解释：
ax,ay,az是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向量。
ax,ay,az是测量得到的重力向量，vx,vy,vz是陀螺积分后的姿态来推算出的重力向量，它们都是机体坐标参照系上的重力向量。
那它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。
向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，ex,ey,ez就是两个重力向量的叉积。
这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，
而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。
由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
*/

计算由当前姿态的重力在三个轴上的分量与加速度计测得的重力在三个轴上的分量的差,这里采用三维空间的差积(向量积)方法求差.
计算公式由矩阵运算推导而来,公式参见http://en.wikipedia.org/wiki/Cross_product 中的Mnemonic部分

/*
向量叉积得出姿态误差，通过加速度计测得的重力坐标系下的单位向量与上一次四元数转换成的单位向量进行叉乘，以此得到其误差量外积在相减得到差分就是误差，把叉积等同于角度误差

ex = (ayvz - azvy) ;
ey = (azvx - axvz) ;
ez = (axvy - ayvx) ;
*/
V_error = acc % V_gravity;

//4.对误差进行积分
/*
积分求误差,关于当前姿态分离出的重力分量,与当前加速度计测得的重力分量的差值进行积分消除误差

原型：
exInt = exInt + ex * Ki;

eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;
*/

V_error_I += V_error * Ki;

//5.互补滤波，姿态误差补偿到角速度上，修正角速度积分漂移
//系数不停地被陀螺积分更新，也不停地被误差修正，它和公式所代表的姿态也在不断更新。 将积分误差反馈到陀螺仪上，修正陀螺仪的值

/*
将该误差V_error输入 PI 控制器后与本次姿态更新周期中陀螺仪测得的角速度相加，最终得到一个修正的角速度值,将其输入四元数微分方程，更新四元数
原型：
gx = gx + Kpex + exInt; //将误差PI后补偿到陀螺仪，即补偿零点漂移
gy = gy + Kpey + eyInt; //修正陀螺输出

gz = gz + Kp*ez + ezInt; //这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减

*/
Gyro += V_error * Kp + V_error_I;

//6.四元数的微分方程
//一阶龙格库塔法更新四元数，deltaT：陀螺采样的间隔
Q.Runge_Kutta_1st(Gyro, deltaT);

//7.四元数规范化处理
//四元数归一化：对四元数的单位化，单位化的四元数可以表示一个旋转
/*
规范化四元数作用：
1.表征旋转的四元数应该是规范化的四元数，但是由于计算误差等因素，
计算过程中四元数会逐渐失去规范化特性，因此必须对四元数做规范化处理
2.意义在于单位化四元数在空间旋转时是不会拉伸的,仅有旋转角度.这类似与线性代数里面的正交变换

3.由于误差的引入，使得计算的变换四元数的模不再等于1，变换四元数失去规范性，因此再次更新四元数
计算欧拉角时候必须要对四元数规范化处*/
Q.normalize();

// 8.四元数转欧拉角
//四元数转欧拉角:用欧拉角输出控制PID（因为角度比较直观）
Q.to_euler(&angle.x, &angle.y, &angle.z);
}
/********************************** End Quaternion_CF ***********************************/