2014年6月18日水曜日

カルマンフィルタ

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

その中で、例題プログラムがあり、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()


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


2014年6月15日日曜日

Harten-Yeeをちょっと修正

以前のものを多少修正しました。
もうちょっと美しく出来れば良いんですけど・・・。
1次風上も残してますが、20行くらい少なくなってます。


#advection #################################################
# Harten Yee Upwind ########################################
############################################################
#import#####################################################
from pylab import *
def cal_fai(z):
    eps = 0.0000000001
    if abs(z) > eps:
        return abs(z)
    else:
        return (z*z+eps*eps)/(2*eps)
def cal_sigma(z,dt,dx):
    return 0.5*(cal_fai(z)-dt/dx*z*z)
def minmod2(x,y):
    if abs(x)<=abs(y) and x*y >=0.0:
        return x
    elif abs(x)>abs(y) and x*y >= 0.0:
        return y
    elif x*y<0:
        return 0.0
    else:
        print "Error minmode2"
# Harten Yee ##############################################
class Cell:
    def __init__(self,dxl,u,f,fn,fl,fr):
        self.dxl   = dxl
        self.u     = u
        self.f     = f
        self.fn    = fn
        self.fl    = fl
        self.fr    = fr
        self.df    = 0.0
        self.gi    = 0.0
        self.gamma = 0.0
        self.fai   = 0.0
    def upwind(self,dt,cellup,celldn):
        ultmp = 0.5*(u + cellup.u)
        urtmp = 0.5*(celldn.u + u)
        if self.u > 0:
            self.fl = ultmp*cellup.f
            self.fr = urtmp*self.f
        else:
            self.fl = ultmp*self.f
            self.fr = urtmp*celldn.f
    def caldf(self,celldn):
        self.df = celldn.f - self.f
    def calgi(self,cellup):
        self.gi = minmod2(self.df,cellup.df)
    def calgamma(self,dt,celldn):
        if self.df != 0:
            self.gamma = cal_sigma(self.u,dt,self.dxl)*(celldn.gi - self.gi)/self.df
        else:
            self.gamma = 0.0
    def calfai(self,celldn):
        self.fai = cal_sigma(self.u,dt,self.dxl)*(self.gi+celldn.gi)-cal_fai(self.u+self.gamma)*self.df
    def yee(self,dt,cellup,celldn):
        csdn = 0.5*(celldn.u*celldn.f+self.u*self.f)
        csup = 0.5*(self.u*self.f + cellup.u*cellup.f)
        cfdn = 0.5*self.fai
        cfup = 0.5*cellup.fai
        self.fr = csdn+cfdn
        self.fl = csup+cfup
    def euler(self,dt):
        self.fn = self.f - dt/self.dxl*(self.fr - self.fl)
    def shift(self):
        self.f = self.fn
#set parameters ############################################
in_ff = open('test.txt')
imx=100
dx =  1.0
dt =  0.2
u  =  0.2
print "-"*40
print "u  =",u
print "dx =",dx
print "dt =",dt
print "-"*40
############################################################
#initial condition ################################
cells = []
fini = []
i = 0
for x in in_ff:
    print i
    a = x.split("\t")
    for k in range(0,1):
        print a[k]
    cells = cells + [Cell(float(dx),float(u),float(a[1]),float(a[1]),0.0,0.0)]
    fini = fini + [float(a[1])]
    i=i+1
imx=i
print "imx=",imx
print "*"*40
print "xl,f"
for i in range(0,imx):
    print i,"  ",cells[i].f,cells[i].u
# Time integration ################################
for n in range(0,1000):
    #Harten Yee##############################
    for i in range(0,imx-1):
        cells[i].caldf(cells[i+1])
    for i in range(2,imx-1):
        cells[i].calgi(cells[i-1])
    for i in range(3,imx-2):
        cells[i].calgamma(dt,cells[i+1])
    for i in range(2,imx-1):
        cells[i].calfai(cells[i+1])
    for i in range(1,imx-3):
        cells[i].yee(dt,cells[i-1],cells[i+1])
    for i in range(2,imx-2):
        cells[i].euler(dt)
    #shift#########################################
    for i in range(2,imx-2):
        cells[i].shift()
# Exact ###########################################
fout = [0]
fex = [0]
xl  = [0]
for i in range(0,imx-1):
    if i>=49 and i<=59:
        fex = fex +[1.0]
    else:
        fex = fex +[0.0]
    fout = fout + [float(cells[i].f)]
    xl = xl + [float(i)]
#output############################################
ff=open("output.dat","w")
for i in range(0,imx-1):
    ff.write(str(i))
    ff.write("    ")
    ff.write(str(fout[i]))
    ff.write("\n")
ff.close()
#####################graph#########################
y1=fout
y2=fini
y3=fex
print len(fout),len(fini),len(fex)
plot(xl,y1,label = 'Harten-Yee')
plot(xl,y2,label = 'INITIAL')
plot(xl,y3,label = 'EXACT')
xlabel('x')
ylabel('f')
title('Hatren-Yee')
axis([-0,100.0,-0.2,1.2])
legend()
grid(True)
savefig('Harten-Yee')
show()
###################################################