カルマンフィルタ

足立先生の「カルマンフィルタの基礎」を勉強させていただきます。
初心者にとっては非常に良い本だと思ってます。

その中で、例題プログラムがあり、MATLABなのですが、
ビンボーなため、持っておりません。

そこでpythonで書いてみました。
最初の例題は、スカラー変数なので、若干弄っております。
コメントはしません。
色々おこられそうなので。。。

本当に勉強になります。

import numpy as np

import pylab as pl

import random

def kf(A,B,Bu,C,Q,R,u,y,xhat,P):

     xhatm = A*xhat+Bu*u

     Am = A;Bm=B;Cm=C;I =1

     Pm = A*P*Am + B*Q*Bm

     G = Pm*C/(C*Pm*Cm+R)

     xhat_new = xhatm + G*(y-C*xhatm)

     P_new = (I-G*C)*Pm

     return xhat_new,P_new,G

A = 1.

b = 1.

c = 1.

Q = 1.

R = 5.

N = 300

x = np.zeros(N)

y = np.zeros(N)

v = np.zeros(N)

w = np.zeros(N)

xhat = np.zeros(N)

t = np.zeros(N)

for i in range(N):

v[i] = random.normalvariate(0,Q)

w[i] = random.normalvariate(0,R)

t[i] = i

#print v[i],w[i]

y[0] = c*x[0] + w[0]

for k in range(1,N):

     x[k] = A*x[k-1]+b*v[k-1]

     y[k] = c*x[k] + w[k]

P = 0

xhat[0] = 0.0

for k in range(1,N):

      xhat[k],P,G = kf(A,b,0,c,Q,R,0,y[k],xhat[k-1],P)

pl.plot(t,y,label='measured')

pl.plot(t,x,label='ture')

pl.plot(t,xhat,label='estimate')

pl.grid(True)

pl.legend()

pl.savefig('kf')

pl.show()

とりあえずうまくいってるようです。