連立微分方程式の数値解析

制御入力のあるシステム

制御入力$u(t)$がある線形システムの挙動をシミュレーションすることを考える。
このとき、式（１）の連立微分方程式を解けばよい。

\dfrac{dx}{dt} = Ax+Bu \\

y=Cx
\tag{1}

実装

import numpy as np
from scipy.integrate import odeint
import matplotlib.pyplot as plt
import seaborn

def func(x,t,A,B,u):
    x = x.reshape(x.shape[0],1)
    u = u.reshape(u.shape[0],1)
    dxdt = np.zeros_like(x)
    dxdt = A@x+B@u
    dxdt = dxdt.reshape(-1,)
    return dxdt

def state_space_equation(x_now,t_now,t_next,u_now):
    x_init = x_now
    t = np.linspace(t_now,t_next,100)
    u = u_now

    A = np.array([[-1,0,0],[0,-1,0],[0,0,-1]])
    B = np.array([[1,0,0],[0,1,0],[0,0,1]])
    C = np.array([[1,0,0],[0,1,0],[0,0,1]])

    x = odeint(func,x_init,t,args=(A,B,u))
    y = np.array(C)@x.T
    y = y.T
    t = t.reshape(t.shape[0],1)
    return x,y,t

def system():
    t_s,t_f,dt = 0,10,1e-1
    time = np.arange(t_s, t_f, dt)

    x_now = np.array([10,5,2])
    u_now = np.array([0,0,0])

    for i,t_now in enumerate(time):
        t_next = t_now + dt
        x,y,t = state_space_equation(x_now,t_now,t_next,u_now)
        
        u_now = np.array([0,0,0])-y[-1,:]
        x_now = x[-1,:]
        
        if i == 0:
            obs_x = x
            obs_y = y
            obs_t = t
        else:
            obs_x = np.concatenate([obs_x,x],axis=0)
            obs_y = np.concatenate([obs_y,y],axis=0)
            obs_t = np.concatenate([obs_t,t],axis=0)
    return obs_x,obs_y,obs_t

def savefig(t,sol,pngfn):
    plt.figure()
    for i in range(sol.shape[1]):
        plt.plot(t,sol[:,i])
    plt.savefig(pngfn)
    plt.close()

def savecsv(t,x,y,csvfn):
    sol = np.concatenate([t,x,y],axis=1)
    np.savetxt(csvfn, sol, delimiter=',')

x,y,t = system()
pngfn = "scenario_1.png"
savefig(t,y,pngfn)
csvfn = "scenario_1.csv"
savecsv(t,x,y,csvfn)