我們知道在平面（x，y）中的旋轉(zhuǎn)可以用復(fù)數(shù)來(lái)表示，同樣的三維中的旋轉(zhuǎn)可以用單位四元數(shù)來(lái)描述。我們來(lái)定義一個(gè)四元數(shù)：

　　我們可以把它寫成，其中，。那么是矢量，表示三維空間中的旋轉(zhuǎn)軸。w是標(biāo)量，表示旋轉(zhuǎn)角度。那么就是繞軸旋轉(zhuǎn)w度，所以一個(gè)四元數(shù)可以表示一個(gè)完整的旋轉(zhuǎn)。只有單位四元數(shù)才可以表示旋轉(zhuǎn)，至于為什么，因?yàn)檫@就是四元數(shù)表示旋轉(zhuǎn)的約束條件。

　　而剛才用歐拉角描述的方向余弦矩陣用四元數(shù)描述則為：

　　所以在軟件解算中，我們要首先把加速度計(jì)采集到的值（三維向量）轉(zhuǎn)化為單位向量，即向量除以模，傳入參數(shù)是陀螺儀x、y、z值和加速度計(jì)x、y、z值：

　　void IMUupdate（float gx， float gy， float gz， float ax， float ay， float az）

　　{ float norm;

　　float vx， vy， vz; float ex， ey， ez;

　　norm = sqrt（ax*ax + ay*ay + az*az）;

　　ax = ax / norm;

　　ay = ay / norm;

　　az = az / norm;

　　下面把四元數(shù)換算成方向余弦中的第三行的三個(gè)元素。剛好vx、vy、vz 。其實(shí)就是上一次的歐拉角（四元數(shù)）的機(jī)體坐標(biāo)參考系換算出來(lái)的重力的單位向量。

　　estimated direction of gravity vx = 2*（q1*q3 - q0*q2）;

　　vy = 2*（q0*q1 + q2*q3）;

　　vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

　　axyz是機(jī)體坐標(biāo)參照系上，加速度計(jì)測(cè)出來(lái)的重力向量，也就是實(shí)際測(cè)出來(lái)的重力向量。

　　axyz是測(cè)量得到的重力向量，vxyz是陀螺積分后的姿態(tài)來(lái)推算出的重力向量，它們都是機(jī)體坐標(biāo)參照系上的重力向量。

　　那它們之間的誤差向量，就是陀螺積分后的姿態(tài)和加計(jì)測(cè)出來(lái)的姿態(tài)之間的誤差。

　　向量間的誤差，可以用向量叉積（也叫向量外積、叉乘）來(lái)表示，exyz就是兩個(gè)重力向量的叉積。

　　這個(gè)叉積向量仍舊是位于機(jī)體坐標(biāo)系上的，而陀螺積分誤差也是在機(jī)體坐標(biāo)系，而且叉積的大小與陀螺積分誤差成正比，正好拿來(lái)糾正陀螺。（你可以自己拿東西想象一下）由于陀螺是對(duì)機(jī)體直接積分，所以對(duì)陀螺的糾正量會(huì)直接體現(xiàn)在對(duì)機(jī)體坐標(biāo)系的糾正。

　　integral error scaled integral gain exInt = exInt + ex*Ki;

　　eyInt = eyInt + ey*Ki;

　　ezInt = ezInt + ez*Ki;

　　用叉積誤差來(lái)做PI修正陀螺零偏

　　integral error scaled integral gain exInt = exInt + ex*Ki;

　　eyInt = eyInt + ey*Ki;

　　ezInt = ezInt + ez*Ki; // adjusted gyroscope measurements

　　gx = gx + Kp*ex + exInt;

　　gy = gy + Kp*ey + eyInt;

　　gz = gz + Kp*ez + ezInt;

　　四元數(shù)微分方程，其中T為測(cè)量周期，為陀螺儀角速度，以下都是已知量，這里使用了一階龍哥庫(kù)塔求解四元數(shù)微分方程：

　　integrate quaternion rate and normalise

　　q0 = q0 + （-q1*gx - q2*gy - q3*gz）*halfT;

　　q1 = q1 + （q0*gx + q2*gz - q3*gy）*halfT;

　　q2 = q2 + （q0*gy - q1*gz + q3*gx）*halfT;

　　q3 = q3 + （q0*gz + q1*gy - q2*gx）*halfT;

　　最后根據(jù)四元數(shù)方向余弦陣和歐拉角的轉(zhuǎn)換關(guān)系，把四元數(shù)轉(zhuǎn)換成歐拉角：

　　所以有：

　　ANGLE.Yaw = atan2（2 * q1 * q2 + 2 * q0 * q3， -2 * q2*q2 - 2 * q3* q3 + 1）* 57.3; // yaw

　　ANGLE.Y= asin（-2 * q1 * q3 + 2 * q0* q2）* 57.3; // pitch

　　ANGLE.X= atan2（2 * q2 * q3 + 2 * q0 * q1， -2 * q1 * q1 - 2 * q2* q2 + 1）* 57.3; // roll