2016年2月18日
傳感器的原理
加速度計：
加速度計---我們可以把它想作一個圓球在一個方盒子中。
假定這個盒子不在重力場中或者其他任何會影響球的位置的場中，球處于盒子的正中央。
你可以想象盒子在外太空中，或遠在航天飛機中，離任何天體，一切東西都處于無重力狀态。
在圖中你可以看到我們給每個軸配置設定了一對牆（我們移除了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)
我們花了大段的篇幅來解釋加速度計模型，最後所要的隻是以上這幾個公式。
根據你的應用場合，你可能會用到我們推導出來的幾個過渡公式。
加速度傳感器可以用來測量加速度，或者檢測傾斜、沖擊、振動等運動狀态，
幫助實作工業、醫療、通信、消費電子和汽車等領域中的多種應用。
根據不同的應用，加速度傳感器的測量範圍從幾g 到幾十g 不等。
數字輸出的加速度傳感器還會內建多種中斷模式。
這些特性可以為使用者提供更加友善靈活的解決方案。
陀螺儀：
接下來要介紹陀螺儀子產品。
但在此之前，我們再介紹幾個很常用的公式：
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的速度旋轉。
請注意，負号表示該裝置朝着反方向旋轉。按照慣例，一個方向的旋轉是正值。一份好的陀螺儀說明書會告訴你哪個方向是正的，否則你就要自己測試出哪個旋轉方向會使得輸出腳電壓增加。最好使用示波器進行測試，因為一旦你停止了旋轉，電壓就會掉回零速率水準。如果你使用的是萬用表，你得保持一定的旋轉速度幾秒鐘并同時比較電壓值和零速率電壓值。如果值大于零速率電壓值那說明這個旋轉方向是正向。
加速度計和陀螺儀的資料融合算法：
注：具體的代碼實作和算法測試，請閱讀這篇文章： http://starlino.com/imu_kalman_arduino.html
融合加速度計和陀螺儀時，首先要做的就是統一它們的坐标系。
最簡單的辦法就是将加速度計作為參考坐标系。
大多數的加速度計規格書都會指出對應于實體晶片或裝置的XZY軸方向。
例如，下面就是Acc Gyro闆的說明書中給出的XYZ軸方向：
接下來的步驟是：
- 确定陀螺儀的輸出對應到上述讨論的RateAxz，RateAyz值。
- 根據陀螺儀和加速度計的位置決定是否要反轉輸出值
不要設想陀螺儀陀的輸出有XY，它會适應加速度計坐标系裡的任何軸，盡管這個輸出是IMU子產品的一部分。最好的辦法就是測試。
接下來的示例用來确定哪個陀螺儀的輸出對應RateAxz。
- 首先将裝置保持水準。加速度計的XY軸輸出會是零加速度電壓（Acc Gyro闆的值是1.65V）
- 接下來将裝置繞Y軸旋轉，換句話說就是将裝置在XZ平面内旋轉，是以X、Z的加速度輸出值會變化而Y軸保持不變。
- 當以勻速旋轉裝置的時候，注意陀螺儀的哪個通道輸出值變化了，其他輸出應該保持不變。
- 在陀螺儀繞Y軸旋轉（在XZ平面内旋轉）的時候輸出值變化的就是AdcGyroXZ，用于計算RateAxz
- 最後一步，确認旋轉的方向是否和我們的模型對應，因為陀螺儀和加速度的位置關系，有時候你可能要把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）内的角度。更多資訊請閱讀 atan2.
是以，知道了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可得：
RxGyro = 1 / SQRT (1 + cot(Axz(n))^2 * sec(Ayz(n))^2 ), 其中 cot(x) = 1 / tan(x) ， sec(x) = 1 / cos(x)
這個公式隻用了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
現在，可以再次進行下一輪循環了。
注：關于此算法的具體實作和測試，請閱讀這篇文章：
http://starlino.com/imu_kalman_arduino.html
加速度計和陀螺儀IMU融合的其他資源：
http://www.mikroquad.com/pub/Res ... ryFilter/filter.pdf
http://stackoverflow.com/questio ... -accelerometer-data
http://www.dimensionengineering.com/accelerometers.htm
2016年2月17日
傳感器的使用與濾波算法（資料處理）的關系
誤差或錯誤資料的來源
1[2013]直立位置的角度值和在倒下去或傾斜的時候的快慢值（角速度），其實他們是一個量，因為對角速度積分就是角度值。
角速度隻是反映了倒下去或傾斜的快慢，即變化量。
好比你拿一根筷子直立在手指上，你看見它要倒下去的時候，手肯定會跟着移動；
你一方面看到的是筷子倒下去的角度，另一個是筷子倒的時候的快慢，角度大速度快，你自然移動地快、幅度也大，角度小、速度慢，你自然移動的慢幅度也小。
測量角度一般使用加速度計就可以了，加速度計是分為模拟的和數字的兩種，都是可以使用的。
在實際情況中，加速度計測量的角度是不準确的，因為在運動過程中存在震動加速度，這會使輸出值不準确，不能真實反映偏轉角度。
這是電子元器件本身的問題，有些人說用簡單的數字濾波（中值、均值等），這些濾波濾除的是幹擾信号，這信号本身的錯誤怎麼濾除？
再來考慮另一個器件陀螺儀，我們知道陀螺儀是測量角速度的，但是角速度換算成角度是需要一個積分過程，假如在輸入時有一個極小的誤差，那麼這個誤差随着積分将會越來越大，最後得出的角度自然也是不準确的。（元器件的使用是最基本知識，需要把這兩個傳感器的使用先搞明白，明白角度具體是怎麼計算出來的？）
濾波算法（資料處理）
這個時候才有我們常說的卡爾曼濾波、互補濾波的登場，很多人在設計過程中總是覺得卡爾曼或者互補濾波是很高端的東西，視線全被它們蒙蔽了，實際上它們最終的目的仍然是得到最準确的角度偏離值。對于這一目标，傳感器的性能、電路設計同樣很重要的。
其實作為應用，我們隻需知道“卡爾曼濾波”輸入的兩個量，一個是測量值，一個是預測值，程式都是成型的，重點是在參數的調試上。整個算法中影響輸出的就是Kg的值，可以簡單的了解為一種權重行為，相信誰更多一點而已。整個調試過程有三個參數需要調整，Q R 及那個0.0235 。具體的調試，目前還說不清楚，往往算法的調試都是經驗，嘗試多了就有規律了，需要再把這種經驗給理論化簡單描述出來。
代碼如下：
說明：簡化版卡爾曼濾波
volatile float QingJiao = 0; //最終準确角度輸出變量定義
volatile float Gyro_Data = 0; //陀螺儀
float Q =1, R =3900; //調整卡爾曼的滞後 3900
static float RealData = 0,RealData_P =10000;
float NowData = 0,NowData_P =0 ;
float Kg = 0,gyroscope_rate = 0,gyroscope_rat = 0,accelerometer_angle;
volatile float gyroscope_angle=0 ; //用卡爾曼濾波時不用此變量
int Gyro1_zero=0;
void kalman_update(void)
{
if(zeroflag>1000) //與開機自檢有關，沒用到的可以删去
{zeroflag=1001; //確定zeroflag不會溢出
//-------------------------------------------------------------------------------------------------------------------------------
Acc_z = Acc_z - 28850; //加速度計采集的AD值減去直立時的輸出值
Gyro1_zero=zerosub/1000; //陀螺儀開機自檢累加1000次後取均值 得到陀螺儀零偏值
Gyro1 = Gyro1 - Gyro1_zero; //陀螺儀AD采集值減去陀螺儀零偏值
Gyro_Data = Gyro1;
accelerometer_angle= Acc_z*180/(47915.71-12843.7); //加速度計計算出的角度 歸一化到-90 到+90
gyroscope_rate = Gyro1*0.0235*0.005; //0.0235 是轉換角度的比例值 0.005是控制周期
gyroscope_rat =gyroscope_rat-Gyro1*0.0235*0.005; //積分角速度得到角度
//卡爾曼五個公式的算法實作
NowData = RealData-gyroscope_rate;
NowData_P = Q+RealData_P;
Kg = NowData_P/(NowData_P+R);
RealData = NowData +Kg*(accelerometer_angle - NowData);
RealData_P =(1-Kg)*NowData_P;
QingJiao = RealData; //将準确角度結果給QingJiao
}
}
假如已經得到準确角度，自然是開始以此作為控制量，那我們要控制成啥樣？想一想也知道是要把這個角度值控制成0度（例如：将直立時定義為0度），那麼自然使用常用的PID算法，偏差自然就是QingJiao-0=QingJiao，當然也可以反過來，這根據自己對方向的定義。我們來個最簡單的位置式PD算法：
fValue = (float) P *QingJiao -(float) D*Gyro_Data;
P就是PID的P參數 D就是PID的D參數，QingJiao反映幅度，Gyro_Data反映快慢。（這也需要不斷調試出來的。）再把fvalue值給平衡車的直流電機的控制調速PWM輸出就可以了。
實際在做的時候，往往沒那麼簡單，是以一定要一步一步做好之後再做後面的，假如你第二部沒做好，在第三部時你怎麼也直立不起來，你不知道到底是PD參數不對，還是卡爾曼出來的角度本身不準。是以個人經驗：得到準确角度是整個過程至關重要的一步。平衡車的直立是一直是一個動态過程，即使最好的狀态一動不動，也是在動态控制中，隻是看不出而已。這裡隻針對直立控制，即最基本的自平衡。
參考：
加速度計和陀螺儀指南 http://www.geek-workshop.com/thread-1695-1-1.html
A Guide To using IMU (Accelerometer and Gyroscope Devices) in Embedded Applications. Posted on: December 29, 2009 by starlino
http://www.starlino.com/imu_guide.html
6軸和9軸傳感器：慣性測量單元 和 航姿參考系統
IMU(Inertial measurement unit )和AHRS (Attitude and heading reference system)
2016年2月19日
在研究技術問題之前，我并不關心這些傳感器的細節，但要上新的産品、需要更新的技術的時候，市場上找人不是一件容易的事情。
與其等工程師都會的時候，可能你的産品也沒什麼競争力了。無奈，自己跟着參與吧。
就算少數人會使用新技術，你也需要有足夠的人際技巧，其實不能說是技巧，應該是一套方法論來團結你的盟友，比如共同的理想（對未來的預見）、協調一緻的目标、價值觀及一緻行動準則。這些遠遠要比程式設計設計複雜，有時候我在想馬雲真的是非常厲害的人物，能團結18羅漢在相當長的時間裡，聽他的”鬼話“，被他”忽悠“，這是需要怎樣的一套方法呢！想想當下，如果你要團結一批人，是非常不容易的。
言歸正傳，不管什麼IMU或AHRS對我來說都是傳感器數量和結構的問題，但要采用前人的成果，就得大家說一樣的話，就是這些專業術語。
AHRS由加速度計，磁場計，陀螺儀構成
AHRS的輸出中的絕對方向來自于地球的重力場和地球的磁場，尤其是磁場
靜态終精度取決于對磁場的測量精度和對重力的測量精度 ,而則陀螺決定了他的動态性能。
磁場和重力場越正交，則航姿測量效果越好；反之，如果磁場和重力場平行了，比如在地磁南北極，這裡的磁場是向下的，即和重量場方向相同了。
這時航向角是沒法測出的，這是航姿系統的缺陷。在高緯度的地方航線角誤差會越來越大。當然，在實際非航天級的應用中很少遇到這種情況，故此段隻是為了”知其是以然“可略去。
消費級陀螺儀和加速度計的噪聲相對來說很大，
以平面陀螺為例用ADI的陀螺儀進行積分一分鐘會漂移2度左右，這種前提下如果沒有磁場和重力場來校正三軸陀螺的話，
那麼基本上3分鐘以後物體的實際姿态和測量輸出姿态就完全變樣了，是以，低價陀螺儀和加速度計的架構下必須運用場向量來進行修正。
AHRS利用三維的陀螺儀來快速跟蹤被測物體的三維的姿态，它以陀螺儀為核心，同時也測量加速度和地磁場的方向為系統提供可靠的參考。
具體測量載體三個方向的的絕對角速率、加速度以及磁場強度，并采用特定姿态解算方法和卡爾曼濾波資訊融合得到載體的四元數、姿态資料等。
需要實時的內建算法為系統提供準确，可靠，及時以及穩定的姿态輸出。
從陀螺儀、加速度計、磁力計以及内部溫度傳感器得到的資料，全部被傳輸到嵌入式系統-MCU中。
MCU依據特定的算法以及存儲在Flash存儲器中的标定資料處理來自傳感器的原始資料，
作為其基本算法，航姿參考系統（AHRS）采用自适應卡爾曼濾波算法，自動調整并适應不斷變化的動态條件，無需外部人為的幹預。
産品均在特定的環境實驗條件下，參考已知溫度下的加速度、角速率和磁場進行全面的标定，并将标定資料輸入每個産品中。
産品應用：
船舶測控、工程機械、車輛、通訊天線和雷達、平台穩定系統、
機器人控制、無人機/直升機、虛拟現實、遊戲、動畫、體育、醫療康複等。
參考文獻：
四元數與歐拉角之間的轉換 http://blog.csdn.net/yaokang522/article/details/46789075
請教四軸AHRS算法的問題 http://www.openedv.com/posts/list/49740.htm
STM32可以用的卡爾曼濾波，附帶AHRS姿态解算源碼 http://bbs.elecfans.com/jishu_487795_1_1.html
AHRS--APM飛控基于DCM算法的姿态及航向解算核心代碼 http://www.docin.com/p-880458906.html
AHRS 姿态闆首試 http://www.amobbs.com/thread-5468886-5-1.html
The Extended Kalman Filter: An Interactive Tutorial for Non-Experts http://home.wlu.edu/~levys/kalman_tutorial/
捷聯慣導算法心得 http://www.amobbs.com/thread-5492189-1-1.html
9DOF姿态融合 四元數 歐拉角轉換 有代碼有内涵 http://www.amobbs.com/thread-5549022-1-1.html