o
    şĽg= ă                   @   sć  d dl mZ d dlmZ d dlmZ d dlmZmZm	Z	m
Z
 d dlmZ d dlmZ d dlmZ d dlmZmZ d d	lmZ d d
lmZ d dlmZ d dlmZ d dlmZ d dlmZ d dl m!Z! d dl"m#Z#m$Z$m%Z% d dl&m'Z'm(Z(m)Z)m*Z*m+Z+m,Z,m-Z-m.Z.m/Z/m0Z0m1Z1m2Z2m3Z3m4Z4m5Z5 d dl6m7Z7 ed\Z8Z9Z:Z;Z<Z=Z>Z?Z@ZAZBZCZDed\ZEZFZGZHZIZJZKZLZMZNZOZPZQZRZSZTe'de<d deB eC e<  eCd  e<ZUe'e@de<ZVe'eGe? e< eGe< e? e<ZWdd ZXdd ZYdd ZZdd Z[dd  Z\d!d" Z]d#d$ Z^d%d& Z_d'd( Z`d)d* Zad+d, Zbd-d. Zcd/d0 Zdd1d2 Zed3d4 Zfd5d6 Zgd7d8 Zhd9d: Zid;d< Zjd=d> Zkd?d@ ZldAdB ZmdCdD ZndEdF ZodGdH ZpdIdJ ZqdKdL ZrdMdN ZsdOdP ZtdQdR ZudSdT ZvdUdV ZwdWdX ZxdYdZ Zyd[d\ Zzd]d^ Z{d_S )`é    )ÚAdd)ÚFunction)ÚMul)ÚIÚpiÚRationalÚoo)ÚPow)ÚS)Úsymbols)ÚexpÚlog)Úsqrt)Úatan)Úeye)Úfactor)ÚCRootOf)Úsimplify)ÚTuple)ÚImmutableMatrixÚMatrixÚ
ShapeError)ÚTransferFunctionÚSeriesÚParallelÚFeedbackÚTransferFunctionMatrixÚ
MIMOSeriesÚMIMOParallelÚMIMOFeedbackÚ
StateSpaceÚgbtÚbilinearÚforward_diffÚbackward_diffÚphase_marginÚgain_margin)Úraisesz.a, x, b, c, s, g, d, p, k,    tau, zeta, wn, Tza0:4,    b0:4, c0:4, d0:4é   é   c                  C   s  t td td t d t} | jtd ksJ | jtd t d ks$J | jtd td t d tfks6J t td td t}|jtd ksIJ |jtd ksRJ |jtd td tfks`J t td td d t}|jtd ksuJ |jtd d ksJ |jtd td d tfksJ t td dtd   d td dt  d t}|jtd dtd   d td dt  d tfksÄJ t td td  td td  t}|jtd td  ksăJ |jtd td  td td  tfksůJ t td td t}|jtd td tfksJ t td dt t}|jtd dt tfks+J t td td t}|jtd td tfksDJ t ddt}|jdksRJ |jdksZJ |jddtfkseJ t ddt}	|	jdkssJ |	jdks{J |	jddtfksJ t dtd  dt  dt  d	td  d
t  t}
t dtd  dt  dt  d	td  d
t  t}|
|krÁJ t tt ttd   ttd   t	t t
t  t}t tt ttd   ttd   t	t t
t  t}||ksüJ |  |  kr|    ksJ  J t ttd  ttd   tt  t tt ttd   tt  t}|jttd  ttd   t tt  tt ttd   tt  tfks[J t td t ttd  tt  t t}t td t ttd  tt  t t}|jttd  ttt  ttd   tfksJ ||ksŁJ t tt t ttd  t
t  t	 t}|jttt  ksĂJ |jt	t
t  ttd   ksŐJ |jttt  t	t
t  ttd   tfksîJ t dtd t d t}|jdtd t d tfksJ t ddt}|jddtfksJ t ttd  ttd   t ttd  t}|jttd  t ttd   ttd  tfksMJ t td td  ttd   tt t t}|jttd  td td   tt t tfks~J td\}}}}t ||t  |t  td d| t  |d  t}|j|t |t  | ks°J |j|t |t  | |d d| t  td  tfksĎJ ttdd  ttdd  ttdd  ttdd  ttdd  ttdd  ttdd  d S )Nr(   r)   é   é   é   é	   é   ç      ŕ?é   é   r   g333333ă?gffffff!Ŕg      Đ?géżzomega_o, k_p, k_o, k_ic                   S   s   t ddtS )Nr*   r   Šr   ÚsŠ r4   r4   ú[/var/www/visachat/venv/lib/python3.10/site-packages/sympy/physics/control/tests/test_lti.pyÚ<lambda>u   ó    z4test_TransferFunction_construction.<locals>.<lambda>c                   S   s   t tdtS ŠNr   r2   r4   r4   r4   r5   r6   v   r7   c                   S   s   t ddtS r8   r2   r4   r4   r4   r5   r6   w   r7   c                   S   s   t tg d˘ttS ŠNŠr(   r)   r,   )r   r   r3   r4   r4   r4   r5   r6   y   ó    c                   S   s    t td dt  d td dS )Nr)   r(   r,   r2   r4   r4   r4   r5   r6   {   s     c                   S   s   t td dt dS )Nr(   r+   r*   )r   Úpr4   r4   r4   r5   r6   |   r;   c                   S   s   t dddS )Nr,   r*   r0   )r   r4   r4   r4   r5   r6   }   r7   )r   r3   ÚnumÚdenÚargsr<   Úa0Úa1Úa2Úb0Úb1ÚaÚbÚgÚdÚb2r   r'   Ú
ValueErrorÚ	TypeError)ÚtfÚtf1Útf2Útf3Útf4Útf4_Útf5Útf5_Útf6Útf6_Útf7Útf8Útf7_Útf8_Útf9Útf10Útf10_Útf11Útf12Útf13Útf14Útf15Úomega_oÚk_pÚk_oÚk_iÚtf18r4   r4   r5   Ú"test_TransferFunction_construction   s   $ 08$,4444,DN((2($2",6,64 >rg   c            ?         s  t dttddddd} td ttd  dt  td d  d	ddt  ddt   d
dt  ddtd     }d
td  dtd   d td td
   }ttd td d t}tt t }|| Ą  }t |tĄ}ttd td d ttt  t}t	ddt td d  dd}t | ĄtdttksJ t
tfdd t
tfdd t tĄtttd  dt  td d tksËJ t tĄtttd  dt  td d tksĺJ t
tfdd t tĄtd	dtksüJ t |tĄtddt  ddt   d
dt  ddtd    tks!J t |tĄtd
td  dtd   d td td
  tksBJ ||ksIJ t |tĄtdtd  dt  d td d tksfJ t ddgg d˘tĄ}	td dt g}
td td dg}t |
|tĄ}g d˘ddg |	ttd dtd  dt  d tks¨J |ttd t dt  td td  ttd   d tksÉJ t
t fdd dg}ddg}d}t |||tĄ}	|	tdt d td dt  td dt   tksJ ttd td  t ttd  t}ttd td  d  td t}| Ą t|  kr5|ks8J  J ttd td  td td  t}	ttd td t}|	 Ą t|	  kre|kshJ  J tdt d td d d t}tddt}ttt  ttt   ttt   tt tt   }ttt  ttt   tt t tt   }ttt  tt  ttt  tt   ttt  tt   ttt  tt   ttt  tt   ttdt    }tt t t tt  ttt  tt   tt t tt  t  ttt  tt   }tt t tt  tt t  tt t t tt   ttd  t  }t||t}tttt  ttt   tt tt t  ttt  t  t}| Ą ttd dt  d td dtd   d tks}J |	 Ą ttd dt  d td dt  d tksJ | Ą |ksŁJ | Ą t||tks°J | Ą tttt  ttt   |tksÇJ tt t }ttd  tt  t }t||t}tdt d dtd  dt  d t}tdt d dtd  dt  d t} | tdtdtdtdtdiĄ| ksJ | tdtdtdtdtdiĄ Ą |ks1J |  Ą |ks:J td\}!}"}#}$|!t |$td  |#td   |" }%}&t|%|&t}'tdt dtd  dtd   d t}(tdt dtd  dtd   d t})|' |!d|"d|#d|$diĄ|)ksJ |' |!d|"d|#d|$diĄ Ą |(ksŠJ |) Ą |(ks˛J tttd  ttd   tt  t tt t t}*tdtd  dtd   t d tdt  t}+tdtd  dtd   t d tdt  t},|* tdtdtdtdiĄ|,ksJ |* tdtdtdtdiĄ Ą |+ks"J |, Ą |+ks+J ttttd   tt t t}-tdtd  t tt t}.tdtd  t tt t}/|- tdtd iĄ|/kseJ |- tdtd iĄ Ą |.ksuJ |/ Ą |.ks~J |	 tĄtd td dt  d  dt td dt  d   dtd dt  d   ksŽJ | dt Ątdd! tdt d!  ksĆJ | tdd Ąttt  tt ttdd   tdt t d  dt t ttdd   d  td!t t d
   ddtdd   dtdd   t d
t t ttdd   d"t t  d"t t ttdd    d!t t    ksDJ | Ą dksMJ |	 Ą tddksYJ |' Ą dksbJ |. Ą dkskJ |) Ą dkstJ tdtt Ą t ksJ tt!d t" t"t!}0tt"t!d t" t!}1tt!d t!d# t! t!d  t!}2tt!d# t! t!d  t!d t!}3| #Ą ttt t gksÄJ | #Ą g ksÍJ |	 #Ą d$dgksŘJ |/ #Ą tgksâJ |- #Ą t t gksďJ |+ #Ą d%t gksűJ t$|( #Ą t$d&d'd(t  d'd(t  gksJ t$| #Ą t$d)d*t  d)d*t  gks*J |1 #Ą t"tdd t"tdd  tt"tdd  t t"tdd  gksQJ |2 #Ą t%t!d
 t! d ddt%t!d
 t! d dt%t!d
 t! d dt%t!d
 t! d dt%t!d
 t! d dt%t!d
 t! d dt%t!d
 t! d d	t%t!d
 t! d dt%t!d
 t! d dg
ksľJ t
t&d+d  td,d-d.\}4}5td/d-d0}6ttd t tt  |4t |5 t}2ttd t tt  |4t d t}7ttd t tt  |4t |6 t}8| 'Ą du s	J | 'Ą d-u sJ |	 'Ą du sJ |( 'Ą du s$J | 'Ą d-u s-J |7 'Ą d-u s6J |8 'Ą d-u s?J |2 'Ą du sHJ |/ 'Ą d u sQJ |- 'Ą d u sZJ | (Ą ddgkseJ | (Ą g ksnJ |	 (Ą ddgksyJ |/ (Ą dtdd t tdd  d dtdd  t tdd  d1 t)ddtdd  t t tdd  d1  dtdd  t tdd  d1 t)ddtdd  t t tdd  d1  gksâJ |- (Ą tt tdd tt tdd  d t)dt tt tdd  d  tt tdd  d t)dt tt tdd  d  gk	s+J t$|+ (Ą t$d2d3t)d4td  d   d3t)d4td  d  d2 gk	sRJ |0 (Ą t"tdd t"tdd  d t)dt t"tdd  d  t"tdd  d t)dt t"tdd  d  gk	sJ |3 (Ą t%t!d
 t! d ddt%t!d
 t! d dt%t!d
 t! d dt%t!d
 t! d dt%t!d
 t! d dt%t!d
 t! d dt%t!d
 t! d d	t%t!d
 t! d dt%t!d
 t! d dg
k	sőJ t
t&d5d  ttd td td  d
 t}tdt d dt t| tt d td td  d
 tk
s/J  tdt d dt tk
sAJ ttd td tttd d dt tttd d d dt d t}(ttd d td d t}  *Ą d   k
rt+d  k
r|k
sJ  J    *Ą d   k
r˛t+d  k
r˛|(k
sľJ  J d t+d  k
rËtddtk
sÎJ  J t, *Ą d d   k
rôt+d  k
rôttd td tk
s÷J  J   *Ą d d6   kr#t+d6  kr#tdt d td d d tks&J  J t
tfd7d t
tfd8d t
tfd9d ttd td dt  d t}ttt td d t}9t-|ttd td d tksqJ |j. tdĄ|j/ tdĄ  krdksJ  J | tdĄttd td dt  d tks¤J |9 tdĄttd dtksľJ  0ttiĄtdt d dt tksËJ t
t1fd:d  ttdĄksáJ tttt  ttt   tt t t}:|: 0tt"iĄttt"t  ttt"   tt t" t"ksJ |: tt"Ątttt  ttt   tt t tks0J tttd  dtd   d td	 d t};tdt dt d	t  t}<tddt}=tddt}>|; Ą t ttd  dtd   d ttd	 d dddddksJ |< Ą t td tdt d	t  dddddksJ |= Ą t tdtddddddksłJ |> Ą tddddksÁJ d S );Nr   é˙˙˙˙FŠÚevaluater)   r+   r(   r,   r.   r-   r*   r0   c                      ó
   t   ĄS ŠNŠr   Úfrom_rational_expressionr4   )Úexpr_2r4   r5   r6      ó   
 z1test_TransferFunction_functions.<locals>.<lambda>c                      rk   rl   rm   r4   )Úexpr_3r4   r5   r6      rp   c                      rk   rl   rm   r4   )Úexpr_4r4   r5   r6      rp   )r,   r*   r+   r:   c                      s   t   tĄS rl   )r   Úfrom_coeff_listsr3   r4   )Úden3Únum3r4   r5   r6   ¨   ó    y      đż       @y      đż       Ŕé   ç      đ?ç       @éý˙˙˙ç      @ç      @ç      @zc1, d0:3g      @r1   iů˙˙˙é   é   é
   éű˙˙˙g      ĐżgŢÍúSç?g3pUÍcâżgăHŤ8óšć?gŮżgÄ%1Cő9ĺ?c                   S   s(   t td ttd  t td  t Ą S )Nr)   r   )r   Úxr@   Úpolesr4   r4   r4   r5   r6     ó   ( zq, rT)ÚnegativeÚt)Úpositiveé   g      Ŕ?gv¸-kÁĆń?gâ3ěŮżc                   S   s(   t ttd  t td  td t Ą S )Nr   r)   )r   r@   r   Úzerosr4   r4   r4   r5   r6   /  r   éţ˙˙˙c                      s    t d t  d  S ŠNr)   r(   Šr3   r4   ŠrP   r4   r5   r6   B  r;   c                      s    t  S rl   r   r4   ŠrR   r4   r5   r6   C  ó    c                      s     S rl   r4   r4   ŠrP   rR   r4   r5   r6   D  r   c                      s      ttdiĄS ŠNr)   )Úxreplacer<   r   r4   ŠrO   r4   r5   r6   O  ó    )2r   r	   r3   r<   r   r   ÚtauÚto_exprrn   r   r'   ÚZeroDivisionErrorrJ   rs   Úfrom_zpkr   r   rB   rA   r@   rC   rD   rI   ÚexpandÚsubsÚevalfr   Úeval_frequencyÚwnr
   Údc_gainr   r   r   Úkr   Ústrr   ÚNotImplementedErrorÚ	is_stabler   r   ÚdoitÚpowr   r   r=   r>   r   rK   )?Úexpr_1Úexpr_5Úexpr_6rL   ÚdelayÚexpr_7ÚH1ÚH2Úexpr_8rM   Únum2Úden2rN   r   r   ÚgainÚtf0rE   rF   ÚG1ÚG2ÚcrH   ÚeÚfrG   ÚG3ÚG4Úp1Úp2ÚSP1Úexpect1Úexpect1_Úc1Úd0Úd1Úd2Úp3Úp4ÚSP2Úexpect2Úexpect2_ÚSP3Úexpect3Úexpect3_ÚSP4Úexpect4Úexpect4_Útf_Ú_tfÚTF_Ú_TFÚqÚrr   Ú	stable_tfÚ
stable_tf_rT   rV   rW   rZ   r[   r]   r4   )rt   ro   rq   rr   ru   rO   rP   rR   r5   Útest_TransferFunction_functions   s   4,$44
6
˙
.
˙
&
˙.B:$"($( 4,tXH@>:.(((,&(($(400$(  `0â
˙  0.NDP(ţ$$$H,˙P˙N8˙&˙B0˙DP(ţ .$ >B2R^ &42",(<:,F6* rÔ   c                      s  t td td tt td td tt td td t d tt tdt t tks5J  tks@J    t ksPJ   tks^J tddd ttfd	d
 tt fdd
 ttfdd
 ttfdd
 ttfdd
 ttfdd
  t ks¨J  t ks´J   t  ksÂJ   t ksŃJ ttfdd
 ttfdd
 ttfdd
 ttfdd
 ttfdd
 ttfdd
 d S )Nr.   r+   r,   r(   r)   rł   FŠÚcommutativec                      ó    t g d˘ S r9   Šr   r4   ŠrM   r4   r5   r6   m  ó    z@test_TransferFunction_addition_and_subtraction.<locals>.<lambda>c                      s     S rl   r4   r4   )rł   rN   r4   r5   r6   n  r   c                      ó     S rl   r4   r4   ŠrO   rP   r4   r5   r6   o  r   c                      s    t d  S ŠNr(   r   r4   rŮ   r4   r5   r6   p  r7   c                      s    d S ŠNr0   r4   r4   rŮ   r4   r5   r6   q  r   c                      s   dt d    S ŠNr(   r,   Šr<   r4   rŮ   r4   r5   r6   r  rÚ   c                      s    t g d˘ S r9   rŘ   r4   rŮ   r4   r5   r6   z  rÚ   c                      s     S rl   r4   r4   rÜ   r4   r5   r6   {  r   c                      s    t d  S rÝ   r   r4   rŮ   r4   r5   r6   |  r7   c                      s    d S rŢ   r4   r4   rŮ   r4   r5   r6   }  r   c                      s   t d   S )Nr+   r   r4   )rN   r4   r5   r6   ~  r7   c                      s   dt d    S )Nr(   r*   rŕ   r4   rŮ   r4   r5   r6     rÚ   )r   r3   r<   r   r   r'   rJ   r4   r4   )rł   rM   rN   rO   rP   r5   Ú.test_TransferFunction_addition_and_subtraction`  s2    rá   c                      s
  t td td  d t t td td tt ttd d tt td td tt td td tt td td tt ddt  t ksPJ    t  ks]J     t  ksoJ         t     ksJ  tksJ      t t ddttksŹJ     t tksżJ  ttdksÍJ   t tdksÝJ   ttdksîJ tdd	d
ttfdd tt fdd ttfdd ttfdd ttfdd ttfdd ttfdd ttfdd ttfdd ttfdd ttfdd ttfdd ttfdd ttfdd ttfdd d S )Nr,   r-   r(   r+   r*   r.   rh   rł   FrŐ   c                      ó    t g d˘ S r9   rŘ   r4   Šrś   r4   r5   r6     rÚ   zCtest_TransferFunction_multiplication_and_division.<locals>.<lambda>c                      ó     S rl   r4   r4   )rą   rł   r4   r5   r6     r   c                      rä   rl   r4   r4   Šrś   ÚG5r4   r5   r6     r   c                      s    t d  S rÝ   r   r4   Šrć   r4   r5   r6     r7   c                      s   d  S ŠNr-   r4   r4   rç   r4   r5   r6     r   c                      s    t g d˘ S r9   rŘ   r4   ră   r4   r5   r6   Ą  rÚ   c                      ó    d S r8   r4   r4   )ÚG6r4   r5   r6   ˘  r   c                      s     S rl   r4   r4   rĺ   r4   r5   r6   Ł  r   c                      ré   r   r4   r4   rç   r4   r5   r6   ¤  r   c                      s    t d  S r   r   r4   rç   r4   r5   r6   Ľ  r7   c                      s   t dt d     S )Nr*   r)   r   r4   )r˛   r4   r5   r6   Ś  r;   c                      s   d  S r8   r4   r4   )rˇ   r4   r5   r6   §  r   c                      s   d   S rÝ   r4   r4   )rę   ÚG7r4   r5   r6   ¨  r7   c                      s      S rl   r4   r4   Šrć   rę   rë   r4   r5   r6   Š  r7   c                      s       S rl   r4   r4   rě   r4   r5   r6   Ş  rÚ   )	r   r3   r<   r   r   r¤   r   r'   rJ   r4   r4   )rą   r˛   rś   rˇ   rć   rę   rë   rł   r5   Ú1test_TransferFunction_multiplication_and_division  sF   $2˙& "rí   c                  C   sę   t d\} }}t| d td t|  | t  | d  | }t|td  |td  |}ttt td  td  tt  t tttd   t}tttd  td  tt  t ttd  t}|jsdJ |jsiJ |jsnJ |jrsJ d S ŠNzomega_o, zeta, taur)   r,   r*   )r   r   r3   r<   rE   rF   Ú	is_proper)rb   Úzetar   rą   r˛   rś   rˇ   r4   r4   r5   Útest_TransferFunction_is_proper­  ó   ,80


rń   c                  C   sę   t d\} }}t| d td t|  | t  | d  | }t|td  |td  |}ttt td  td  tt  t tttd   t}tttd  td  tt  t ttd  t}|jrdJ |jriJ |jsnJ |jrsJ d S rî   )r   r   r3   r<   rE   rF   Úis_strictly_proper)rb   rđ   r   rM   rN   rO   rP   r4   r4   r5   Ú(test_TransferFunction_is_strictly_properš  rň   rô   c                  C   sę   t d\} }}t|d td t| | t  |d  |}t| td  | td  | }ttt td  td  tt  t tttd   t}tttd  td  tt  t ttd  t}|jsdJ |jsiJ |jrnJ |jrsJ d S )Nztau, omega_o, zetar)   r,   r*   )r   r   r3   r<   rE   rF   Úis_biproper)r   rb   rđ   rM   rN   rO   rP   r4   r4   r5   Ú!test_TransferFunction_is_biproperĹ  rň   rö   c                     s  t ttd  ttd   tt  ttd  ttd   tt t  t t tt t tt t tt tt tt  t ttt dtd dt	 t
 t  t
d  ttdt} tdt}t }|j fksnJ |jtksuJ tt  }|jt  fksJ |jtksJ t | dt}t | dt}t t||}|j t||fksľJ t }|j fksĹJ t||}|j||fksÓJ |jtksÚJ tt  }	|	jt  fksôJ t }
||
ks˙J ||krJ tt fdd tt fd	d ttfd
d tt fdd ttfdd ttfdd d S )Nr,   r)   r*   r(   ÚX_dÚXc                      ó
   t  S rl   Šr   r4   ŠrL   rO   r4   r5   r6   ô  rp   z*test_Series_construction.<locals>.<lambda>c                      ó   t  S rl   rú   r4   ŠrL   rN   rO   rP   r4   r5   r6   ő  rv   c                      s   t   S rl   rú   r4   ŠrN   rO   r4   r5   r6   ö  r7   c                      ó   t d S r   rú   r4   ŠrL   rP   r4   r5   r6   ÷  r7   c                      ó   t td tt   S r   )r   r3   r<   r4   rţ   r4   r5   r6   ř  ó    c                      ó   t  tg d˘S ŠN)r(   r)   r,   r*   )r   r   r4   r   r4   r5   r6   ů  r   )r   r@   r3   rA   rB   rC   r<   rD   rI   rđ   r   r   r   r?   Úvarr   r'   rJ   rK   )ÚinpÚoutÚs0Ús1Útf3_rQ   Ús2Ús3Ús4Ús6Ús7r4   rý   r5   Útest_Series_constructionŃ  s@   H(


r  c                     s  t ttd  ttd   tt  ttd  ttd   tt t  t t tt t tt t t} t dtd dt	 t
 t  t
d  t}t | |g| |   ggt|  g|  g| ggt| ggttgtgt gg}t tdt gtĄt}|jfksJ |jtksJ |j|j|jf  krŁdksŚJ  J t}|jfksśJ |jtks˝J |j|j|jf  krÍdksĐJ  J tt | }|jt | fksęJ |j|j|jf  krúdksýJ  J ttdd  tt fdd ttfd	d ttfd
d ttfdd ttfdd ttfdd d S )Nr,   r)   r*   r(   )r)   r(   c                   S   s   t  S rl   Šr   r4   r4   r4   r5   r6     s    z.test_MIMOSeries_construction.<locals>.<lambda>c                      s
   t  S rl   r  r4   )Útf_1Útfm_1r4   r5   r6     rp   c                      s   t    S rl   r  r4   Šr  Útfm_2r4   r5   r6     rv   c                      rů   rl   r  r4   )Útfm_3Útfm_5r4   r5   r6      rp   c                      r˙   r   r  r4   Šr  r  r4   r5   r6   #  r7   c                      s   t td tt    S r   )r   r3   r<   r4   r  r4   r5   r6   $  ó    c                      s   t tdt g S rÝ   )r   r   r<   r4   Šr  r4   r5   r6   %  r;   )r   r@   r3   rA   rB   rC   r<   rD   rI   rđ   r   r   ÚTF3ÚTF2ÚTF1Úfrom_Matrixr   r   r?   r  ÚshapeÚnum_outputsÚ
num_inputsr   r'   rJ   rK   )Útf_2Útf_3Útfm_4Ús8Ús9Ús11r4   )r  r  r  r  r  r5   Útest_MIMOSeries_constructionü  s4   H(
&&&r(  c                     s   t dtd dt t t  td  t t tdtt tt t tt t tt tt tt  t ttt ttd  tt  t tt t}    t	   krlt	t	   krlt	 t	ksoJ  J    t	 t
ksJ   |  t
t	 | ksJ   |  t
t	 |  ks J    |  t
t	 | ksłJ    |  t
t	  |  ksČJ    |  t
t	  | ksÜJ   |   t
t	 t	| ksńJ   |   t
t	 t	t ddtt	| ksJ      t	t
  ks&J    t	  ks4J    t	t ddtt	 ksHJ tt fdd tt fdd ttfdd t	 dd	t	  Ą   krt ttd dt t t  td  tksJ  J t	 t
  dd	t	 t
   Ą   krët ttt t t t t td dt t t  td     tt t td dt t t  td  d  tksîJ  J t	  dd	t	   Ą   kr(t tt t t  tt t td dt t t  td   tks+J  J t	  d
d	t	   Ą kr>J t	t
 t
  Ą t ttd dt t t  td   d t t ttt t   t  tt t td dt t t  td   tksJ t	     Ą t tt t t  tt t td dt t t  td   tksšJ t	  Ą  t t tt t  tt t td dt t t  td   tksćJ t	t
   Ą t ttt t d  ttd dt t t  td   d  tt t d td dt t t  td   tks.J t	  t Ąt ttd dt t t  td  tksMJ t	   t Ąt tt t t  tt t td dt t t  td   tks{J t	t
 t
 }|jsJ |jrJ |jsJ t	 }|jsĽJ |jsŤJ |jrąJ t	  t
  }|jsÂJ |jsČJ |jrÎJ d S )Nr(   r)   rh   c                      s      S rl   r4   r4   ŠrM   rN   rP   r4   r5   r6   <  r7   z'test_Series_functions.<locals>.<lambda>c                      s      S rl   r4   r4   r)  r4   r5   r6   =  r7   c                      râ   r9   rŘ   r4   r   r4   r5   r6   >  rÚ   Tri   F)r   r3   rđ   r   r   rB   r<   r@   rA   r   r   r'   rJ   rŁ   Úrewriterď   ró   rő   )rR   ÚS1ÚS2ÚS3r4   ŠrM   rN   rO   rP   r5   Útest_Series_functions(  sx   ($2˙  "&*(*:0("&˙6p˙*@˙&t
˙@
˙@
˙n
˙>@
˙r/  c               
      sJ	  t tttgt t tgg t t gt gt ggt t gg} t t t gt tggt tt gt t ggt t gtggt tgt gg}   tt ks^J   |  tt |ksqJ    | tt  | ksJ    tt ksJ      tt t ksłJ tt fdd tt fdd ttfdd ttfdd tt fdd tt fdd tt fdd tt fd	d   |   t|   ksJ    |  t|   ks*J ttfd
d ttfdd ttfdd ttfdd ttfdd t ddt  	Ą   krJt t
td  tt t d  td dt t t  td  d  t t t tt t  td dt t t  td  d   tt t d  tt t d td dt t t  td  d  tft
td tt t d  td dt t t  td  d  t t t tt t  td dt t t  td    tt t tt t  td dt t t  td    tt t d td dt t t  td  d  tffksMJ  J tddt  dt dtd  dt  d  gg}tdt gdtd  dt  d dt  gg}t  |tĄt  |tĄ}}t|| 	Ą t t
dtd d  td dt  d d  td d td dt  d d  tffksÁJ t|| 	Ą  Ą t t
ddtffks×J t|    ddt|     	Ą   krĆt t
td tt t d  td dt t t  td  d  tt t d td dt t t  td  d   tt t d  tt t d td dt t t  td  d  tft
td  tt t d  td dt t t  td  d  t t t tt t  td dt t t  td    tt t tt t  td dt t t  td    tt t d td dt t t  td  d  tffksÉJ  J ttddtt 	Ą   kr t t
t t t t t t t td dt t t  td     tt t td dt t t  td   tt
tt t ttt t   t  tt t tft
t t t t t t t td dt t t  td     tt t td dt t t  td   tt
t t t t t ttt t   t  tt t d tff  kr tt t ĄksŁJ  J d S )Nc                      ó     t  S rl   Šr  r4   ŠÚtfm1Útfm2r4   r5   r6   u  r7   z+test_MIMOSeries_functions.<locals>.<lambda>c                      r0  rl   )r@   r4   r2  r4   r5   r6   v  r7   c                      s     t d  S rÝ   r   r4   ŠÚtfm4Útfm6r4   r5   r6   w  rÚ   c                      s      d S rŢ   r4   r4   r5  r4   r5   r6   x  rv   c                      s   dt d     S )Nrh   r+   rŕ   r4   r2  r4   r5   r6   y  r;   c                      s       S rl   r4   r4   Šr3  r4  r6  r4   r5   r6   }  rv   c                      s       S rl   r4   r4   Šr3  r4  r6  Útfm5r4   r5   r6   ~  rÚ   c                      s       S rl   r4   r4   r9  r4   r5   r6     rÚ   c                      ó     t  S rl   r1  r4   Šr6  r:  r4   r5   r6     r7   c                      r;  rl   ŠrA   r4   r<  r4   r5   r6     r7   c                      s      t d  S r   r   r4   r<  r4   r5   r6     r   c                      s     d S rč   r4   r4   r<  r4   r5   r6     r7   c                      s   t d  d    S )Nr,   r(   rŕ   r4   r<  r4   r5   r6     ó    Tri   r)   r(   r,   )r   r  r  r  r   r   r'   rJ   rK   rŁ   r   r   rB   r3   r<   r   rđ   r   r  r   r*  )Útfm3Útfm7Úmat_1Úmat_2Útm_1Útm_2r4   )r3  r4  r6  r:  r7  r5   Útest_MIMOSeries_functionsf  s    &*(2"""4˙Ž4˙ţ˙20V
˙,
˙4˙°4˙ţţ
˙~˙˙˙p>˙˙
ţürE  c                     sŔ  t ttd  ttd   tt  ttd  ttd   tt t  t t tt t tt t tt tt tt  t ttt dtd dt	 t
 t  t
d  ttdt} tdt}t }|j fksnJ |jtksuJ tt  }|jt  fksJ |jtksJ t | dt}t | dt}t t|| }|j t|| fksˇJ t }|j fksÇJ t||}|j||fksŐJ |jtksÜJ t }	||	ksçJ ||kríJ tt }
|
jt fksJ tt  }|jt  fks!J tt fdd tt fd	d ttfd
d tt fdd ttfdd ttfdd d S )Nr,   r)   r*   r(   r÷   rř   c                      rů   rl   Šr   r4   rű   r4   r5   r6   Ď  rp   z,test_Parallel_construction.<locals>.<lambda>c                      rü   rl   rF  r4   rý   r4   r5   r6   Đ  rv   c                      s   t   S rl   rF  r4   rÜ   r4   r5   r6   Ń  r7   c                      r˙   r   rF  r4   r   r4   r5   r6   Ň  r7   c                      r  r   )r   r3   r<   r4   rţ   r4   r5   r6   Ó  r  c                      r  r  )r   r   r4   r   r4   r5   r6   Ô  r   )r   r@   r3   rA   rB   rC   r<   rD   rI   rđ   r   r   r   r?   r  r   r'   rJ   rK   )r  r  Úp0r¸   r
  rQ   rš   rÁ   rÂ   Úp5Úp6Úp7r4   rý   r5   Útest_Parallel_constructionŠ  sD   H(


 rK  c                     sţ  t tgtgtgg t t gtgtggt tggt tgtgtggt ttgttgg} t ttgttgg}t  tdt ggtĄt }|j fksSJ |j	t
ksZJ |j|j|jf  krjdksmJ  J tt }|jt fksJ |j	t
ksJ |j|j|jf  krdksJ  J t t}|j tfksąJ |j	t
ks¸J |j|j|jf  krČdksËJ  J t }|j fksŰJ |j|j|jf  krëdksîJ  J t|| }|j|| fksüJ |j|j|jf  krdksJ  J tt  }|jt  fks.J |j|j|jf  kr@dksCJ  J ttdd  tt fdd tt fdd ttfdd tt fd	d ttfd
d d S )Nr(   )r,   r(   Šr)   r)   c                   S   s   t dS )Nr4   Šr   r4   r4   r4   r5   r6   ü  r   z0test_MIMOParallel_construction.<locals>.<lambda>c                      s   t  tS rl   )r   r  r4   r2  r4   r5   r6   ˙  r7   c                      s   t  S rl   rM  r4   )r3  r?  r6  r4   r5   r6     r7   c                      rů   rl   rM  r4   )r?  r@  r4   r5   r6     rp   c                      r˙   r   rM  r4   )r3  r6  r4   r5   r6     r7   c                      s   t td tt    S r   )r   r3   r<   r4   )r4  r6  r4   r5   r6   	  r  )r   r  r  r  r  r   r<   r   r?   r  r3   r  r   r!  r   r'   rK   rJ   )r:  r7  Úp8Úp9Úp10Úp11Úp12Úp13r4   )r3  r4  r?  r6  r@  r5   Útest_MIMOParallel_construction×  sD   
&&&&
*"*rT  c                     sô	  t dtd dt t t  td  t t tdtt tt t tt t tt tt tt  t ttt ttd  tt  t tt t}    t	 ksVJ    |  t	 | ksgJ    |  t	  |  kszJ    t	 t
ksJ    t	 t
 ksJ    t	   ksŠJ    t
t ddtt	 ksźJ    t
t	 ksĚJ   |   t
t	 | ksßJ   |   t
t ddtt	|  ks÷J     |  t	t
 | ksJ      t	t
  ks&J   |  |   t
t	 | t	| ks?J tt fdd tt fdd ttfdd t	 dd	t	  Ą   krt ttd dt t t  td   d td dt t t  td  tksJ  J t	 t
  dd	t	 t
   Ą   krt ttt t  td dt t t  td  d  t t t td dt t t  td    tt t td dt t t  td    tt t td dt t t  td  d  tksJ  J t	  dd	t	   Ą   krt tt ttt t  td dt t t  td    t t t t td dt t t  td    tt t td dt t t  td   tksJ  J t	  d
d	t	   Ą krJ t	t
 t
 Ą t ttt t  td dt t t  td   ttt t   tt t td dt t t  td   tksăJ t	     Ą t t t ttt t  td dt t t  td    t t t t td dt t t  td    tt t td dt t t  td   tks?J t	  Ą  t t t ttt t  td dt t t  td    t tt t td dt t t  td    tt t td dt t t  td   tksJ t	t
   Ą t ttt t  td dt t t  td   ttt t   dt t dt  td dt t t  td    tt t td dt t t  td   tksúJ t	  t Ąt ttd dt t t  td   d td dt t t  td  tks+J t	   t Ąt tt ttt t  td dt t t  td    t t t t td dt t t  td    tt t td dt t t  td   tksJ t	 t	t	   kr˘t	t	 ksĽJ  J t	t
 t
}|jsśJ |jrźJ |jsÂJ t	   }|jsĐJ |jrÖJ |jsÜJ t	  t
 }|jsěJ |jrňJ |jsřJ d S )Nr(   r)   rh   c                      ó      S rl   r4   r4   r)  r4   r5   r6      r7   z)test_Parallel_functions.<locals>.<lambda>c                      s      S rl   r4   r4   r)  r4   r5   r6   !  r7   c                      r×   r9   rŘ   r4   r   r4   r5   r6   "  rÚ   Tri   F)r   r3   rđ   r   r   rB   r<   r@   rA   r   r   r'   rJ   rŁ   r*  rď   ró   rő   )rR   ÚP1ÚP2ÚP3r4   r.  r5   Útest_Parallel_functions  sś   ($"& "& &0.02"J˙
˙4,˙*˙˙˙˙ţ˙*j,˙˙&n
˙l,˙
˙j,˙
˙h˙,˙
˙J
˙^˙,˙
˙@rY  c               
      sě  t tt tt  t tt} t ttd  tt  t tt t}ttgtgt	gg tt g|gt ggt|g| gtggtt| gt|ggtttgt	| ggtt gg}t| g|  g| gg   t
   krt
t
 ksJ  J    t
   ksJ    | |   t
 t| |  ks°J      |  t
  t|   ksĆJ     t
   ksŮJ      t
    ksěJ tt fdd tt fdd ttfdd ttfdd ttfdd ttfdd tt fd	d tt fd
d tt fdd   | t|t
 ks[J  | |  t| |t
 ksrJ    ||  tt
||t
   ksJ ttfdd ttfdd ttfdd tt fdd tt fdd ttfdd tt fdd t
 ddt
  Ą   krqt
  tĄ  krqtt t td dt t t  td   d td dt t t  td  tft t ttd   tt  ttt   tt tft t t t tt t td dt t t  td    tt t td dt t t  td   tffkstJ  J d S )Nr)   c                      s     t  S rl   )r  r4   r2  r4   r5   r6   f  r7   z-test_MIMOParallel_functions.<locals>.<lambda>c                      s     t  S rl   r=  r4   r2  r4   r5   r6   g  r7   c                      s     t d  S rÝ   r   r4   Šr4  r?  r4   r5   r6   h  rÚ   c                      s      d S rč   r4   r4   rZ  r4   r5   r6   i  rv   c                      s   dt d     S rß   rŕ   r4   rZ  r4   r5   r6   j  r;   c                      s      S rl   r4   r4   )r4  r?  r@  r4   r5   r6   l  r7   c                      s      S rl   r4   r4   )r3  r4  r@  r4   r5   r6   m  r7   c                      rU  rl   r4   r4   r8  r4   r5   r6   o  r7   c                      s      S rl   r4   r4   r8  r4   r5   r6   p  r7   c                      s     t  S rl   r1  r4   r<  r4   r5   r6   u  r7   c                      s     t  S rl   )rB   r4   rZ  r4   r5   r6   v  r7   c                      s     t d  S )Nr.   r   r4   rZ  r4   r5   r6   w  rÚ   c                      s      d S r8   r4   r4   )r3  r4  r?  r4   r5   r6   x  rÚ   c                      s   dt d     S rß   rŕ   r4   )r3  r?  r4   r5   r6   y  r;   c                      s       S rl   r4   r4   )r4  r?  r6  r:  r4   r5   r6   |  rÚ   c                      s      S rl   r4   r4   )r3  r4  r:  r4   r5   r6   ~  r7   Tri   r(   )r   r@   r<   rA   r3   rB   r   r  r  r  r   r   r'   rJ   rK   rŁ   r*  r   r   rđ   )rP   rR   r7  r4   )r3  r4  r?  r6  r:  r@  r5   Útest_MIMOParallel_functionsT  sZ   $8 6,&&".6"
˙PJ˙,˙˙ţr[  c                     s  t dtd dt t t  td  t t tdtt tt t tt t tt tt tt  t ttt ttd  tt  t tt tt tt tt t} t	t ddt   }|j
t ddtt dfksqJ |jt ddtks|J |jt ksJ |jtksJ t	  }|j
 tdfks˘J |j ksŠJ |jtksłJ |jtksşJ t	  }|j
t dfksÎJ |jt ksŘJ t	| }|j
| dfksçJ |jksîJ |jtksőJ t	t ddt}|j
t ddtdfksJ |jtksJ |t	ksJ t	t ddt}|j
t ddtdfks6J |jtks>J t	|  t ddt }|j
tt ddtt| t ddt dfksdJ |jtt ddtt| ksvJ tt fdd tt fdd ttdd  ttdd  ttfd	d tt fd
d tt fdd ttfdd d S )Nr(   r)   rh   c                      s   t   S rl   Šr   r4   ŠrM   rN   rO   r4   r5   r6   ł  rv   z,test_Feedback_construction.<locals>.<lambda>c                      r  r9   )r   r   r4   rŮ   r4   r5   r6   ´  r   c                   S   s   t tddttd S rÝ   )r   r   r3   r4   r4   r4   r5   r6   ľ  r>  c                   S   s
   t ddS rÝ   r\  r4   r4   r4   r5   r6   ś  rp   c                      s   t   S rl   r\  r4   )rN   rP   rR   r4   r5   r6   ¸  rv   c                      ó   t  dS )Nç      ř?r\  r4   ŠrM   rN   r4   r5   r6   š  r7   c                      s   t   d  S )Nrh   r\  r4   rŮ   r4   r5   r6   ş  rÚ   c                      rů   rl   r\  r4   r   r4   r5   r6   ť  rp   )r   r3   rđ   r   r   rB   r<   r@   rA   r   r?   r   Úsys1Úsys2r  r'   rK   rJ   )rT   Úf1Úf2Úf3Úf4Úf5Úf6Úf7r4   )rM   rN   rO   rP   rR   r5   Útest_Feedback_construction  sR   ($$
4$rj  c               	      s  t ddt t dtd dt t t  td  tt tdtt tt t tt t tt tt tt  t ttt ttd  tt  t tt t} t tt tt t}   |  t	t
d| ksoJ   |   t	   Ą t
|   Ą dksJ     t ksJ       t   ksŻJ      t ksÂJ      t  ksŐJ    |    t | ksęJ    |     t |  t |  fv sJ t ddt|   t|ks!J |  |   t|  ks/J ttfdd tt fdd t    Ą t tt t td dt t t  td   ttt t  tt t td dt t t  td    tksJ t   jdttt t  tt t td dt t t  td    d  ksšJ t  Ą t tt t td dt t t  td   ttt t  tt t td dt t t  td    td dt t t  td   tksJ t jdttt t  tt t td dt t t  td    d  ks8J t |  Ą t ttt  td dt t t  td   tt ttd   tt   tt td dt t t  td    td dt t t  td   tksJ t | djdt t ttd   tt   tt td dt t t  td    d  ksĂJ t| Ą t ttt  tt tt  t  tttt  t t tt tt  t    tksöJ t| t ddt Ą  t t t t  tt  tt tt  t  ttt  ttt  t t tt tt  t    tks:J t   Ą t ddtksJJ t|   t Ąt tt td dt t t  td   tt ttd   tt   tt td dt t t  td    td dt t t  td   tks J tt ddt t Ąt ttt t tt  t tksżJ d S )Nr(   r)   rh   c                      s      d     S rÝ   r4   r4   r]  r4   r5   r6   Ň  ó    z)test_Feedback_functions.<locals>.<lambda>c                      s         S rl   r4   r4   rý   r4   r5   r6   Ó  r  )r   r3   rđ   r   r   rB   r<   r@   rA   r   r¤   rŁ   r   r'   rK   rJ   Úsensitivityr*  )rR   rT   r4   )rL   rM   rN   rO   rP   r5   Útest_Feedback_functionsž  s   ($.<*&&*F(:*˙˙
˙B
˙:*˙˙˙
˙B
˙H&˙˙˙
˙N
˙P
˙d
˙ D&˙˙˙
˙˙rm  c            	      C   sř  t td dt} t td dt}t| |d}t| |d}t ddt}t|t s)J t|ts0J |j| ks7J |j| ks>J |jt|t|| ksKJ |jt|t||  ksYJ t| |}t| |}| | |kskJ | | |kssJ | Ą t td d td td  d tksJ | Ą t ttd td td  d   d td td  d tksŻJ t| | |d}|tt| ||ksĂJ |j| | ksĚJ |jt|t|t| |ksÜJ t | | |t}|t t| ||tksńJ |j| | ksúJ d S )Nr(   r)   rh   )	r   r3   r   Ú
isinstancer=   r>   r   r   rŁ   )	rM   rN   Úfd1Úfd2Úunitr	  r¸   Úfd3rO   r4   r4   r5   Ú!test_Feedback_as_TransferFunctionń  s0   

0H rs  c                  C   sć  t dddd\} }}}}}t ddd\}}}| ||d   | t | }	|| }
|||  }|d t ||  }t| |
t d  |	td  d t}td|td  tt  t}td|t t}t|t dt}t|tt}tddt}t|| |}t|| | |}|tt|t|||ksJ |jt|t||ksĄJ |jt	|t|t|t||ks´J || t td  |d  ||  |td  tt  d  t | | td | |d |     | t | | t |d  |t |  t| ||  |td  tt   t | | td | |d |       td | |td  tt   | t | | t |d  |t |  t| ||  |td  tt   t | | td | |d |       || t t | || d  |td  tt   t | | td | |d |     td | |td  tt   | t | | t |d  |t |  t| ||  |td  tt   t | | td | |d |         ||   }| 
Ą |  Ą d	ksńJ d S )
NzI_b, I_s, m, h, l2, l1T)ÚrealÚnonnegativezK_D, K_P, v)rt  r)   r(   r,   r   )r   rG   r   r3   rł   r   r   r=   r>   r   r   r   )ÚIbÚIsÚmÚhÚl2Úl1ÚKDÚKPÚvÚtau1_sqÚtau2Útau3ÚKÚGthetaÚGdeltaÚGpsiÚDcontÚPIcontÚGunityÚGinnerÚGouterÚexprr4   r4   r5   Útest_issue_26161  sR   ˙&&H˙:
˙H˙˙$˙FH˙˙˙ý˙r  c                  C   s  t dtt} t ttd d t}t ttd t}t ttd d t}t| |g||gg}t||g|| gg}t||g| |gg}t||}|j||dfksOJ |j|ksVJ |j|ks]J |jtksdJ |jdkskJ |  |kssJ t||d}|j||dfksJ |j|ksJ |j|ksJ |jtksJ |jdksJ t|t	||}	|	j|t	||dfks´J |	j|ksťJ |	jt	||ksĹJ |	jtksĚJ |	jdksÓJ t
ddt gddgg}
t |
tĄ }}t||}|j||dfksöJ |j|j  kr|ksJ  J d S )Nr(   r,   r)   rh   r   )r   r3   r   r   r?   ra  rb  r  Úsignr   r   r  )rM   rN   rO   rP   r  r  r  rc  rd  re  Úmatra  Ú
controllerrf  r4   r4   r5   Útest_MIMOFeedback_construction6  s>   

(r  c                     s¨  t dtt t ttd d tt ttd t} t ttd d t}t ddt}t dtd t}t g| |ggt| g| ggtjtdtdt |g||ggt|  | g| |ggt|  ggt| |ggtt fdd ttfdd ttfd	d ttfd
d ttfdd ttfdd ttfdd ttfdd tjtdtd	tt	fdd d S )Nr(   r,   r)   rh   Šr  c                      rů   rl   Šr   r4   r`  r4   r5   r6   o  rp   z*test_MIMOFeedback_errors.<locals>.<lambda>c                      s   t t S rl   )r   r   r4   )r  r  r  r4   r5   r6   p  rÚ   c                      ó   t  dS rÝ   r  r4   )r  Útfm_6r4   r5   r6   r  r7   c                      s
   t   S rl   r  r4   )Útfm_7r4   r5   r6   s  rp   c                      r  )Nr   r  r4   r  r4   r5   r6   u  r7   c                      r^  rÝ   r  r4   Šr$  r  r4   r5   r6   w  r7   c                      s   t   S rl   r  r4   r  r4   r5   r6   x  r7   c                      s   t   dS rÝ   r  r4   r  r4   r5   r6   y  r7   c                      r  rÝ   r  r4   )r  Útfm_8r4   r5   r6   |  r7   )	r   r3   r   r  r   r'   rK   rJ   r<   )rO   rP   rR   rT   r4   )
rM   rN   r  r  r  r$  r  r  r  r  r5   Útest_MIMOFeedback_errors]  s.   r  c            
      C   sN  t dtt} t ttd t}t ddt}t dtd t}tjtdtd}t| |g||gg}t| |g||gg}t| |g| | gg}t||}t|t|| d}	|jtt	j
dgdt	j
ggkseJ |	jtdtd  td  td t d  dtd  td  td t d  gtd  tggksJ | Ą tt ddt tt ddtft ddtt ddtff  krÁ| tĄksÄJ  J |	jd	d
dtt td  dtd   dtd   td  td dtd   dtd   dtd   t tt dtd  dtd   td t d tft ddtt td  t dtffks&J |	jd	dtt tdtd  td   td t d  dtd  td  td t d   ttd t d d  tt dtd  dtd   td t d tft ddtt td  t dtffksJ |	 Ą tt tdtd  tdt d   d  td t d tt dtd  td  td t d tft ddtt tdt  dtffksŐJ |	jd
dtt td  t td t d tt dtd  dtd   td t d tft ddtt td  t dtffksJ | Ą  |  Ą ks%J d S )Nr(   rh   r)   r  r   r   r*   r,   FT)Úcancelr   r+   )r  )r   )r   r3   r   r  r   r   r   rl  r   r
   ÚHalfrŁ   r*  )
rM   rN   rO   rP   r  r  r  r$  ÚF_1ÚF_2r4   r4   r5   Útest_MIMOFeedback_functions  st   
"("˙˙
˙

˙˙˙ý^*˙ ˙
˙
*"˙˙*˙ ţ
˙8&˙˙
˙
N ˙
˙r  c                     s  t ttd  tt  t tt tt tt tt  t tt tt gg} | j| j	| j
f  kr7dks:J  J | jtttt ksHJ | jtksOJ ttt gtgg}|j|j	|j
f  krjdksmJ  J |jttttt ttksJ |jtksJ tttgt g tgg}|j|j	|j
f  krŚdksŠJ  J |jttttttt t tksÁJ |jtksČJ tt fdd tt fdd ttfdd ttfd	d ttd
d  ttdd  ttfdd ttdd  d S )Nr)   )r(   r(   rL  )r,   r)   c                      s   t tgtg ggS rl   Šr   r  r  r4   r   r4   r5   r6   ť  r;   z:test_TransferFunctionMatrix_construction.<locals>.<lambda>c                      s   t t gtggS rl   Šr   r  r  r4   r   r4   r5   r6   ź  r;   c                      s   t tgt ggS rl   r  r4   r   r4   r5   r6   ż  r   c                      s   t ttg ggS rl   r  r4   r   r4   r5   r6   Ŕ  r   c                   S   s   t ttgttddgggS ŠNr(   r)   )r   r  r  r  r   r4   r4   r4   r5   r6   Ă  rk  c                   S   s   t ttddggttggS r   )r   r  r   r  r  r4   r4   r4   r5   r6   Ä  rk  c                      s   t tt gS rl   r  r4   r   r4   r5   r6   Ç  rv   c                   S   s
   t tgS rl   )r   r  r4   r4   r4   r5   r6   Č  rp   )r   rA   r3   rB   r@   r<   r   r  r  r   r!  r?   r   r  r  r  r'   rJ   rK   )Útfm3_r:  r@  r4   r   r5   Ú(test_TransferFunctionMatrix_construction§  s,   $&&&&0r˘  c               
   C   sş  t ttd  tt  t tt t} tttd  td  td d  dgtttd  ttd d   gg}tdt d td d  gg}tddgddgg}t |tĄtt ttd  td  td d tt ddtgt tdtt tttggks~J t |tĄtt dt d td d tggksJ t |tĄtt ddtt ddtgt ddtt ddtggksşJ tt	gt
gg}| tt	 gt
 ggksŃJ tt	t
tg| t	 t gg}| tt	 t
 t g|  t	tggksôJ t |tĄ}tt tt t ttd  tt tt ttd t  tgg}| tdĄtt ttd  td  td d tt ddtgt ddtt dttggksHJ | tdiĄtt ttd  td  td d tt ddtgt ddtt dttggksxJ | tdtdiĄtt ttd  td  td d tt ddtgt ddtt dttggksŞJ | tdĄtt dt t ttd  tt dt tt td   tggksÓJ | tdĄtt tt t td tt tt t td  tggksřJ | tdĄtt dttd  tt tt ttd  tggksJ | tdtdttiĄtt tt td tt tt td  tggks@J | tdd t Ątdt t dt  dt t t dt   tt t t dt d  tt   tdt t dt d  dt t    ggksJ | Ą tt ttd  td  td d tt tdtgt ddtt tttggksťJ | Ą tt tt t ttd  tgt tt tt td   tggksăJ | Ą  Ą |ksîJ | Ą  Ą |ksůJ | Ą td d tdt d  td d tdt d  tdd tdt d  tdd tdt d  gg gg dgggks?J | Ą ddgtttt gggksTJ | Ą tt ttd td    tt ttd td    gg t t ggt gtt ttd td    tt ttd td    gt t gggks§J | Ą g d	˘g gg g ggks¸J | Ą dgdgggksĆJ | Ą g g tt ggt dt  tdt t td  dt   t dt  tdt t td  dt   gg tt gggks	J ttt dtd d tt td dt  d dtt dttgg}ttt td d dtd  td  dt  d tt dtd  dtd  td  dt  d tgg}	| Ą tt td dt  d ttd d  tggksJ |	 Ą tt ddtd  td  dt  d tggksJ | Ą t ttd  td  td d tt ddtt tdtt tttgksÇJ | Ą t tt t ttd  tt tt tt td   tgksëJ | Ą tt dtd d tt td dt  d dtt dttgksJ |	 Ą tt td d dtd  td  dt  d tt dtd  dtd  td  dt  d tgksJJ | Ą tt ttd
  td  td d tt ddtft dt dtt tttffksyJ | tdtdtdiĄ Ą tt dttt dt dtd  d tffks J tt td td  t ttd  tt td td  td td  tgg}
|
 Ą t|
  krôtt td  td  d td tt td td tffks÷J  J | Ą tt td dtd   dt  td d tt ddtft tdtt tttffks(J |
 Ą tt td td  t td  t tt td dt  d td dt  d tffks[J d S )Nr)   r(   r,   r*   r-   r   r+   r   )rh   r   r,   r}   rx   ry   gTăĽÄ 	@g
×Łp=
@gÖ˙9Ě@gTăĽÄ @) r   rA   r3   rB   r@   r   r<   r   r  r  r  r  rE   r   r   r   r
   r   r   Ú	transposeÚ
elem_polesr   r   rđ   Ú
elem_zerosr   r   rŁ   Ú_flatr   r   r   )rR   rA  rB  Úmat_3r3  r4  ÚH_1ÚH_2ÚH_3ÚH_4ÚH_5r4   r4   r5   Ú%test_TransferFunctionMatrix_functionsĘ  s    $"ţ 
0˙˙
"˙
˙˙*>^`dRJDLZPx˙*NL˙"f˙Dj>>RHLrN
˙˙
˙$"˙:˙8˙
˙V˙r­  c                  C   sÔ  t dtt t t} t| td\}}t t|d  |d  t|d  |d  t}t tt dttt d    tdttt d     tt tt d  ttt d    t}tj| Ą | Ą   Ą j	ksgJ t dtt t t} t| td\}}t |d t|d  |d  t}t tt tt tt  t  t}tj| Ą | Ą   Ą j	ksŞJ t dtt t t} t| td\}}t t|d  t|d  |d  t}t tt ttt   ttttt    t}tj| Ą | Ą   Ą j	ksôJ t dtt t t} t| td\}}t t|d  |d  t|d  |d  t}t td t dtdt t d    dt dtdt t d     tt dt t d  tdt t d    t}tj| Ą | Ą   Ą j	kshJ d S )	Nr(   r/   r   r)   g333333Ó?r,   r   r1   )
r   rE   r3   rF   r!   ÚTr
   ÚZeror   r=   )rL   ÚnumZÚdenZÚtf_test_bilinearÚtf_test_manualÚtf_test_forwardÚtf_test_backwardÚtf_test_gbtr4   r4   r5   Útest_TransferFunction_gbt2  s(   ,^  " $, ,v&rˇ  c                  C   sĐ   t dtt t t} t| t\}}t t|d  |d  t|d  |d  t}t tt dttt d    tdttt d     tt tt d  ttt d    t}tj| Ą | Ą   Ą j	ksfJ d S )Nr(   r   r)   )
r   rE   r3   rF   r"   rŽ  r
   rŻ  r   r=   )rL   r°  rą  r˛  rł  r4   r4   r5   Útest_TransferFunction_bilinearX  s
   ,^$r¸  c                  C   s   t dtt t t} t| t\}}t |d t|d  |d  t}t tt tt tt  t  t}tj| Ą | Ą   Ą j	ksBJ d S ŠNr(   r   )
r   rE   r3   rF   r#   rŽ  r
   rŻ  r   r=   )rL   r°  rą  r´  rł  r4   r4   r5   Ú"test_TransferFunction_forward_diffc  s
    "$rş  c                  C   s   t dtt t t} t| t\}}t t|d  |d  t|d  |d  t}t tt ttt   ttttt    t}tj| Ą | Ą   Ą j	ksMJ d S rš  )
r   rE   r3   rF   r$   rŽ  r
   rŻ  r   r=   )rL   r°  rą  rľ  rł  r4   r4   r5   Ú#test_TransferFunction_backward_diffn  s
   ,,$rť  c                     sŢ   t dtd d t} t td dt}t dtt t t t td ttt  td d tt|g ggt| ddt	dt
d  t  ksJJ t|dksRJ ttfd	d
 tt fdd
 ttfdd
 d S )Nr   r,   r(   r)   iL˙˙˙é´   é   r   c                      ó   t  S rl   Šr%   r4   r   r4   r5   r6     r   z4test_TransferFunction_phase_margin.<locals>.<lambda>c                      rž  rl   rż  r4   r   r4   r5   r6     r   c                      ó   t t S rl   )r%   r   r4   ŠÚtf_mr4   r5   r6     r7   )r   r<   r3   rE   rF   r   r   r   r%   r   r   r   r'   rĄ   rJ   r`  r4   ŠrO   rP   rÂ  r5   Ú"test_TransferFunction_phase_marginy  s   $(rÄ  c                     sţ   t td dtd  td  td  t} t td dt  d dt}t dtt t t t td ttt  td d tt|g ggt| dtt	dd  td ksZJ t|t
ksbJ ttfdd	 tt fd
d	 ttfdd	 d S )Nr)   r+   r(   r   iě˙˙˙r1   i  c                      rž  rl   Šr&   r4   r   r4   r5   r6     r   z3test_TransferFunction_gain_margin.<locals>.<lambda>c                      rž  rl   rĹ  r4   r   r4   r5   r6     r   c                      rŔ  rl   )r&   r   r4   rÁ  r4   r5   r6     r7   )r   r3   rE   rF   r   r   r   r&   r   r
   r   r'   rĄ   rJ   r`  r4   rĂ  r5   Ú!test_TransferFunction_gain_margin  s   ($(rĆ  c                  C   sĘ  t ddgddgg} t ddg}t ddgg}t dg}t| |||}|jt ddgddggks2J |jt ddgks=J |jt ddggksIJ |jt dgksSJ |jt ddgddggt dgdggt ddggt dggfksuJ tt tgt tgt t	gt t
g}|jt tggksJ |jt tggksJ |jt t	ggksŠJ |jt t
ggks´J |jt tggt tggt t	ggt t
ggfksĎJ tt ddgddggt ddgddggt ddgddggt ddgddgg}|jt ddgddggksJ |jt ddgddggksJ |jt ddgddggks&J |jt ddgddggks6J |jt ddgddggt ddgddggt ddgddggt ddgddggfksbJ t ttgt	t
gg}t ttgttgg}t ttgttgg}	t ttgttgg}
t|||	|
}|jt ttgt	t
ggksĄJ |jt ttgttggksąJ |jt ttgttggksÁJ |jt ttgttggksŃJ |jt ttgt	t
ggt ttgttggt ttgttggt ttgttggfksýJ t }|jt dggt dggt dggt dggfksJ t ddgddgg}t ddg}t||}|jt ddgddggksAJ |jt ddgksMJ |jt ddggksZJ |jt dggksfJ |jt ddgddggt dgdggt ddggt dggfksJ |jdksJ |jdksJ |jdksĄJ |jdksŠJ |jdksąJ |jdksšJ ttd	d
  ttdd
  ttdd
  ttdd
  ttdd
  ttdd
  d S )Nr   r(   ç      řżr   r/   r)   TFc                   S   s@   t tttd d gttd gttd d gtdt gS r   Šr    r   r3   r4   r4   r4   r5   r6   ő  ó     ˙z.test_StateSpace_construction.<locals>.<lambda>c                   S   s@   t ttgttd td d gttd d gtdt gS )Nr(   r,   r)   rČ  r4   r4   r4   r5   r6   ÷  rÉ  c                   S   sL   t ttgttd gttd d gtd dt  d ggtdt gS r   rČ  r4   r4   r4   r5   r6   ů  s    2˙c                   S   sX   t tt t gtdggttd dgdtggtdtggtdt dt gttggS )Nr   r)   rČ  r4   r4   r4   r5   r6   ű  s
    ýc                   S   s   t td td dt dS r   )r    r3   r4   r4   r4   r5   r6     r  c                   S   s"   t tddgtdgtdgdS )Nr)   r/   rh   r(   r   )r    r   r4   r4   r4   r5   r6     s    
˙)r   r    Ústate_matrixÚinput_matrixÚoutput_matrixÚfeedforward_matrixr?   r@   rA   rB   Úa3rC   rD   rI   Úb3Úc0r˝   Úc2Úc3rž   rż   rŔ   Úd3Úis_SISOr'   r   rK   )ÚA1ÚB1ÚC1ÚD1Úss1Úss2Úss3ÚA4ÚB4ÚC4ÚD4Úss4Úss5ÚA6ÚB6Úss6r4   r4   r5   Útest_StateSpace_construction  s¸   
D˙6ý    ˙˙˙˙ú
    ˙˙˙˙ú
8
 ˙˙
ű
rĺ  c            
   	      sŘ  t ddgddgg} t ddgddgg}t ddgddgg}t ddgdd	gg}t| ||| t g d
˘g d˘g d˘g}t ddgddgddgg}t g d˘g d˘g}t ddgddgg}t||||t tt dgt dgt dgt dgtt g d˘g d˘g d˘g d˘g d˘gt ddgddgddgddgddggt g d˘g d˘gt ddgddgg}tt g d˘g d˘g d˘g d˘g d˘gt ddgddgddgddgddggt g d ˘g d!˘gt d"d#gddgg}	  |ksôJ   |	ksüJ d$ tt dggt dggt dggt d$ggksJ d% tt dggt dggt d&ggt d'ggks8J d% tt dggt d(ggt dggt d'ggksVJ tt fd)d* ttfd+d* d S ),Nr*   r(   r)   rz   r+   éü˙˙˙r   r,   rh   )rz   r*   r)   )rh   rz   r   )r)   r+   r,   r   )r*   r)   rz   )r(   r*   r,   )r*   r(   r   r   r   )r)   rz   r   r   r   )r   r   rz   r*   r)   )r   r   rh   rz   r   )r   r   r)   r+   r,   )r)   rć  r*   r)   rz   )r   r(   r(   r*   r,   r.   )rz   r*   r)   r   r   )rh   rz   r   r   r   )r)   r+   r,   r   r   )é   é   i÷˙˙˙r*   r(   )éń˙˙˙iî˙˙˙r   r)   rz   iö˙˙˙rç  ré  )r   r   rz   r)   rć  )r,   r   éú˙˙˙r   r(   rę  r   r/   r_  ç      @g      @r}   c                      rŰ   rl   r4   r4   )rŮ  rŰ  r4   r5   r6   B  r   z%test_StateSpace_add.<locals>.<lambda>c                      rä   rl   r4   r4   )rÚ  rŕ  r4   r5   r6   C  r   )r   r    r'   r   )
rŐ  rÖ  r×  rŘ  ÚA2ÚB2ÚC2ÚD2Úexpected_addÚexpected_mulr4   )rŮ  rÚ  rŰ  rŕ  r5   Útest_StateSpace_add  s   &űűţţđ˙űűţţđ˙<<<rň  c               
   C   s  t ttgttgg} t ttgttgg}t t	t
gt
tgttgg}t ttgttgttgg}t| |||}| }t ddgddgg}t ddg}t ddgg}t dg}	t||||	}
|tt ttgttggt ttgttggt t	 t
 gt
 t gt t ggt t t gt t gt t ggksJ |
 tt ddgddggt dgdggt ddggt dggksťJ | |ksÂJ |
    |
ksĚJ d S )Nrh   r(   )r   r@   rA   rB   rÎ  rC   rD   rI   rĎ  rĐ  r˝   rŃ  rŇ  rž   rż   rŔ   rÓ  r    )ÚAÚBÚCÚDÚSSÚSS_negÚ	state_matÚ	input_matÚ
output_matÚfeedforward_matÚsystemr4   r4   r5   Útest_StateSpace_negationE  sN   

˙˙

ţ

ţů˙
˙
ü˙rţ  c                  C   s  t ttgttd d gttd d gtdt g} t ttt gttd td  gttd td  gttt g}|  tdiĄt tdggtdggtdggtdggks`J | tdiĄt ttd ggtdggtdtd  ggttd ggksJ |  ttiĄt ttggttd d ggttd d ggtdt ggks˛J | ttttiĄt ttt ggttd td  ggttd  td  ggttt ggksĺJ tt t	 }t
td  tt  t }t t|gt|g}t tdt d ggtdtd  d	t  d ggtdggtdgg}t td
t d ggtdtd  dt  d ggtdggtdgg}| t	dtdtdtd	t
diĄ|ks`J | t	dtdtdtd	t
diĄ Ą |ksvJ | Ą |ksJ d S )Nr(   r)   r,   r+   é$   é   r   r   r*   ry   rx   r{   r|   r}   )r    r   r3   r<   r   r   rE   rF   rA   r@   rI   rD   rC   r   )rŮ  rÚ  r¸   rš   ÚGÚexpectÚexpect_r4   r4   r5   Ú!test_SymPy_substitution_functionsg  s$   :F@P@˙N˙JJ(,r  c                  C   sŘ  t ddgddgg} t ddg}t ddgg}t dg}t| |||}| tĄ}|  tĄ}|d d }|d d }|tdt d	 td d
t  d tksPJ |j|j ksYJ |j|jksaJ t g d˘g d˘g d˘g}	t g d˘g d˘g d˘g}
t g d˘g d˘g d˘g}t g d˘g d˘g d˘g}t|	|
||}| tĄ}|d d tdtd  dtd   dt  d dtd  dtd   dt  d tksĎJ |d d tdtd  dtd   dt  d dtd  dtd   dt  d tksýJ |d d tdtd  d t  d dtd  dtd   dt  d tks&J t tĄ}|tt ddgtd  d!t t	 ggt dgdggt ddggt dggksUJ | tĄd d tkscJ t
td"d#  d S )$Nr   rh   r,   r)   r+   r(   r   rw   é;   r.   r0   )rÇ  r   r,   )r(   r   r(   )r)   r(   r(   )r/   r   r(   )r   r(   r)   )r)   r)   r,   )r   r(   r   )r   r)   r(   )r(   r   r)   )r)   r)   r   )r(   r(   r(   )r,   r)   r(   ry   rx   g      %@rë  r/   g      @g      @g      @r{   r   c                   S   s6   t ttd  td  tt  t ttd  t tĄS r   )r   rF   r3   r<   rE   r*  r    r4   r4   r4   r5   r6   Š  s   6 z!test_conversion.<locals>.<lambda>)r   r    r*  r   r3   r=   r>   r  r   rđ   r'   rJ   )rŐ  rÖ  r×  rŘ  rŞ   Útm1Útm2rM   rN   rě  rí  rî  rď  rŤ   Útm3r÷  r4   r4   r5   Útest_conversion  sF   

,
\\R

˙˙
ű
˙r	  c                  C   s  t ddgddgg} t ddg}t ddgg}t dg}t| |||}tt ddgddggt ddgddggt ddgddgg}tt ddgddggt ddgddgg}| Ą d	ks^J | Ą d
ksfJ | Ą t ddgddggksvJ | Ą t ddgddgddgddggksJ | Ą t dgdggt dgdgggks˘J | Ą t dgdgdgdgggksľJ | Ą d	ks˝J | Ą d
ksĹJ | Ą t ddgddggksŐJ | Ą t g d˘g d˘gksĺJ | Ą t dgdggt dgdgggksűJ | Ą t dgdgggksJ t ddgddgg}t dgdgg}t ddgg}	t dgg}
t|||	|
}tt ddgddggt dgdggt ddggt dgg}| |Ą}|j	|j	|j	 ks`J |j
|j
|j
 kslJ |j|j|j ksxJ |jt g d˘g d˘g d˘g d˘gksJ |jt ddgddgddgddggks¤J |jt g d˘g d˘gks´J |jt ddgddggksÄJ d S )NrÇ  r   r(   r   r/   r*   r)   rh   TFr,   rz   g      čż)r(   rh   r)   r   )r   r(   r   r   )r(   r   r   r   )r   r   r(   r   )r   r   r   r(   )r   r    Úis_observableÚobservability_matrixÚobservable_subspaceÚis_controllableÚcontrollability_matrixÚcontrollable_subspaceÚappendÚ
num_statesr!  r   rĘ  rË  rĚ  rÍ  )ÚA_matÚB_matÚC_matÚD_matÚSS1ÚSS2ÚSS3rŐ  rÖ  r×  rŘ  rŮ  rÚ  rŰ  r4   r4   r5   Útest_StateSpace_functionsŹ  sB   
<* ,,&  , <
,, $r  N)|Úsympy.core.addr   Úsympy.core.functionr   Úsympy.core.mulr   Úsympy.core.numbersr   r   r   r   Úsympy.core.powerr	   Úsympy.core.singletonr
   Úsympy.core.symbolr   Ú&sympy.functions.elementary.exponentialr   r   Ú(sympy.functions.elementary.miscellaneousr   Ú(sympy.functions.elementary.trigonometricr   Úsympy.matrices.denser   Úsympy.polys.polytoolsr   Úsympy.polys.rootoftoolsr   Úsympy.simplify.simplifyr   Úsympy.core.containersr   Úsympy.matricesr   r   r   Úsympy.physics.controlr   r   r   r   r   r   r   r   r    r!   r"   r#   r$   r%   r&   Úsympy.testing.pytestr'   rE   r   rF   rł   r3   rG   rH   r<   r   r   rđ   r   rŽ  r@   rA   rB   rÎ  rC   rD   rI   rĎ  rĐ  r˝   rŃ  rŇ  rž   rż   rŔ   rÓ  r  r  r  rg   rÔ   rá   rí   rń   rô   rö   r  r(  r/  rE  rK  rT  rY  r[  rj  rm  rs  r  r  r  r  r˘  r­  rˇ  r¸  rş  rť  rÄ  rĆ  rĺ  rň  rţ  r  r	  r  r4   r4   r4   r5   Ú<module>   sx    D"((a a"++,>C.5H463% '"(#h&lA"-