因爲感覺寫的太好了，所以就轉了！
詳細可以參考原來的帖子：http://www.amobbs.com/thread-5492189-1-1.html
---------------------------------------------------------------------------------------------------------------------------------
1、四個概念：“地理”座標系、“機體”座標系、他們之間換算公式、換算公式用的係數。
地理座標系：東、北、天，以下簡稱地理。在這個座標系裏有重力永遠是（0,0,1g），地磁永遠是（0,1,x）（地磁的垂直不關心）兩個三維向量。
機體座標系：以下簡稱機體，上面有陀螺、加計、電子羅盤傳感器，三個三維向量。
換算公式：以下簡稱公式，公式就是描述機體姿態的表達方法，一般都是用以地理爲基準，從地理換算到機體的公式，有四元數、歐拉角、方向餘弦矩陣。
換算公式的係數：以下簡稱係數，四元數的q0123、歐拉角的ROLL/PITCH/YAW、餘弦矩陣的9個數。係數就是描述機體姿態的表達方法的具體數值。
姿態，其實就是公式+係數的組合，一般經常用人容易理解的公式“歐拉角”表示，係數就是橫滾xx度俯仰xx度航向xx度。
2、五個數據源：重力、地磁、陀螺、加計、電子羅盤，前兩個來自地理，後三個來自機體。
3、陀螺向量：基於機體，也在機體上積分，因爲地理上無參考數據源，所以很獨立，直接在公式的老係數上積分，得到新系數。
狹義上的捷聯慣導算法，就是指這個陀螺積分公式，也分爲歐拉角、方向餘弦矩陣、四元數，他們的積分算法有增量法、數值積分法（X階龍格-庫塔）等等
4、加計向量、重力向量：加計基於機體，重力基於地理，重力向量（0,0,1g）用公式換算到機體，與機體的加計向量算出誤差。理論上應該沒有誤差，這誤差逆向思維一下，其實就是換算公式的係數誤差。所以這誤差可用於糾正公式的係數（橫滾、俯仰），也就是姿態。
5、電子羅盤向量、地磁向量：同上，只不過要砍掉地理上的垂直向量，因爲無用。只留下地理水平面上的向量。誤差可以用來糾正公式的係數（航向）。
6、就這樣，係數不停地被陀螺積分更新，也不停地被誤差修正，它和公式所代表的姿態也在不斷更新。
如果積分和修正用四元數算法（因爲運算量較少、無奇點誤差），最後用歐拉角輸出控制PID（因爲角度比較直觀），那就需要有個四元數係數到歐拉角係數的轉換。常用的三種公式，它們之間都有轉換算法。
再搞個直白一點的例子：
機體好似一條船，地理就是那地圖，姿態就是航向（船頭在地圖上的方位），重力和地磁是地圖上的燈塔，陀螺/積分公式是舵手，加計和電子羅盤是瞭望手。
舵手負責估計和把穩航向，他相信自己，本來船向北開的，就一定會一直往北開，覺得轉了90度彎，那就會往東開。
當然如果舵手很牛逼，也許能估計很準確，維持很長時間。不過只信任舵手，肯定會迷路，所以一般都有地圖和瞭望手來觀察誤差。
瞭望手根據地圖燈塔方位和船的當前航向，算出燈塔理論上應該在船的X方位。然而看到實際燈塔在船的Y方位，那肯定船的當前航向有偏差了，偏差就是ERR=X-Y。
舵手收到瞭望手給的ERR報告，覺得可靠，那就聽個90%*ERR，覺得天氣不好、地圖誤差大，那就聽個10%*ERR，根據這個來糾正估算航向。。
------------------------------------------------------
來點乾貨，注意以下的歐拉角都是這樣的順序：先航向-再俯仰-然後橫滾
公式截圖來自：袁信、鄭鍔的《捷聯式慣性導航原理》，鄧正隆的《慣性技術》。
--------------------------------------------------
根據加計計算初始歐拉角
這個無論歐拉角算法還是四元數算法還是方向餘弦矩陣都需要，因爲加計和電子羅盤給出歐拉角的描述方式比較方便。
imu.euler.x = atan2(imu.accel.y, imu.accel.z);
imu.euler.y = -asin(imu.accel.x / ACCEL_1G);
ACCEL_1G 爲9.81米/秒^2，accel.xyz的都爲這個單位，算出來的euler.xyz單位是弧度
航向imu.euler.z可以用電子羅盤計算
--------------------------------------------------
歐拉角微分方程
如果用歐拉角算法，那麼這個公式就夠了，不需要來回轉換。
矩陣上到下三個角度（希臘字母）是roll pitch和yaw，公式最左邊的上面帶點的三個是本次更新後的角度，不帶點的是上個更新週期算出來的角度。
Wx,y,z是roll pitch和yaw方向的三個陀螺在這個週期轉動過的角度，單位爲弧度，計算爲間隔時間T*陀螺角速度,比如0.02秒*0.01弧度/秒=0.0002弧度.
--------------------------------------------------
以下是四元數
--------------------------------------------------
四元數初始化
q0-3爲四元數四個值,用最上面公式根據加計計算出來的歐拉角來初始化
--------------------------------------------------
四元數微分方程
四元數更新算法，一階龍庫法，同樣4個量（入、P1-3）也爲四元數的四個值,即上面的q0-3。
Wx,y,z是三個陀螺的這個週期的角速度，比如歐拉角微分方程中的0.01弧度/秒，T爲更新週期,比如上面的0.02秒。
再來一張，另外一本書上的，仔細看和上面是一樣的delta角度，就是上面的角速度*週期，單位爲弧度
--------------------------------------------------
四元數微分方程更新後的規範化
每個週期更新完四元數，需要對四元數做規範化處理。因爲四元數本來就定義爲四維單位向量。
求q0-3的平方和，再開根號算出的向量長度length。然後每個q0-3除這個length。
--------------------------------------------------
四元數轉歐拉角公式
把四元數轉成了方向餘弦矩陣中的幾個元素，再用這幾個元素轉成了歐拉角
先從四元數q0-3轉成方向餘弦矩陣：
再從方向餘弦矩陣轉成歐拉角
代碼：
//更新方向餘弦矩陣
t11=q.q0*q.q0+q.q1*q.q1-q.q2*q.q2-q.q3*q.q3;
t12=2.0*(q.q1*q.q2+q.q0*q.q3);
t13=2.0*(q.q1*q.q3-q.q0*q.q2);
t21=2.0*(q.q1*q.q2-q.q0*q.q3);
t22=q.q0*q.q0-q.q1*q.q1+q.q2*q.q2-q.q3*q.q3;
t23=2.0*(q.q2*q.q3+q.q0*q.q1);
t31=2.0*(q.q1*q.q3+q.q0*q.q2);
t32=2.0*(q.q2*q.q3-q.q0*q.q1);
t33=q.q0*q.q0-q.q1*q.q1-q.q2*q.q2+q.q3*q.q3;
//求出歐拉角
imu.euler.roll = atan2(t23,t33);
imu.euler.pitch = -asin(t13);
imu.euler.yaw = atan2(t12,t11);
if (imu.euler.yaw < 0){
imu.euler.yaw += ToRad(360);
}
----------------------------------------------------
以下代碼摘自網上，很巧妙，附上註釋，有四元數微分，有加計耦合。
沒電子羅盤，其實耦合原理也一樣。
-
//=====================================================================================================
-
// IMU.c
-
// S.O.H. Madgwick
-
// 25th September 2010
-
//=====================================================================================================
-
// Description:
-
//
-
// Quaternion implementation of the 'DCM filter' [Mayhony et al].
-
//
-
// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
-
//
-
// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
-
// orientation. See my report for an overview of the use of quaternions in this application.
-
//
-
// User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz')
-
// and accelerometer ('ax', 'ay', 'ay') data. Gyroscope units are radians/second, accelerometer
-
// units are irrelevant as the vector is normalised.
-
//
-
//=====================================================================================================
-
-
//----------------------------------------------------------------------------------------------------
-
// Header files
-
-
#include "IMU.h"
-
#include <math.h>
-
-
//----------------------------------------------------------------------------------------------------
-
// Definitions
-
-
#define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
-
#define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases
-
#define halfT 0.5f // half the sample period
-
-
//---------------------------------------------------------------------------------------------------
-
// Variable definitions
-
-
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
-
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
-
-
//====================================================================================================
-
// Function
-
//====================================================================================================
-
-
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
-
float norm;
-
float vx, vy, vz;
-
float ex, ey, ez;
-
-
// normalise the measurements
-
norm = sqrt(ax*ax + ay*ay + az*az);
-
ax = ax / norm;
-
ay = ay / norm;
-
az = az / norm;
-
把加計的三維向量轉成單位向量。
-
-
-
// 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;
-
-
這是把四元數換算成《方向餘弦矩陣》中的第三列的三個元素。
-
根據餘弦矩陣和歐拉角的定義，地理座標系的重力向量，轉到機體座標系，正好是這三個元素。
-
所以這裏的vx\y\z，其實就是當前的歐拉角（即四元數）的機體座標參照系上，換算出來的重力單位向量。
-
-
-
// error is sum of cross product between reference direction of field and direction measured by sensor
-
ex = (ay*vz - az*vy);
-
ey = (az*vx - ax*vz);
-
ez = (ax*vy - ay*vx);
-
-
axyz是機體座標參照系上，加速度計測出來的重力向量，也就是實際測出來的重力向量。
-
axyz是測量得到的重力向量，vxyz是陀螺積分後的姿態來推算出的重力向量，它們都是機體座標參照系上的重力向量。
-
那它們之間的誤差向量，就是陀螺積分後的姿態和加計測出來的姿態之間的誤差。
-
向量間的誤差，可以用向量叉積（也叫向量外積、叉乘）來表示，exyz就是兩個重力向量的叉積。
-
這個叉積向量仍舊是位於機體座標系上的，而陀螺積分誤差也是在機體座標系，而且叉積的大小與陀螺積分誤差成正比，正好拿來糾正陀螺。（你可以自己拿東西想象一下）由於陀螺是對機體直接積分，所以對陀螺的糾正量會直接體現在對機體座標系的糾正。
-
-
-
-
// 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;
-
-
用叉積誤差來做PI修正陀螺零偏
-
-
-
-
-
// 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;
-
四元數微分方程
-
-
-
-
// normalise quaternion
-
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
-
q0 = q0 / norm;
-
q1 = q1 / norm;
-
q2 = q2 / norm;
-
q3 = q3 / norm;
-
四元數規範化
-
}
-
-
//====================================================================================================
-
// END OF CODE
-
//====================================================================================================
