1.幾個基本概念。
地理坐標系：北東地。北為X軸、東為Y軸、地為Z軸。
機體坐標系：見下圖。
姿態角：我的理解是，roll角是Zb在ZOY平面與Z軸的夾角。pitch角是Zb在ZOX平面與Z軸的夾角。yaw角是Xb在XOY平面與X軸的夾角。
旋轉矩陣：地理坐標系到機體坐標系的轉換矩陣。旋轉矩陣為正交陣，逆即轉置。
傳感器：都是基於機體坐標系的。加速度計測三軸的加速度，磁力計測三軸的磁感應強度，陀螺儀測三軸的角速度。注意角速度的定義，方向是沿着三軸的。得到姿態角主要靠陀螺儀測得的角速度積分，但存在漂移誤差，因此要用加速度計和磁力計隨時校正。
很重要的：在地理坐標系下，重力加速度坐標始終為（0,0,1g），磁場坐標為（M,0,D）。在地球表面，磁場始終是沿着磁感應線指北的，某點的磁場強度為磁感應線在該點的切線，它在北方有個分量，在垂直方向有個分量，在Y軸是沒有分量的，即（M,0，D）。這里還有一點，地理的北極和磁場的北極是不重合的，有一定的夾角，即磁偏角。根據經緯度是可以查到這個夾角的大小的。我們定義的地理坐標系的北當然是地理的北。但用磁力計求得實際是Xb與地磁的北的夾角，我們最后會加上磁偏角，這樣就是Xb與地理北的夾角了，就是真正的航向角啦。
理解重力加速度和磁感應強度的概念是非常重要的。
好了，我們可以開始求初始姿態角了。
2.加速度計求roll,pitch角。
飛機靜止放在放地上時，地理坐標系下重力加速度為（0，0，g），機體坐標系下加速度測量的三軸加速度為（ax,ay,az）
它們兩者之間的關系是怎么表示呢，可以想象一個長方體，（ax,ay,az）是三條邊，（0,0，g）是對角線。
這個圖可以清晰的說明一切啦。畫圖花了我好久。求姿態角就是求兩個坐標系的夾角關系。在求俯仰角和橫滾角時，我們完全可以把Z軸當做參考來求。上面我說俯仰角是Zb在XOZ平面的投影與Z軸的夾角，這里我是把Z軸投影到XbOZb平面上的，仔細想想是一樣的，Z軸在XbOZb平面上的投影和Zb軸的夾角依然是俯仰角啦。橫滾同理。
這樣roll=arctan(ay/az)，pitch =-arctan(ax/az) 方向的正負號是自己定義的。
3.磁力計求航向角。
地理坐標系下該點的磁場強度為（M,0，D），磁力計測得的三軸磁場強度為（mx,my,mz）。
當我們認為飛機是完全水平放在地上的時候，即Z軸和Zb軸是平行的時候，完全不用考慮垂直方向了。我們只用考慮XOY平面了。
那么yaw=arctan(my/mx)
很簡單對不對，但實際上飛機是不可能水平放在地面上的，肯定會有一點傾斜的，也就是說肯定會有roll和pitch角啦。只要飛機傾斜，磁力計測得的磁場強度mx就不是真正的水平X軸上的分量了，my當然也不是了。我們必須把根據mx,my，mz，roll，pitch求出投影在XOY面上的兩個量了。
我們是先用加速度計求出roll和pitch角，然后根據求得的roll和pitch角，磁力計的測量的（mx,my,mz）來求yaw角。對，這才是求初始姿態角正確的打開方式。
直接按思路求出XOY上投影量比較復雜，但絕對是可以的，下面有一種比較巧妙的辦法。
4.好吧，我們在這里又要講另外一個很重要的線性代數知識。才能理解上面圖片里說的什么。
根據上圖可以得到一個重要結論，那就是，從機體坐標系到地理坐標系的轉換矩陣的第3行的三個元素即重力加速度在機體坐標的三個分量，即地理坐標系下[0,0,g]對應機體坐標系下[ax,ay,az]！同理，磁力計的三個分量，也就是可以認為地理坐標系下[M,0,D]對應機體坐標系下[mx,my,mz]。
為什么重力加速度對應的機體測量值可以當做旋轉矩陣的第三行。
如上圖，這是通過轉換矩陣把地理坐標轉換到機體坐標，可以看出x3,x6,x9除以g就是重力加速度在機體坐標下的三個分量。可能我沒說清楚，上文說的[ax,ay,az]是單位化了的（為了讓你看的更清楚，該圖片的沒喲，是真是測量值）！[mx,my,mz]也是單位化了的！我們在談旋轉矩陣時，向量都是單位化的，因為牢記旋轉矩陣是單位正交的！！！！這是地理坐標到機體坐標的矩陣，轉置一下，就是從機體坐標系到地理坐標系的轉換矩陣了。故從機體坐標系到地理坐標系的轉換矩陣的第3行的三個元素即重力加速度在機體坐標的三個分量。