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

最新記事(inside out)へ  |   年と月を指定して記事を読む(クリック!)

2009年11月 を読む << 2009年12月 を読む >> 2010年1月 を読む

2009-12-31[n年前へ]

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

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

 動かし方は、

ruby kalman.rb 100
というようにでも起動すると、「出力」→「観測」→「予測」のカルマン・フィルタの動作を99回ほど繰り返す、という具合です。出力結果例は、また後ほど示すころにしますが、開始後およそ5回程度からは、「真の出力値」に近い予測を行うことができています。

 (最後に配列を使ってグラフ化などをするわけでもないことと、一回前のデータ以外は不必要だということがわかりにくくなってしまいそうだったので)本来は不要な配列を使いたくありませんでした。ただ、変数が(一見)増えたように見えるのもどうかと思い、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