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行的三個元素即重力加速度在機體座標的三個分量。
三軸加速度計 三軸陀螺儀 三軸磁力計
1.今天要做的事情 三軸加速度計 1.加速度計的原理 三軸加速度 輸出的速度是去掉重力後的整體加速度，因此，三軸加速度計可以測量物體的比力。即，去掉重力後的整體加速度，應用 測量角度 比力 通過彈簧壓縮長度來測量 在沒有外力的作用下，加速度能夠精準地測量俯仰角和滾轉角並且沒有累計誤差 mems 主要...
MEMS加速度計效能測評
本文介紹了微機電系統 mems 加速度計的多項特性，它們使得該技術對狀態監控應用極具吸引力。本文通過回顧一些資料來說明mems技術的發展狀況及效能水平，並將其與商用壓電 pzt 狀態監控加速度計進行比較。對mems工藝技術的投資加上設計創新，已大大改善mems效能，使得mems足以成為更廣泛狀態監控...
IMU中的加速度計的作用
imu中的加速度計的作用 加速度計加速度計概述加速度計是慣性導航體系的中心元件之 依託它比照力的丈量，完結慣導體系確定載體的夥置 速度以及發生跟蹤訊號的使命。載體加速度的丈量有必要非常精確地進行，並且是在由陀螺安穩的參閱座標系中進行。在不需要進行高度操控的慣導體系中，只需兩個加應度計就可以完結上述使...