2014年2月26日 星期三

卡爾曼濾波

為了將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

沒有留言:

張貼留言