SHARE
TWEET

Untitled

a guest Mar 24th, 2019 73 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. import numpy as np
  2. import sympy as sp
  3. from sympy.abc import z
  4. from sympy.polys.partfrac import apart, apart_list
  5. from sympy import *
  6. from scipy import signal
  7. import matplotlib.pyplot as plt
  8.  
  9.  
  10. trm = ((z**2 + z + 1) / (z**3 - 2.55*z**2 + 1.175*z - 0.15))
  11. wspG = [1, -2.55, 1.175, -0.15]
  12.  
  13. a = apart(trm,z).evalf()
  14.  
  15. prw1 = a.args[0]
  16. prw2 = a.args[1]
  17. prw3 = a.args[2]
  18.  
  19. wsp1 = a.args[0].args[0]
  20. wsp2 = a.args[1].args[0]
  21. wsp3 = a.args[2].args[0]
  22.  
  23.  
  24. print('GRAF UKLADU: ')
  25. print('\n')
  26. print('            /','------------> ', prw1, '------------->', '\\')
  27. print('U(wejscie) ---','------------> ', prw2, '------------->', '----> Y(wyjscie)')
  28. print('           \\','------------> ', prw3, '------------->', '/')
  29.  
  30.  
  31. polaczone = together(a)
  32. print('\n')
  33. print('polaczone:',polaczone)
  34.  
  35.  
  36. #MODEL STANOWY========================
  37.  
  38. A = np.array([[wspG[1],wspG[2],wspG[3]],[1,0,0],[0,1,0]])
  39. print('MACIERZ A: ')
  40. print(A[0])
  41. print(A[1])
  42. print(A[2])
  43.  
  44. B = np.array([[1,0,0]]).T
  45. C = np.array([[1,1,1]])
  46. D = 0
  47. print('MACIERZ B: ')
  48. print(B)
  49. print('MACIERZ C: ')
  50. print(C)
  51. print('MACIERZ D: ')
  52. print(D)
  53.  
  54. #MODEL STANOWY========================
  55.  
  56. #RYSOWANIE============================
  57.  
  58.  
  59.  
  60.  
  61. #RYSOWANIE============================
  62.  
  63. #STEROWNIK LQR========================
  64.  
  65. At = np.matrix.transpose(A)
  66. print('transpose A:')
  67. print(At)
  68. Bt = np.matrix.transpose(B)
  69. P = np.array([[0,1,1],[1,2,3],[1,3,2]])
  70.  
  71. Q = np.array([[-3,0,0],[0,-3,0],[0,0,-3]])
  72. R = -50
  73.  
  74. #P_Q = At*(P - P*B*(1/(R+Bt*P*B))*Bt*P)*A
  75.  
  76. def podstawianieP(P):
  77.  
  78.     P_BtP = np.matmul(Bt,P)
  79.     P_BtPB = np.matmul(P_BtP,B)
  80.     P_PB = np.matmul(P,B)
  81.     P_PBR = np.matmul(P_PB,1/(P_BtPB+R))
  82.     P_PA = np.matmul(P_PBR,Bt)
  83.     P_PA2 = np.matmul(P_PA,P)
  84.     P_PA2 = P - P_PA2
  85.     P_PA3 = np.matmul(P_PA2,A)
  86.     P_PA4 = np.matmul(At,P_PA3)
  87.     P = Q + P_PA4
  88.     return P
  89.  
  90.  
  91. for i in range(20):
  92.     P = podstawianieP(P)
  93.     print('macierz P:')
  94.     print(P)
  95.  
  96. P_BtP = np.matmul(Bt,P)
  97. P_BtPB = np.matmul(P_BtP,B)
  98. P_PBR = np.matmul(1/(P_BtPB+R),Bt)
  99. P_PA = np.matmul(P_PBR,P)
  100. F = np.matmul(P_PA,A)
  101.  
  102. noweA = A - np.matmul(B,F)
  103.  
  104. #STEROWNIK LQR========================
  105.  
  106. #RYSOWANIE2===========================
  107.  
  108. name = 'odp skokowa'
  109. name2 = 'odp z regulatorem LQR'
  110. rts = np.roots(wspG)
  111.  
  112. p1 = np.poly1d([1,-rts[0]])
  113. p2 = np.poly1d([1,-rts[1]])
  114. p3 = np.poly1d([1,-rts[2]])
  115.  
  116. num = np.poly1d([1,1,1])
  117. den = p1*p2*p3
  118.  
  119. sys = signal.TransferFunction(num,den)
  120.  
  121. #=====================================
  122.  
  123. nowywsp1 = -noweA[0][0]
  124. nowywsp2 = -noweA[0][1]
  125. nowywsp3 = -noweA[0][2]
  126.  
  127. nowywspG = [1,nowywsp1,nowywsp2,nowywsp3]
  128.  
  129. nowyRoots = np.roots(nowywspG)
  130. print(nowyRoots)
  131.  
  132. p1n = np.poly1d([1,nowyRoots[0]])
  133. p2n = np.poly1d([1,nowyRoots[1]])
  134. p3n = np.poly1d([1,nowyRoots[2]])
  135.  
  136. numn = np.poly1d([1,1,1])
  137. denn = p1n*p2n*p3n
  138.  
  139. sysn = signal.TransferFunction(numn,denn)
  140.  
  141.  
  142.  
  143. tn,yn = signal.step(sysn)
  144. t,y = signal.step(sys)
  145. plt.figure(1)
  146. plt.plot(t,y,'k-',label=name)
  147. plt.plot(tn,yn,'b-',label=name2)
  148. plt.axis([0,50,0,10000])
  149. plt.legend(loc='best')
  150. plt.show()
  151.  
  152. #RYSOWANIE2===========================
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top