連成振動をpythonでシミュレーションしてみた

行列計算と正規分布が使えるようになったので、バネマスの連成振動を状態空間モデルで解いてみることにした。

・2質点系
・減衰、外力は働かないが、観測時に観測誤差が入るとする

matplotlibとかで出力できるとかっこいいのだが、とりあえずはファイル出力するようにしておいた。

#!/usr/bin/python
# -*- coding:utf-8 -*-

import numpy as np
from scipy import stats

#-- initialization
m = 1.0
k = 1.0
kappa = 1.0
dt = 0.01
omega1 = (k + kappa) / m
omega2 = kappa / m
stepNum = 10000
freq = 10

x1 = 0.0
x2 = 0.0
v1 = 1.0
v2 = -0.5

mu = 0.0
sigma = 1.0

#-- state equation
x = np.matrix([[x1],[x2],[v1],[v2]])
A = np.matrix([[0.0,0.0,1.0,0.0],
             [0.0,0.0,0.0,1.0],
             [-omega1,omega2,0.0,0.0],
             [omega2,-omega1,0.0,0.0]])

#-- observation equation
H = np.matrix([[1.0,0.0,0.0,0.0],
             [0.0,1.0,0.0,0.0]])
z = H * x

#-- file writer
fw = open("./result.csv","w")
fw.write(",z1,z2\n")

template = "%f,%f,%f\n"
#-- Runge-Kutta 4th order Method
for t in range(stepNum):
    if(t % freq == 0):
        tv = 0.05 * np.matrix(stats.norm.rvs(0,1,size = 2))
        v = tv.transpose()
        z = H * x + v
        fw.write(template % (t*dt,float(z[0]),float(z[1])))
    
    k1 = dt * A * x
    k2 = dt * A * (x + k1/2.0)
    k3 = dt * A * (x + k2/2.0)
    k4 = dt * A * (x + k3)
    x = x + (k1 + 2.0*k2 + 2.0*k3 + k4)/6.0  

fw.close()

行列のRunge-Kutta法がきれいに書ける。ファイル出力してExcelとかでグラフを描くと、ちゃんと連成振動していることがわかる。matplotlibも修行しないとなー。