mpu6050姿態解算與卡爾曼濾波（1）數學

定義地理座標系n系：x軸指向東，y軸指向北，z軸指向天。在mpu6050晶片上定義載體座標系b系。那麼b系的姿態就是指n系與b系相對的旋轉關係，即如何由n系旋轉到b系。

描述這種旋轉關係通常使用尤拉角(ψ,θ,γ)T，姿態矩陣T(3x3)，四元數Q=(q0，q1，q2，q3)T。

尤拉角指的是將n系按照z軸->x’軸->y”軸的順序依次轉動 ψ,θ,γ 後就是b系，事實上尤拉角並沒有規定轉動順序，以上轉動順序z軸->x’軸->y”軸為慣性導航中常使用的規定，(ψ,θ,γ)T依次命名為偏航、俯仰、滾轉。

姿態矩陣T描述的是三維空間中的兩個標準正交基之間的向量轉換關係，T是一個3x3的正交陣。若一個向量在n系下的座標為三維列向量x

n，則該向量在b系下的座標為xb=Tbn⋅xn，反之xn=Tnb⋅xb。T陣是一個正交矩陣，故Tbn與Tnb互為轉置矩陣，互為逆矩陣。

四元數Q的本質涉及到更高等的數學，若僅考慮用四元數描述姿態，只需記住：描述姿態的四元數是歸一化的，即:

|Q|=q20+q21+q22+q23−−−−−−−−−−−−−−√=1
每個歸一化的四元數都描述了一個姿態，或者說描述了一個可由n系旋轉得到的b座標系。

以上三種描述方式是相互等價的，只要確定了兩個座標系的旋轉關係，那麼就唯一地確定了一組尤拉角，也唯一地確定了一個姿態矩陣和歸一化四元數。三種描述方式可以互相推導，由尤拉角可以推匯出姿態矩陣，再由姿態矩陣可以推匯出四元數。同樣，由四元數可以得到姿態矩陣，再得到尤拉角。

改正：以上三種描述方式是等價的，但是要注意對於兩個確定的旋轉關係的座標系，表示兩者旋轉關係的三種方式中，姿態矩陣一定是唯一的，但是四元數卻可以有兩個，而尤拉角可以有很多。在做彼此的轉換，尤其是涉及到四元數表示的時候，一定要注意。

轉換關係如下：

由尤拉角ψ,θ,γ得到姿態矩陣Tbn：
繞z軸旋轉矩陣Tz：

Tz=⎡⎣⎢cosψ−sinψ0sinψcosψ0001⎤⎦⎥
繞x軸旋轉矩陣Tx:
Tx=⎡⎣⎢1000cosθ−sinθ0sinθcosθ⎤⎦⎥
繞y軸旋轉矩陣Ty:
Ty=⎡⎣⎢cosγ0sinγ010−sinγ0cosγ⎤⎦⎥
則姿態矩陣Tbn=Ty⋅Tx⋅Tz
所以：
T

bn=⎡⎣⎢cosγcosψ+sinγsinθsinψ−cosθsinψsinγcosψ+cosγsinθsinψcosγsinψ+sinγsinθcosψcosθcosψsinγsinψ−cosγsinθcosψ