本帖轉自 http://www.geek-workshop.com/thread-1695-1-1.html
本帖翻譯自 IMU（加速度計和陀螺儀設備）在嵌入式應用中使用的指南。
這篇文章主要介紹加速度計和陀螺儀的數學模型和基本算法，以及如何融合這兩者，側重算法、思想的討論.介紹
本指南旨在向興趣者介紹慣性 MEMS（微機電系統）傳感器，特別是加速度計和陀螺儀以及其他整合 IMU（慣性測量單元）設備。
IMU 單元例子：上圖中 MCU 頂端的 ACC Gyro 6DOF，名爲 USBThumb，支持 USB/ 串口通信
在這篇文章中我將概括這麼幾個基本並且重要的話題：
- 加速度計（accelerometer）檢測什麼
- 陀螺儀（gyroscope，也稱作 gyro）檢測什麼
- 如何將傳感器 ADC 讀取的數據轉換爲物理單位（加速度傳感器的單位是 g，陀螺儀的單位是 度/秒）
- 如何結合加速度傳感器和陀螺儀的數據以得到設備和地平面之間的傾角的準確信息
在整篇文章中我儘量將數學運算降低到最少。如果你知道什麼是正弦、餘弦、正 切函數，那無論你的項目使用哪種平臺你應該都會明白和運用這篇文章中的思想， 這些平臺如 Arduino、Propeller、Basic Stamp、Ateml 芯片、PIC 芯片等等。總有些人認爲使用 IMU 單元需要複雜的數學運算（複雜的 FIR 或 IIR 濾波，如卡爾曼濾波，Parks-McClellan 濾波等）。你如果研究這些會得到很棒且很複雜的結果。我解釋事情的方式，只需要基本的數學。我非常堅信簡單的原則。我認爲 一個簡單的系統更容易操作和監控，另外許多嵌入式設備並不具備能力和資源去 實現需要進行矩陣運算的複雜算法。
我會用我設計的一個新 IMU 模塊——Acc_Gyro Accelerometer + Gyro IMU 作爲例子。在下面的例子中我們會使用這個設備的參數。用這個模塊作爲介紹非常合適，因爲它由 3 個設備組成：
- LIS331AL (datasheet) – 3 軸 2G 模擬加速度計
- LPR550AL (datasheet) – 雙軸（俯仰、翻滾） 500°/s 加速度傳感器
- LY550ALH (datasheet) –單軸（偏航）陀螺儀 最後這個設備在這篇介紹中不使用，不過他在 DCM Matrix implementation 中有重要作用
它們一起組成了一個 6 自由度的慣性測量單元。這是個花哨的名字！然而，在花哨的名字後面是個非常有用的設備組合，接下來我們會詳細介紹之。
第一部分 加速度計
要了解這個模塊我們先從加速度計開始。當我們在想象一個加速度計的時候我們可以把它想作一個圓球在一個方盒子中。你可能會把它想作一個餅乾或者甜圈， 但我就把它當做一個球好了：
我們假定這個盒子不在重力場中或者其他任何會影響球的位置的場中，球處於盒子的正中央。你可以想象盒子在外太空中，遠離任何天體，如果很難想象，那就當做盒子在航天飛機中，一切東西都處於無重力狀態。在上面的圖中你可以看到我們給每個軸分配了一對牆（我們移除了 Y+以此來觀察裏面的情況）。設想每面牆都能感測壓力。如果我們突然把盒子向左移動（加速度爲 1g=9.8m/s^2）， 那麼球會撞上 X-牆。然後我們檢測球撞擊牆面產生的壓力，X 軸輸出值爲-1g
請注意加速度計檢測到得力的方向與它本身加速度的方向是相反的。這種力量通常被稱爲慣性力或假想力 。在這個模型中你你應該學到加速度計是通過間接測量力對一個牆面的作用來測量加速度的，在實際應用中，可能通過彈簧等裝置來
測量力。這個力可以是加速度引起的，但在下面的例子中，我們會發現它不一定是加速度引起的。
如果我們把模型放在地球上，球會落在 Z-牆面上並對其施加一個 1g 的力，見下圖：
在這種情況下盒子沒有移動但我們任然讀取到 Z 軸有-1g 的值。球在牆壁上施加的壓力是由引力造成的。在理論上，它可以是不同類型的力量 - 例如，你可以想象我們的球是鐵質的，將一個磁鐵放在盒子旁邊那球就會撞上另一面牆。引用這個例子只是爲了說明加速度計的本質是檢測力而非加速度。只是加速度所引起的慣性力正好能被加速度計的檢測裝置所捕獲。
雖然這個模型並非一個 MEMS 傳感器的真實構造，但它用來解決與加速度計相關的問題相當有效。實際上有些類似傳感器中有金屬小球，它們稱作傾角開關，但是它們的功能更弱，只能檢測設備是否在一定程度內傾斜，卻不能得到傾斜的程度。
到目前爲止，我們已經分析了單軸的加速度計輸出，這是使用單軸加速度計所能得到的。三軸加速度計的真正價值在於它們能夠檢測全部三個軸的慣性力。讓我們回到盒子模型，並將盒子向右旋轉 45 度。現在球會與兩個面接觸：Z-和 X-，見下圖：
0.71g 這個值是不是任意的，它們實際上是 1/2 的平方根的近似值。我們介紹加速度計的下一個模型時這一點會更清楚。
在上一個模型中我們引入了重力並旋轉了盒子。在最後的兩個例子中我們分析了盒子在兩種情況下的輸出值，力矢量保持不變。雖然這有助於理解加速度計是怎麼和外部力相互作用的，但如果我們將座標系換爲加速度的三個軸並想象矢量力在周圍旋轉，這會更方便計算。
請看看在上面的模型，我保留了軸的顏色，以便你的思維能更好的從上一個模型轉到新的模型中。想象新模型中每個軸都分別垂直於原模型中各自的牆面。矢量
R 是加速度計所檢測的矢量（它可能是重力或上面例子中慣性力的合成）。RX，
RY，RZ 是矢量 R 在 X，Y，Z 上的投影。請注意下列關係：
，R ^ 2 = RX ^ 2 + RY ^ 2 + RZ ^ 2（公式 1） 此公式等價於三維空間勾股定理。
還記得我剛纔說的1/2 的平方根0.71 不是個隨機值吧。如果你把它們代回上式， 回顧一下重力加速度是 1g，那我們就能驗證：
1 ^ 2 =（SQRT（1/2））^ 2 + 0 ^ 2 +（SQRT（1/2））^ 2
在公式 1 中簡單的取代： R=1, Rx = -SQRT(1/2), Ry = 0 , Rz = -SQRT(1/2)
經過一大段的理論序言後，我們和實際的加速度計很靠近了。RX，RY，RZ 值是實際中加速度計輸出的線性相關值，你可以用它們進行各種計算。
在我們運用它之前我們先討論一點獲取加速度計數據的方法。大多數加速度計可歸爲兩類：數字和模擬。數字加速度計可通過 I2C，SPI 或 USART 方式獲取信息， 而模擬加速度計的輸出是一個在預定範圍內的電壓值，你需要用 ADC（模擬量轉數字量）模塊將其轉換爲數字值。我將不會詳細介紹 ADC 是怎麼工作的，部分原因是這是個很廣的話題，另一個原因是不同平臺的 ADC 都會有差別。有些 MCU 具有內置 ADC 模塊，而有些則需要外部電路進行 ADC 轉換。不管使用什麼類型的
ADC 模塊，你都會得到一個在一定範圍內的數值。例如一個 10 位 ADC 模塊的輸出值範圍在 0 .. 1023 間，請注意，1023 = 2 ^ 10 -1。一個 12 位 ADC 模塊的
輸出值範圍在 0 .. 4095 內，注意，4095 = 2 ^ 12-1。
我們繼續，先考慮下一個簡單的例子，假設我們從 10 位 ADC 模塊得到了以下的三個軸的數據：
|
AdcRx |
= |
586 |
|
AdcRy |
= |
630 |
|
AdcRz |
= |
561 |
每個 ADC 模塊都有一個參考電壓，假設在我們的例子中，它是 3.3V。要將一個
10 位的 ADC 值轉成電壓值，我們使用下列公式： VoltsRx = AdcRx * VREF / 1023
小注：8 位 ADC 的最大值是 255 = 2 ^ 8 -1，12 位 ADC 最大值是 4095 = 2 ^ 12
-1。
將 3 個軸的值代入上式，得到：
|
VoltsRx |
= |
586 |
* 3.3 |
/ |
1023 |
=～1.89V（結果取兩位小數） |
|
VoltsRy |
= |
630 |
* 3.3 |
/ |
1023 |
=～2.03V |
|
VoltsRz |
= |
561 |
* 3.3 |
/ |
1023 |
=～1.81V |
每個加速度計都有一個零加速度的電壓值，你可以在它的說明書中找到，這個電壓值對應於加速度爲 0g。通過計算相對 0g 電壓的偏移量我們可以得到一個有符號的電壓值。比方說，0g 電壓值 VzeroG= 1.65V，通過下面的方式可以得到相對 0g 電壓的偏移量:
DeltaVoltsRx = 1.89V - 1.65V = 0.24V DeltaVoltsRy = 2.03V - 1.65V = 0.38V
DeltaVoltsRz = 1.81V - 1.65V = 0.16V
現在我們得到了加速度計的電壓值，但它的單位還不是 g（9.8m/s^2），最後的轉換，我們還需要引入加速度計的靈敏度（Sensitivity），單位通常是 mV/g。比方說，加速度計的靈敏度 Sensitivity= 478.5mV / g = 0.4785V /g。靈敏度值可以在加速度計說明書中找到。要獲得最後的單位爲 g 的加速度，我們使用下列公式計算：
RX = DeltaVoltsRx /Sensitivity RX = 0.24V / 0.4785V / G =～0.5g
RY = 0.38V / 0.4785V / G =～0.79g RZ = 0.16V / 0.4785V / G =～0.33g
當然，我們可以把所有的步驟全部放在一個式子裏，但我想通過介紹每一個步驟以便讓你瞭解怎麼讀取一個 ADC 值並將其轉換爲單位爲 g 的矢量力的分量。
Rx = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity (公式 2) Ry = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity
Rz = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity
現在我們得到了慣性力矢量的三個分量，如果設備除了重力外不受任何外力影響， 那我們就可以認爲這個方向就是重力矢量的方向。如果你想計算設備相對於地面
的傾角，可以計算這個矢量和 Z 軸之間的夾角。如果你對每個軸的傾角都感興趣， 你可以把這個結果分爲兩個分量：X 軸、Y 軸傾角，這可以通過計算重力矢量和
X、Y 軸的夾角得到。計算這些角度比你想象的簡單，現在我們已經算出了 Rx，Ry，Rz 的值，讓我們回到我們的上一個加速度模型，再加一些標註上去：
我們感興趣的角度是向量 R 和 X，Y，Z 軸之間的夾角，那就令這些角度爲 Axr，
Ayr，Azr。觀察由 R 和 Rx 組成的直角三角形：
cos(Axr) = Rx / R , 類似的： cos(Ayr) = Ry / R
cos(Azr) = Rz / R
從公式 1 我們可以推導出 R = SQRT( Rx^2 + Ry^2 + Rz^2)
通過 arccos()函數（cos()的反函數）我們可以計算出所需的角度： Axr = arccos(Rx/R)
Ayr = arccos(Ry/R)
Azr = arccos(Rz/R)
我們花了大段的篇幅來解釋加速度計模型，最後所要的只是以上這幾個公式。根據你的應用場合，你可能會用到我們推導出來的幾個過渡公式。我們接下來要介紹陀螺儀模塊，並向大家介紹怎麼融合加速度計和陀螺儀的數據以得到更精確的傾角值。
但在此之前，我們再介紹幾個很常用的公式： cosX = cos(Axr) = Rx / R
cosY = cos(Ayr) = Ry / R cosZ = cos(Azr) = Rz / R
這三個公式通常稱作方向餘弦 ，它主要表達了單位向量（長度爲 1 的向量）和
R 向量具有相同的方向。你可以很容易地驗證： SQRT（cosX ^ 2 + COSY ^ 2 + cosZ ^ 2）= 1
這是個很好的性質，因爲它避免了我們一直檢測 R 向量的模（長度）。通常如果我們只是對慣性力的方向感興趣，那標準化模長以簡化其他計算是個明智的選擇。
第二部分陀螺儀
對於陀螺儀我們將不會像加速度計一樣介紹它的等價盒子模型，而是直接跳到加速度計的第二個模型，通過這個模型我們會向大家介紹陀螺儀是怎麼工作的。
陀螺儀的每個通道檢測一個軸的旋轉。例如，一個 2 軸陀螺儀檢測繞 X 和 Y 軸的旋轉。爲了用數字來表達這些旋轉，我們先引進一些符號。首先我們定義：
Rxz – 慣性力矢量 R 在 XZ 平面上的投影Ryz – 慣性力矢量 R 在 YZ 平面的上投影
在由 Rxz 和 Rz 組成的直角三角形中，運用勾股定理可得： Rxz^2 = Rx^2 + Rz^2 ，同樣：
Ryz^2 = Ry^2 + Rz^2 同時注意：
R^2 = Rxz^2 + Ry^2 ，這個公式可以公式 1 和上面的公式推導出來，也可由 R 和 Ryz 所組成的直角三角形推導出來
R ^ 2 = Ryz ^ 2 + RX ^ 2
在這篇文章中我們不會用到這些公式，但知道模型中的那些數值間的關係有助於理解。
相反，我們按如下方法定義 Z 軸和 Rxz、Ryz 向量所成的夾角： AXZ - Rxz（矢量 R 在 XZ 平面的投影）和 Z 軸所成的夾角
AYZ - Ryz（矢量 R 在 YZ 平面的投影）和 Z 軸所成夾角
現在我們離陀螺儀要測量的東西又近了一步。陀螺儀測量上面定義的角度的變化率。換句話說，它會輸出一個與上面這些角度變化率線性相關的值。爲了解釋這一點，我們先假設在 t0 時刻，我們已測得繞 Y 軸旋轉的角度（也就是 Axz）， 定義爲 Axz0，之後在 t1 時刻我們再次測量這個角度，得到 Axz1。
角度變化率按下面方法計算：
RateAxz = (Axz1 – Axz0) / (t1 – t0).
如果用度來表示角度，秒來表示時間，那這個值的單位就是 度/秒。這就是陀螺儀檢測的東西。
在實際運用中，陀螺儀一般都不會直接給你一個單位爲度/秒的值（除非它是個特殊的數字陀螺儀）。就像加速度計一樣，你會得到一個 ADC 值並且要用類似公式 2 的式子將其轉換成單位爲 度/秒的值。讓我們來介紹陀螺儀輸出值轉換中的
ADC 部分（假設使用 10 位 ADC 模塊，如果是 8 位 ADC，用 1023 代替 255，如果是 12 爲 ADC 用 4095 代替 1023）。
RateAxz = (AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity 公式 3 RateAyz = (AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity
AdcGyroXZ，AdcGyroYZ - 這兩個值由 ADC 讀取，它們分別代表矢量 R 的投影在
XZ 和 YZ 平面內裏的轉角，也可等價的說，旋轉可分解爲單獨繞 Y 和X 軸的運動。Vref – ADC 的參考電壓，上例中我們使用 3.3V
VzeroRate – 是零變化率電壓，換句話說它是陀螺儀不受任何轉動影響時的輸出值，對 Acc Gyro 板來說，可以認爲是 1.23V（此值通常可以在說明書中找到
——但千萬別相信這個值，因爲大多數的陀螺儀在焊接後會有一定的偏差，所以可以使用電壓計測量每個通道的輸出值，通常這個值在焊接後就不會改變，如果有跳動，在設備使用前寫一個校準程序對其進行測量，用戶應當在設備啓動的時候保持設備靜止以進行校準）。
Sensitivity –陀螺儀的靈敏度，單位 mV/(deg/s)，通常寫作 mV/deg/s,它的意思就是如果旋轉速度增加 1°/s,陀螺儀的輸出就會增加多少 mV。Acc_Gyro 板的靈敏度值是 2mV/deg/s 或 0.002V/deg/s
讓我們舉個例子，假設我們的 ADC 模塊返回以下值： AdcGyroXZ = 571
AdcGyroXZ = 323
用上面的公式，在代入 Acc Gyro 板的參數，可得：
RateAxz = (571 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s) =~ 306 deg/s RateAyz = (323 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s) =~ -94 deg/s 換句話說設備繞 Y 軸（也可以說在 XZ 平面內）以 306°/s 速度和繞 X 軸（或者說 YZ 平面內）以-94°/s 的速度旋轉。請注意，負號表示該設備朝着反方向旋轉。按照慣例，一個方向的旋轉是正值。一份好的陀螺儀說明書會告訴你哪個方向是正的，否則你就要自己測試出哪個旋轉方向會使得輸出腳電壓增加。最好使用示波器進行測試，因爲一旦你停止了旋轉，電壓就會掉回零速率水平。如果你使用的是萬用表，你得保持一定的旋轉速度幾秒鐘並同時比較電壓值和零速率電壓值。如果值大於零速率電壓值那說明這個旋轉方向是正向。
第三部分 將它們綜合起來。融合加速度計和陀螺儀的數據
如果你在閱讀這篇文章你可能已經有了或準備購買一個 IMU 設備，或者你準備用獨立的加速度計和陀螺儀搭建一個。
在使用整合了加速度計和陀螺儀的 IMU 設備時，首先要做的就是統一它們的座標系。最簡單的辦法就是將加速度計作爲參考座標系。大多數的加速度計技術說明書都會指出對應於物理芯片或設備的 XZY 軸方向。例如，下面就是 Acc Gyro 板
接下來的步驟是：
- 確定陀螺儀的輸出對應到上述討論的 RateAxz，RateAyz 值。
- 根據陀螺儀和加速度計的位置決定是否要反轉輸出值
不要設想陀螺儀陀的輸出有 XY，它會適應加速度計座標系裏的任何軸，儘管這個輸出是 IMU 模塊的一部分。最好的辦法就是測試。
接下來的示例用來確定哪個陀螺儀的輸出對應 RateAxz。
- 首先將設備保持水平。加速度計的 XY 軸輸出會是零加速度電壓（Acc Gyro 板的值是 1.65V）
- 接下來將設備繞 Y 軸旋轉，換句話說就是將設備在 XZ 平面內旋轉，所以 X、Z 的加速度輸出值會變化而 Y 軸保持不變。
-當以勻速旋轉設備的時候，注意陀螺儀的哪個通道輸出值變化了，其他輸出應該保持不變。
- 在陀螺儀繞Y 軸旋轉（在 XZ 平面內旋轉）的時候輸出值變化的就是 AdcGyroXZ， 用於計算 RateAx
-後一步，確認旋轉的方向是否和我們的模型對應，因爲陀螺儀和加速度的位置關係，有時候你可能要把 RateAxz 值反向
-重複上面的測試，將設備繞 Y 軸旋轉，這次查看加速度計的 X 軸輸出（也就是
AdcRx）。如果 AdcRx 增大（從水平位置開始旋轉的第一個 90°），那 AdcGyroXZ 應當減小。這是因爲我們觀察的是重力矢量，當設備朝一個方向旋轉時矢量會朝相反的方向旋轉（相對座標系運動）。所以，如果你不想反轉 RateAxz，你可以在公式 3 中引入正負號來解決這個問題：
RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity , 其中 InvertAxz= 1 或-1
同樣的方法可以用來測試 RateAyz，將設備繞 X 軸旋轉，你就能測出陀螺儀的哪個輸出對應於 RateAyz，以及它是否需要反轉。一旦你確定了 InvertAyz，你就能可以用下面的公式來計算 RateAyz：
RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity 如果對 Acc Gyro 板進行這些測試，你會得到下面的這些結果：
- RateAxz 的輸出管腳是 GX4，InvertAxz = 1
- RateAyz 輸出管腳是 GY4，InvertAyz = 1
從現在開始我們認爲你已經設置好了 IMU 模塊並能計算出正確的 Axr，Ayr，Azr 值（在第一部分加速度計中定義）以及 RateAyz，RateAyz（在第二部分陀螺儀中）。下一步，我們分析這些值之間的關係並得到更準確的設備和地平面之間的傾角。
你可能會問自己一個問題，如果加速度計已經告訴我們 Axr，Ayr，Azr 的傾角， 爲什麼還要費事去得到陀螺儀的數據？答案很簡單：加速度計的數據不是 100% 準確的。有幾個原因，還記加速度計測量的是慣性力，這個力可以由重力引起（理想情況只受重力影響），當也可能由設備的加速度（運動）引起。因此，就算加速度計處於一個相對比較平穩的狀態，它對一般的震動和機械噪聲很敏感。這就是爲什麼大部分的 IMU 系統都需要陀螺儀來使加速度計的輸出更平滑。但是怎麼辦到這點呢？陀螺儀不受噪聲影響嗎？
陀螺儀也會有噪聲，但由於它檢測的是旋轉，因此對線性機械運動沒那麼敏感， 不過陀螺儀有另外一種問題，比如漂移（當選擇停止的時候電壓不會回到零速率電壓）。然而，通過計算加速度計和陀螺儀的平均值我們能得到一個相對更準確的當前設備的傾角值，這比單獨使用加速度計更好。
接下來的步驟我會介紹一種算法，算法受卡爾曼濾波中的一些思想啓發，但是它更簡單並且更容易在嵌入式設備中實現。在此之前，讓我們先看看我們需要算法計算什麼值。所要算的就是重力矢量 R=[Rx,Ry,Rz]，它可由其他值推導出來， 如 Axr，Ayr，Azr 或者 cosX，cosY，cosZ，由這些值我們能得到設備相對地平面的傾角值，這些關係我們在第一部分已經討論過。有人可能會說-根據第一部分的公式 2 我們不是已經得到 Rx，Ry，Rz 的值了嗎？是的，但是記住，這些值只是由加速度計數據推導出來的，如果你直接將它們用於你的程序你會得到難以忍受的噪聲。爲了避免進一步的混亂，我們重新定義加速度計的測量值：
Racc – 是由加速度計測量到得慣性力矢量，它可分解爲下面的分量（在 XYZ 軸上的投影）：
RxAcc = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity RyAcc = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity
RzAcc = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity
現在我們得到了一組只來自於加速度計 ADC 的值。我們把這組數據叫做“vector”，並使用下面的符號：
Racc = [RxAcc,RyAcc,RzAcc]
因爲這些 Racc 的分量可由加速度計數據得到，我們可以把它當做算法的輸入。請注意Racc 測量的是重力，如果你得到的矢量長度約等於1g 那麼你就是正確的：
|Racc| = SQRT(RxAcc^2 +RyAcc^2 + RzAcc^2), 但是請確定把矢量轉換成下面的矢量非常重要：
Racc(normalized) = [RxAcc/|Racc| , RyAcc/|Racc| , RzAcc/|Racc|]. 這可以確保標準化 Racc 始終是 1。
接來下我們引進一個新的向量： Rest = [RxEst,RyEst,RzEst]
這就是算法的輸出值，它經過陀螺儀數據的修正和基於上一次估算的值。這是算法所做的事：
-加速度計告訴我們：“你現在的位置是 Racc” 我們回答：“謝謝，但讓我確認一下”
-然後根據陀螺儀的數據和上一次的 Rest 值修正這個值並輸出新的估算值 Rest。
-我們認爲 Rest 是當前設備姿態的“最佳值”。讓我們看看它是怎麼實現的。
數列的開始，我們先認爲加速度值正確並賦值： Rest(0) = Racc(0)
Rest 和 Racc 是向量，所以上面的式子可以用 3 個簡單的式子代替，注意別重複了：
RxEst（0）= RxAcc（0） RyEst（0）= RyAcc（0） RzEst（0）= RzAcc（0）
接下來我們在每個等時間間隔 T 秒做一次測量，得到新的測量值，並定義爲 Racc
（1），Racc（2），Racc（3）等等。同時，在每個時間間隔我們也計算出新的估算值 Rest（1）,Rest（2），Rest（3），等等。
假設我們在第 n 步。我們有兩列已知的值可以用： Rest(n-1) – 前一個估算值，Rest(0) = Racc(0)
Racc(n) – 當前加速度計測量值
在計算 Rest（n）前，我們先引進一個新的值，它可由陀螺儀和前一個估算值得到。
叫做 Rgyro，同樣它是個矢量並由 3 個分量組成： Rgyro = [RxGyro,RyGyro,RzGyro
我們分別計算這個矢量的分量，從 RxGyro 開始。
首先觀察陀螺儀模型中下面的關係，根據由 Rz 和 Rxz 組成的直角三角形我們能推出：
tan(Axz) = Rx/Rz => Axz = atan2(Rx,Rz)
你可能從未用過 atan2 這個函數，它和 atan 類似，但 atan 返回值範圍是
（-PI/2,PI/2），atan2 返回值範圍是（-PI，PI），並且他有兩個參數。它能將 Rx，Rz 值轉換成 360°（-PI，PI）內的角度。
所以，知道了 RxEst（n-1）和 RzEst（n-1）我們發現： Axz(n-1) = atan2( RxEst(n-1) , RzEst(n-1) ).
記住，陀螺儀測量的是 Axz 角度變化率，因此，我們可以按如下方法估算新的角度 Axz（n）：
Axz(n) = Axz(n-1) + RateAxz(n) * T
請記住，RateAxz 可由陀螺儀 ADC 讀取得到。通過使用平均轉速可由得到一個更準確的公式：
RateAxzAvg =（RateAxz（N）+ RateAxz（N-1））/ 2 Axz(n) = Axz(n-1) + RateAxzAvg * T
同理可得：
Ayz(n) = Ayz(n-1) + RateAyz(n) * T
好了，現在我們有了 Axz（n），Ayz（n）。現在我們如何推導出 RxGyro/RyGyro? 根據公式 1 我們可以把 Rgyro 長度寫成下式：
| Rgyro | = SQRT（RxGyro ^ 2 + RyGyro ^ 2 + RzGyro ^ 2）
同時，因爲我們已經將 Racc 標準化，我們可以認爲它的長度是 1 並且旋轉後保持不變，所以寫成下面的方式相對比較安全：
| Rgyro | = 1
我們暫時採用更短的符號進行下面的計算： x =RxGyro , y=RyGyro, z=RzGyro
根據上面的關係可得：
x = x / 1 = x / SQRT(x^2+y^2+z^2) 分子分母同除以 SQRT（X ^ 2 + Z ^ 2）
x = ( x / SQRT(x^2 + z^2) ) / SQRT( (x^2 + y^2 + z^2) / (x^2 + z^2) ) 注意 x / SQRT(x^2 + z^2) = sin(Axz), 所以:
x = sin(Axz) / SQRT (1 + y^2 / (x^2 + z^2) ) 將 SQRT 內部分式的分子分母同乘以 z^2
x = sin(Axz) / SQRT (1 + y^2 * z ^2 / (z^2 * (x^2 + z^2)) )
注意 z / SQRT(x^2 + z^2) = cos(Axz)， y / z = tan(Ayz), 所以最後可得: x = sin(Axz) / SQRT (1 + cos(Axz)^2 * tan(Ayz)^2 )
替換成原來的符號可得：
RxGyro = sin(Axz(n)) / SQRT (1 + cos(Axz(n))^2 * tan(Ayz(n))^2 ) 同理可得：
RyGyro = sin(Ayz(n)) / SQRT (1 + cos(Ayz(n))^2 * tan(Axz(n))^2 )
提示：這個公式還可以更進一步簡化。分式兩邊同除以 sin（axz（你））可得：
RxGyro = 1 / SQRT (1/ sin(Axz(n))^2 + cos(Axz(n))^2 / sin(Axz(n))^2 * tan(Ayz(n))^2 )
RxGyro = 1 / SQRT (1/ sin(Axz(n))^2 + cot(Axz(n))^2 * sin(Ayz(n))^2 / cos(Ayz(n))^2 )
現在加減 cos(Axz(n))^2/sin(Axz(n))^2 = cot(Axz(n))^2 RxGyro = 1 / SQRT (1/
sin(Axz(n))^2 - cos(Axz(n))^2/sin(Axz(n))^2 + cot(Axz(n))^2 * sin(Ayz(n))^2 / cos(Ayz(n))^2 + cot(Axz(n))^2 )
綜合條件 1、2 和 3、4 可得：
這個公式只用了 2 個三角函數並且計算量更低。如果你有 Mathematica 程序，通過使用 FullSimplify [Sin[A]^2/ ( 1 + Cos[A]^2 * Tan[B]^2)]你可以驗證這個公式。
現在我們發現：
RzGyro = Sign(RzGyro)*SQRT(1 – RxGyro^2 – RyGyro^2).
其中，當 RzGyro>=0 時，Sign(RzGyro) = 1 , 當 RzGyro<0 時，Sign(RzGyro) =-1 。
一 個 簡 單 的 估 算 方 法 ： Sign(RzGyro) = Sign(RzEst(n-1))
在實際應用中，當心 RzEst（n-1）趨近於 0。這時候你可以跳過整個陀螺儀階段並賦值：Rgyro=Rest（n-1）。Rz 可以用作計算 Axz 和 Ayz 傾角的參考，當它趨近於 0 時，它可能會溢出並引發不好的後果。這時你會得到很大的浮點數據，並且 tan()/atan()函數得到的結果會缺乏精度。
現在我們回顧一下已經得到的結果，我們在算法中的第 n 步，並計算出了下面的值：
Racc – 加速度計讀取的當前值
Rgyro –根據 Rest(-1)和當前陀螺儀讀取值所得
我們根據哪個值來更新 Rest（n）呢？你可能已經猜到，兩者都採用。我們會用一個加權平均值，得：
Rest(n) = (Racc * w1 + Rgyro * w2 ) / (w1 + w2) 分子分母同除以 w1，公式可簡化成：
Rest(n) = (Racc * w1/w1 + Rgyro * w2/w1 ) / (w1/w1 + w2/w1) 令 w2=w1=wGyro，可得：
Rest(n) = (Racc + Rgyro * wGyro ) / (1 + wGyro)
在上面的公式中，wGyro 表示我們對加速度計和陀螺儀的相信程度。這個值可以
通過測試確定，根據經驗值 5-20 之間會得到一個很好的結果。
此算法和卡爾曼濾波最主要的差別是它的權重是相對固定的，而卡爾曼濾波中的權重會隨着加速度計讀取的噪聲而改變。卡爾曼濾波注重給你一個“最好”的理論結果，而此算法給你的是實際項目中“夠用”的結果。你可以實現一個算法， 它能根據測量的噪聲而改變 wGyro 值，但對大部分應用來說固定的權重也能工作的很好。
現在得到最新的估算值還差一步：
RxEst(n) = (RxAcc + RxGyro * wGyro ) / (1 + wGyro) RyEst(n) = (RyAcc + RyGyro * wGyro ) / (1 + wGyro) RzEst(n) = (RzAcc + RzGyro * wGyro ) / (1 + wGyro) 現在，再次標準化矢量：
R = SQRT(RxEst(n) ^2 + RyEst(n)^2 + RzEst(n)^2 ) RxEst(n) = RxEst(n)/R
RyEst(n) = RyEst(n)/R RzEst(n) = RzEst(n)/R
現在，可以再次進行下一輪循環了。