龙格-库塔法求解连续系统输出

import control
import numpy as np
import matplotlib.pyplot as plt


G1 = control.tf([1,1],[1,4])
G2 = control.tf([1,2],[1,1,1.25])
G = control.series(2.5*G1,G2)
sys = control.tf2ss(G)

h = 0.3 #步长
L = np.arange(0,15,h) # 仿真步数
u = np.ones(len(L)) # 输入
y = np.zeros(len(L)) # 输出
#初值
xe0 = np.array([0,0,0]).reshape(-1,1)

u0 = 0

for i in np.arange(len(L)): 
    
    K1 = np.dot(sys.A,xe0) + np.dot(sys.B,u0)
    K2 = np.dot(sys.A,(xe0 + h*K1/2)) + np.dot(sys.B,u0)
    K3 = np.dot(sys.A,(xe0 + h*K2/2)) + np.dot(sys.B,u0)
    K4 = np.dot(sys.A,(xe0 + h*K3)) + np.dot(sys.B,u[i])
    xe = xe0 + h*(K1+2*K2+2*K3+K4)/6
    y[i] = np.dot(sys.C,xe)
   
    u0 = u[i]
    xe0 = xe
   
    
plt.plot(L,y)
plt.plot(L,u)
plt.show()