!pip install control

import numpy as np
import matplotlib.pyplot as plt
from control.matlab import *

num1 = np.array([1])
den1 = np.array([1, 1])
G_1 = tf(num1, den1)
print(G_1)

type(G_1)

t = np.linspace(0, 5, 100)
y_1, t_1 = step(G_1, t)
y_1i, t_1i = impulse(G_1, t)

plt.figure(dpi=100, figsize=(8,4))
plt.plot(t_1, y_1, label=r'$\frac{1}{s+1}$')
plt.xlabel(r'$t$ (sec)')
plt.ylabel(r'$y$')
plt.title('Step response')
plt.legend()
plt.grid()
plt.show()

plt.figure(dpi=100, figsize=(8,4))
plt.plot(t_1i, y_1i, label=r'$\frac{1}{s+1}$')
plt.xlabel(r'$t$ (sec)')
plt.ylabel(r'$y$')
plt.title('Impulse response')
plt.legend()
plt.grid()
plt.show()

num2 = np.array([2])
den2 = np.array([1, 2])
G_2 = tf(num2, den2)

num3 = np.array([1])
den3 = np.array([1, 2])
G_3 = tf(num3, den3)

y_2, t_2 = step(G_2, t)
y_3, t_3 = step(G_3, t)

plt.figure(dpi=100, figsize=(8,4))
plt.plot(t_1, y_1, label=r'$\frac{1}{s+1}$')
plt.plot(t_2, y_2, label=r'$\frac{2}{s+2}$')
plt.plot(t_3, y_3, label=r'$\frac{1}{s+2}$')
plt.title('Step responses')
plt.xlabel(r'$t$ (sec)')
plt.ylabel(r'$y$')
plt.legend()
plt.grid()
plt.show()

num = np.array([1])
den = np.array([1, 1, 1])
G_tf = tf(num, den)

print(G_tf)

t = np.linspace(0, 10, 100)

y_tf, t_tf = step(G_tf, t)

plt.figure(dpi=100, figsize=(8,4))
plt.plot(t_tf, y_tf, label='TF')
plt.xlabel(r'$t$ (sec)')
plt.ylabel(r'$y$')
plt.legend()
plt.grid()
plt.show()

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

print (G_ss)

type(G_ss)

y_ss, t_ss = step(G_ss, t)

plt.figure(dpi=100, figsize=(8,4))
plt.plot(t_tf, y_tf, label='TF')
plt.plot(t_ss, y_ss, '--', label='SS')
plt.xlabel(r'$t$ (sec)')
plt.ylabel(r'$y$')
plt.legend()
plt.grid()
plt.show()

print (ss2tf(G_ss))

print (tf2ss(G_tf))

def model(t,z,u):
  z = z.reshape(-1,1)
  u = [[u]]
  return (A@z + B@u).flatten()
  
t_f = 10
dt = 0.01
t_ni = np.arange(0,t_f,dt)
T = len(t_ni)

ic = [0, 0]
state = np.zeros((2,T))
output = np.zeros(T)
input = np.ones(T)

state[:,0] = ic
deriv_p = model(t_ni[0],state[:,0],0)

for k in range(T-1):
  deriv = model(t_ni[k],state[:,k],input[k])
  state[:,k+1] = state[:,k] + dt*(3*deriv - deriv_p)/2
  output[k] = C.dot(state[:,k])
  deriv_p = deriv
output[-1] = output[-2]

plt.figure(dpi=100, figsize=(8,4))
plt.plot(t_tf, y_tf, label='TF')
plt.plot(t_ss, y_ss, '--', label='SS')
plt.plot(t_ni, output, ':', alpha = 0.3, linewidth=4, label='Numerical integration')
plt.xlabel(r'$t$ (sec)')
plt.ylabel(r'$y$')
plt.legend()
plt.grid()
plt.show()

print (f'G_1={G_1}')
print (f'G_2={G_2}')
print (f'G_1+G_2={G_1+G_2}')
print (f'G_1 G_2={G_1*G_2}')

K = 2
G_f = feedback(G_1,K,-1) 
G_f = (1/dcgain(G_f))*G_f
print (G_f)

y_f, t_f = step(G_f, t)

plt.figure(dpi=100, figsize=(8,4))
plt.plot(t_1, y_1, label='open loop')
plt.plot(t_f, y_f, label='closed loop')
plt.xlabel(r'$t$ (sec)')
plt.ylabel(r'$y$')
plt.legend()
plt.grid()
plt.show()

omega = 2*np.pi
zeta = np.linspace(0, 1, 11)
t = np.linspace(0, 2, 100) 

np.set_printoptions(precision=1)
plt.figure(dpi=100, figsize=(10,5))
for z in zeta:
  num = omega**2
  den = np.array([1, 2*z*omega, omega**2])
  Gs = tf(num,den)
  ys, ts = step(Gs, t)
  plt.plot(ts, ys, label=rf'$\zeta$={z:3.1f}')
plt.xlabel(r'$t$ (sec)')
plt.ylabel(r'$y$')
plt.legend()
plt.grid()
plt.show()

omega = 1
zeta = 0.7
num = np.array([1, 0, -omega**2])
den = np.array([1, 2*zeta*omega, omega**2])
Gs = tf(num,den)

print(f'poles at {pole(Gs)}')
print(f'zeros at {zero(Gs)}')

pzmap(Gs, grid=True)
fig = plt.gcf().set_size_inches(8, 8)
plt.xlim([-2,2])
plt.show()

t = np.linspace(0, 10, 100) 
yy, tt = step(Gs)
plt.figure(dpi=100, figsize=(8,4))
plt.plot(tt, yy)
plt.xlabel(r'$t$ (sec)')
plt.ylabel(r'$y$')
plt.grid()
plt.show()