hirax.net::inside out::2009年12月31日

■Rubyで単純なカルマンフィルタを書いてみた　

　An Introduction to the Kalman Filterの一定出力プラント例に対するカルマンフィルタをPythonで実装した Cookbook / KalmanFilteringを参考に、Rubyで単純なカルマンフィルタを適当に書いてみました。

　動かし方は、

ruby kalman.rb 100

　（最後に配列を使ってグラフ化などをするわけでもないことと、一回前のデータ以外は不必要だということがわかりにくくなってしまいそうだったので）本来は不要な配列を使いたくありませんでした。ただ、変数が（一見）増えたように見えるのもどうかと思い、Python例と同じように配列を使った実装にしてしまいました。

　一応、実際に動作する際に「発生すること」「処理すること」の「手順・ループ」を意識しながら、コメント文を書いてみました（正式な用語とは異なるものも多いかもしれませんが）。

　ただし、今回の「一定出力」の例では、「状態方程式」「出力方程式」が登場しないので、そういう拡張をすることを考えた場合には、変数名が適切でないような気がします。そういった辺りのことは、また次の機会に整理し直すことにします。さて、これで、今年を終わりにしたいと思います。

　というわけで、下記がRubyソースになります。

# 簡単なカルマンフィルタRuby実装例

def gaussian(n) # 正規分布作成用適当関数
   sum = 0.0
   ((n*12).to_i).times{ sum += rand() }
   return sum-(12/2*n)
end

operationNum=ARGV[0].to_i   # Operation Num

x=[]               # 出力の真の値
z=[]               # 測定値
xhat=[]           # 事後出力推定値
xhatMinus=[]   # 事前出力推定値
p=[]               # 事後誤差推定値
pMinus=[]       # 事前誤差推定値
kG=[]             # カルマンゲイン

xVall=55.5       # 出力の真の値
q=1.0              # 出力自身が持つノイズの分散
r=2.0              # 観測における誤差の分散
xhat＜＜20          # 初期出力推定値
p＜＜0               # 初期誤差推定値

puts 'k,x,z,xhat,kG'
1.upto(operationNum) do |k|
  # DoProcess
  x[k]=xVall+gaussian(q) # 出力値生成
  z[k]=x[k]+gaussian(r)           # 測定値生成
  
  # Time Updade ("Predict")
  xhatMinus[k]=xhat[k-1] # 状態予測を進める
  pMinus[k]=p[k-1]+q   # 誤差共分散計算を進める
  
  # Measurement Update("Correct")
  kG[k] = pMinus[k]/( pMinus[k]+r) # カルマンゲイン算出
  xhat[k] =xhatMinus[k]+ #測定値を用い事後出力推定値算出
       kG[k]*(z[k]-xhatMinus[k]) 
  p[k] = (1-kG[k])*pMinus[k]  # 事後誤差算出

  # この回のオペレーションでの結果を表示
  puts [k,x[k],z[k],xhat[k],kG[k]].join(',')  if k＞1
end