卡爾曼濾波

為了將sensor量測的結果計算出真正得四軸的姿態，研究了一陣子，看起來大部分的人卡爾曼綠波來做處理

目前study一下稍微有點概念，但實際要套用到四軸上，三軸資料同時運算，又要考慮效率，看起來要回去把大學的線性代數、矩陣運算重新複習一遍了

系統描述

• X(k)=A X(k-1)+B U(k)+W(k) 
• X(k): 這次的狀態
• A X(k-1): 上次的狀態經過A做轉換
• B U(k): 輸入的改變經過B做轉換
• W(k): 實際影響的誤差

系统的测量值：

• Z(k)=H X(k)+V(k) 
• Z(k) 測量值
• H X(k): 將X(k)轉換為測量值
• V(k) 為測量誤差

誤差定義

• Q: 量測的誤差(高斯分布，平均為0) <==固定
• 例如每次溫度計量出來跟實際溫度都會有+-0.5度的誤差，但多次平均後仍與實際溫度相同
• R: 隨著時間演變的誤差(高斯分布，平均為0) <==固定
• 假設系統理想上每隔一分鐘上升一度，但實際上可能有+-0.1度的誤差，但長期的趨勢平均還是每隔一分鐘上升一度

基本公式

1. X(k|k-1)=A X(k-1|k-1)+B U(k)
2. P(k|k-1)=A P(k-1|k-1) A’+Q
3. X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) 
4. Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) 
5. P(k|k)=（I-Kg(k) H）P(k|k-1)

基本概念在這個網站上講解的非常清楚
http://www.geek-workshop.com/thread-1487-1-1.html

以溫度為例計算為例

1. X(k|k-1)=A X(k-1|k-1)+B U(k)
1. 這次推估的溫度X(k|k-1)等於上次預測的溫度X(k-1|k-1) 透過A轉換出來的值加上這次輸入的熱量造成的改變BU(k)做轉換(例如輸入U(k)100卡, 透過B轉換為溫度1度)
2. 這個例子來說A 跟B 都是1
1. 這次的推估溫度X(k|k-1)單位跟上次計算出的溫度X(k-1|k-1)單位相同，不用做轉換，所以A=1
2. 溫度計量到的值就是溫度，所以不用做轉換，B=1
2. P(k|k-1)=A P(k-1|k-1) A’+Q
1. 這次推估的溫度X(k|k-1)的推估誤差P(k|k-1)為上一段時間的誤差P(k-1|k-1)加上測量誤差Q
3. X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) 
1. 這次的計算結果等於推估溫度加上  這次卡爾曼增益Kg(k)* [測量值Z(k)與推估值H X(k|k-1)的誤差]
2. 這邊H=1
4. Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) 
1. 這次卡爾曼增益Kg(k) 為這次的推估誤差P(k|k-1)/(這次的推估誤差P(k|k-1) + 隨著時間演變的誤差R)
2. H'=H=1
5. P(k|k)=（I-Kg(k) H）P(k|k-1)
1. 這次實際的推估誤差為(1-這次卡爾曼增益Kg(k))*這次推估的誤差P(k|k-1)

參考
http://www.360doc.com/content/11/0506/14/3810344_114798067.shtml
http://www.geek-workshop.com/thread-15257-1-1.html
http://gcyrobot.blogspot.com/2012/08/simple-kalman-filter_29.html?m=1
http://jpkc.nwpu.edu.cn/jpkc2005/40/ebook/kcsj/chp12/12_7.htm