# カルマンフィルタ jun hirabayashi jun@hirax.net
# http://www.hirax.net
# How to use:
# Ruby simpleKalman.rb operationNum
# ex.
# Ruby simpleKalman.rb 50

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) # 出力値生成（状態方程式）
  y[k]=x[k]                    # 観測値生成（出力方程式）
  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