単純な場合のカルマンフィルタ実装例

• 参考URL
  • 「An Introduction to The Kalman Filter」 http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
  • 「Pythonでの実装」 http://www.scipy.org/Cookbook/KalmanFiltering
• やること
  • 「ある値xを観測したz(zは観測誤差を含む)を使って、真の値であるxを推定する」
• 上のURLにあるソースコード的なもの

import numpy
import pylab

#初期パラメータ
n = 300 #反復計算回数
sz = (n,) #配列のサイズ
x = -0.23456 #真の値
z = numpy.random.normal(x,0.1,size=sz) #ガウス雑音を含む観測値を作成
R = 1e-5 #状態遷移誤差(分散)
Q = 0.1**2 #観測誤差(分散)

#各状態を保持する配列(0で埋める)
mu_hat = numpy.zeros(sz) #予想のxの推定値
sigma_hat = numpy.zeros(sz) #予想の誤差の推定
mu = numpy.zeros(sz) #計測後のxの推定値
sigma = numpy.zeros(sz) #計測後の誤差の推定
K = numpy.zeros(sz) #カルマンゲイン

#初期値
mu[0] = 0.0
sigma[0] = 1.0
#反復計算
for k in range(1,n):
    #予想
    mu_hat[k] = mu[k-1]
    sigma_hat[k] = sigma[k-1]+R

    #修正
    K[k] = sigma_hat[k]/(sigma_hat[k]+Q)
    mu[k] = mu_hat[k] + K[k]*(z[k]-mu_hat[k])
    sigma[k] = (1-K[k])*sigma_hat[k]

#プロット
pylab.figure()
pylab.plot(z,'k+',label='Measurement value (with Noise)')
pylab.plot(mu,'b-',label='Estimate value')
pylab.axhline(x,color='g',label='Truth value')
pylab.legend()
pylab.xlabel('Iteration')
pylab.ylabel('Value')
pylab.show()