2軸加速度計：
- 測量“加速度”，實際上是每單位質量所受到的力（F=ma,所以a=F/m）；
- 可以用來測量重力，如上圖所示，X軸爲0，Y軸爲-1g；
- 可以用來測傾角：
X軸方向有了重力的分量，左側是正分量，右側是負分量。Y方向上比重力小了一些。
Y方向有有用的信息嗎？可能並沒有：
a)在角度信息上來說，Y方向上沒有X方向上敏感；
b)不能反映傾角的方向。
陀螺儀：
- 測量角速度（旋轉的速度）
- 靜止的時候爲0
- 旋轉的時候爲正或者負
左側爲正，右側爲負。
從傳感器中讀取數據
首先是讀取每一個傳感器的模擬輸入（通過模數轉換器ADC）並把它們變成有用的單位。這需要兩個量補償和比例：
1、補償很容易得到：觀察水平或者靜止時候的傳感器值，如果這個值有噪聲，取平均值。這個補償應該是一個
有符號的整型變量（或者常量）。（儘管ADC的值和補償都不是負的，但是它們將會做減法，所以現在讓它是一
個有符號變量是沒有大礙的。）
2、比例取決於傳感器：它是一個爲了得到期望單位的乘法因子（陀螺儀每秒的單位可以是度也可以是弧度，只
要單位統一就好）。這可以在數據手冊中找到或者通過實驗確定，有時候稱之爲常量、增益或者敏感度。這個比
例應該是一個浮點型變量或者常量。
關於加速度計的更多…
如果需要對360的旋轉進行角度測量估計，而這是必要的，Y軸的測量是有用的但是並非必須。有它，我們可以使用三
角函數來求出兩個軸的反正切並計算出角度；沒有它，我們仍然可以使用sin或者cos找出和X軸之間的夾角，因爲我們知道
重力的大小。但是三角函數的計算需要大量的處理時間，而且是非線性的，所以能夠避免不用就應該避免。
對於一個平衡平臺來說，最重要的需要測量的角度幾乎是接近垂直的。因爲如果平臺向任何方向傾斜超過30度，處理
器除了盡全力去試圖扳回它外，可能幹不了什麼了。所以在這個範圍內，我們可以採用小角度近似而且計算X軸方向可以
減輕處理器負擔和代碼複雜程度。
假設平臺向某個方向傾斜，角度爲Θ，保持靜止（沒有垂直方向上的加速度）
X軸方向：1g*sinΘ
小角度近似：sinΘ=Θ（弧度值）
在5%範圍內（相當於Θ=±π/6=±30°），效果良好。
所以有如下代碼：
x_acc=(float)(x_acc_ADC-x_acc_offset)*x_acc_scale;
且如果x_acc_scale是比例縮放到1g，同時x軸方向爲垂直向下，x_acc將是以弧度做單位的角度值。如果想要得到以度爲單
位的角度，x_acc_scale應該乘以180/π。
理想的測量
爲了控制平臺，能得到它的角度和角速度將是最好的。這是角度的PD（比例微分）控制的基礎，而PD對於這樣的系
統來說效果良好。
電機輸出=Kp×（角度）+Kd×（角速度）
電機輸出具體是怎樣的，這裏就不闡述了。但是通常來說控制程序可以通過Kp和Kd的調節得到穩定平滑的性能效果。
不會產生純比例控制器那樣的過沖。（比如說如果角度爲正，而角速度爲負，在返回水平位置的過程中，電機會提前減
速。）
從效果上來說，PD控制器就像加上了一個可調彈簧和阻尼器。
傳感器圖
最好的方法是？
最明顯的方法
優點：
- 直觀
- 編程簡單
- 陀螺儀反應迅速，可準確快速的角速度測量
缺點：
- 噪聲
- 任何水平方向上的加速度的變化都會影響角度（想象平臺是水平的，但是電機讓它向前運動，加速度計將
- 不能把它和重力分開。）
優點：
- 依然直觀
- 依然編程方便
- 濾波器濾去高頻的水平加速度（噪聲），只有低頻的加速度（重力）可以輸出
缺點：
- 角度的測量由於取平均將會出現延遲，濾波越多，延時越長。延時將會對穩定性產生影響。
單傳感器方法
優點：
- 只有讀取一個傳感器
- 快速，延遲不是問題
- 不受水平加速度的影響
- 依然編程易實現
缺點：
- 嚴重的陀螺儀漂移。如果陀螺儀靜止的時候沒有準確的校零（確實不會），它將會慢慢的漂移直到誤差越
- 來越大。
卡爾曼濾波
優點：
- 理想的濾波器可以融合帶有噪聲的傳感器，並得到乾淨準確的估計
- 考慮已知系統的物理屬性（質量、慣性等等）
缺點：
- 我不知道它是如何工作的，數學上很複雜，需要一些線性代數的知識。而且不同的情況有不同的形式
- 可能很難編程
- 增加處理器負擔
互補濾波器
優點：
- 有助於解決噪聲、漂移和水平方向上的加速度分離的問題
- 快速估計，比單獨的低通濾波快
- 不是很耗處理器時間
缺點：
- 相對於簡單的濾波器，這裏有一點理論，但是對於卡爾曼濾波器來說，什麼都不是
在數字濾波器這方面有大量的理論，大多數我都不懂。沒有理論基礎（Z域的變換，如果你想多看看的話），但
是基本的概念還是很簡單的。
積分：這個很簡單。想象一個以已知速度運行的汽車，你的程序就像一個一毫秒數一次的時鐘。每次爲了得到新
的距離，你在上一次的距離上加上改變量。改變量等於車速乘以從上次到現在的時間，這個時間你可以從控制器
上得到或者來自其他已知的定時器。
距離+=速度*dt;對於我們的平臺來說是，angle+=gyro*dt;
低通濾波器：低通濾波器的目標只是爲了讓低頻的改變通過，濾掉高頻的噪聲波動。實現它的一種方法是在程序
的循環中一點一點的加。
angle=0.98*angle+0.02*x_acc;
比如，如果角度從零開始改變，加速度的值突然跳到10°，在隨後的迭代過程中，角度的估計將會如下表所示
如果角度保持在10°，這個角度的估計將會上升到和這個值持平，到達這個值的時間取決於濾波器常數（例子中的
0.98和0.02）和循環中的採樣率dt
高通濾波器：相對於低通濾波器來說，理解上要難一些，但是理論上來說它倆是完全相反的：它允許高頻信號通過，
濾去一直保持不變的信號，這可以有效的解決漂移的問題。
採樣週期：每個循環的時間。如果採樣頻率是100Hz，採樣週期是0.01秒。
時間常數：濾波器的時間常數和濾波信號的持續時間相關。對於低通濾波器來說，比時間常數更長的信號將會保持
不變，而比時間常數短的信號將會被濾掉。高通濾波器完全相反。一個數字低通濾波器的常數τ：
y=a*y+(1-a)*x;
以採樣週期dt運行的循環，如下：
所以如果已知期望的時間常數和採樣頻率，可以得到濾波器係數a。
互補：這只是說濾波器有兩部分組成，以得到準確線性的估計。在閱讀相關的更多的東西之後，我認爲這裏的濾波
器不僅僅是互補，在時間常數比採樣頻率更大的時候，這是一個很好的近似。
細瞥角度互補濾波器
如果這個濾波器以每秒執行100次的循環中運行，低通高通濾波器的時間常數都將會是
這定義了信任陀螺儀和加速度計的界限。對於時間週期小於半秒，陀螺儀的積分將會優先，水平加速度的噪
聲將會被濾去。對於時間週期長於半秒，加速度的均值相對於陀螺儀來說佔得比重更大一些，這個時候可能會有
一些漂移。
對於絕大部分來說，設計濾波器通常以另一種方式進行。首先，取一個時間常數，然後用它來計算濾波器系
數。取得這個時間常數應可以滿足系統的響應。如果陀螺儀每秒鐘漂移2°（可能是非常糟糕的估計了），時間
常數可能需要小於一秒，以保證每秒鐘在任何方向上漂移度數不會超過兩度。但是時間常數越低，允許水平方向
加速度噪聲通過的就越多。就像大多數控制算法面臨的狀況一樣，這裏有一個平衡點，唯一的辦法就是實驗調試。
爲了選擇正確的濾波器係數，記住這個採樣頻率是很重要的。如果修改程序，加上了更多的浮點運算，採樣
頻率將要下降兩倍，時間常數應該上升兩倍，除非你重新計算濾波器係數。
舉一個例子，考慮使用26.2ms的速度更新控制週期（可能有點慢，但是還能用）。如果想要得到0.75s的時間
常數，濾波器係數應該是
所以有
第二個濾波器參數爲1-0.966=0.034.
濾波器裏面的陀螺儀沒有準確校零的問題值得探討一下。它完全不會導致漂移的問題，但是會影響角度的計算
。舉個例子，我們錯誤地選擇了一個不正確的補償，陀螺儀靜止的時候，得到的值是5°/s。在數學上可以證明（這
裏就不證明了），這在角度估計的影響就是補償率乘以時間常數。所以如果我們取0.75s的時間常數，將會得到3.75°
的角度補償常量。
除了這個非常糟糕的情況（陀螺儀永遠不會需要那麼大的補償），角度補償常量比漂移角度補償簡單得多。比如你可
以反方向旋轉3.75°來解決這個問題。
理論足夠了，是時候看看實驗的效
果了。
*http://www.machinescience.org
採樣頻率：79Hz
濾波器係數：0.98和0.02
時間常數：0.62s
注意濾波器是如何處理這兩個問題的：在不旋轉的時候水平方向上的加速度干擾（藍色高亮）和陀螺儀漂移
（紅色高亮）
採樣頻率：84Hz
濾波器係數：0.98和0.02
時間常數：0.58s
這裏有兩點值得注意：第一，沒有考慮到的啓動的問題（藍色高亮），這將發生在你沒有合適的初始化你的變量
時。長的時間常數意味着開始幾秒是不確定的，這很容易解決：確保重要的變量都準確的初始化了（0或者其他
應該是的值）；第二，注意苛刻的陀螺儀補償（紅色高亮），大約6°/s，注意是如何在角度估計裏面產生角度
補償常量的。補償的角度約等於陀螺儀補償乘以時間常數。這是一個很好的最壞情況分析。
總結
我認爲這個濾波器很適合DIY平衡解決方案，理由如下：
- 效果不錯。角度估計是可靠而準確的，沒有水平加速度干擾問題和漂移問題。
- 處理器友好性。只需要很小的浮點運算量，稍微修改一下代碼即可應用到你的程序中。很容易實現控制周
- 期達到100Hz。
- 它更加的直觀，理論也更加容易闡釋相對於替代品卡爾曼濾波來說。This might not have anything to
- do with how well it works, but in educational
programslike FIRST, it can be an advantage.（在入門教育中這很有優勢）。
在我說對於平衡平臺是一個完美的解決方案之前，我很看看他在一些硬件平臺上的測試，比如DIY Segway？
我不確定它應用到水平定位上表現如何。我不太懷疑：沒有重力，加速度計不能給絕對參考，，當然可以積
分兩次以得到位置估計，但是漂移會很嚴重。儘管濾波的技術可以用不同的傳感器互相補償（可能是加速度計和
編碼器），但是工況是類似的（編碼器不是絕對位置的設備，如果輪子失去牽引力，也會漂移）。使用水平位置
的一個更好的類比是使用GPS做長時間的估計，慣性傳感器做短期集成。