U
    L?h= ã                   @   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.t| jtd t d ksHt| jtd td t d tfksltt td td t}|jtd kst|jtd ks€t|jtd td tfksÀtt td td d t}|jtd ksêt|jtd d kst|jtd td d tfks$t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tt td td  td td  t}|jtd td  ksÎt|jtd td  td td  tfksütt td td t}|jtd td tfks.tt td dt t}|jtd dt tfks`tt td td t}|jtd td tfkstt ddt}|jdks®t|jdksŸt|jddtfksÔtt ddt}	|	jdksðt|	jdks t|	jddtfkst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t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t|  |  kr&|    ks,n t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Ÿt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@t||ksNtt tt t ttd  tt  t
 t}|jttt  kst|jt
tt  ttd   ks²t|jttt  t
tt  ttd   tfksätt dtd t d t}|jdtd t d tfkstt ddt}|jddtfks@t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s¢t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ttd\}}}}t ||t  |t  td d| t  |d  t}|j|t |t  | ksht|j|t |t  | |d d| t  td  tfksŠt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   úV/var/www/html/venv/lib/python3.8/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dddgttS ©Nr(   r)   r,   )r   r   r3   r4   r4   r4   r5   r6   y   r7   c                   S   s    t td dt  d td dS )Nr)   r(   r,   r2   r4   r4   r4   r5   r6   {   r7   c                   S   s   t td dt dS )Nr(   r+   r*   )r   Úpr4   r4   r4   r5   r6   |   r7   c                   S   s   t dddS )Nr,   r*   r0   )r   r4   r4   r4   r5   r6   }   r7   )r   r3   ÚnumÚAssertionErrorÚdenÚargsr:   Úa0Úa1Úa2Úb0Úb1ÚaÚbÚgÚdÚb2r   r'   Ú
ValueErrorÚ	TypeError)ÚtfÚtf1Útf2Útf3Útf4Útf4_Útf5Ztf5_Útf6Ztf6_Útf7Útf8Ztf7_Ztf8_Útf9Útf10Ztf10_Útf11Ztf12Ztf13Ztf14Ztf15Úomega_oZk_pZk_oZk_iZtf18r4   r4   r5   Ú"test_TransferFunction_construction   s    $"0:$.4444*DN((2($2",6,64 >rY   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@t
ttfdd ttfdd t t¡tttd  dt  td d tkst
t t¡tttd  dt  td d tksÐt
ttfdd t t¡td	dtks t
t |t¡tddt  ddt   d
dt  ddtd    tksJt
t |t¡td
td  dtd   d td td
  tkst
||kst
t |t¡tdtd  dt  d td d tksÔt
t ddgdddgt¡}	td dt g}
td td dg}t |
|t¡}dddgddg |	ttd dtd  dt  d tks\t
|ttd t dt  td td  ttd   d tkst
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t
ttd td  t ttd  t}ttd td  d  td t}| ¡ t|  krt|kszn t
ttd td  td td  t}	ttd td t}|	 ¡ t|	  krÒ|ksØn t
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t
|	 ¡ ttd dt  d td dt  d tks<t
| ¡ |ksNt
| ¡ t||tksht
| ¡ tttt  ttt   |tkst
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>t
| tdtdtdtdtdi¡ ¡ |ksjt
|  ¡ |ks|t
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	s2t
|' |!d|"d|#d|$di¡ ¡ |(k	sZt
|) ¡ |(k	slt
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$t
|* tdtdtdtdi¡ ¡ |+k
sLt
|, ¡ |+k
s^t
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
sÒt
|- tdtdi¡ ¡ |.k
sòt
|/ ¡ |.kst
|	 t¡td td dt  d  dt td dt  d   dtd dt  d   ksdt
| dt ¡tdd tdt d  kst
| 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st
| ¡ dks¢t
|	 ¡ t ddksºt
|' ¡ dksÌt
|. ¡ dksÞt
|) ¡ dksðt
tdtt ¡ t!ks
t
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t
| $¡ g ks¢t
|	 $¡ d"dgksžt
|/ $¡ tgksÌt
|- $¡ t t gksæt
|+ $¡ d#t gksþt
t%|( $¡ t%d$d%d&t  d%d&t  gks.t
t%| $¡ t%d'd(t  d'd(t  gks\t
|1 $¡ t#t dd t#t dd  tt#t dd  t t#t dd  gksªt
|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srt
tt'd)d  td*d+d,\}4}5td-d+d.}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kst
| (¡ d+ks,t
|	 (¡ dks>t
|( (¡ dksPt
| (¡ d+ksbt
|7 (¡ d+kstt
|8 (¡ d+kst
|2 (¡ dkst
|/ (¡ d ksªt
|- (¡ d ksŒt
| )¡ ddgksÒt
| )¡ g ksät
|	 )¡ ddgksút
|/ )¡ dt dd t t dd  d dt dd  t t dd  d/ t*ddt dd  t t t dd  d/  dt dd  t t dd  d/ t*ddt dd  t t t dd  d/  gksÌt
|- )¡ 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^t
t%|+ )¡ t%d0d1t*d2td  d   d1t*d2td  d  d0 gks¬t
|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*t
|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òt
tt'd3d  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sft
 tdt d dt tkst
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.n t
   +¡ d   krht,d  krh|(ksnn t
d t,d  krtddtksn t
t- +¡ d d   krèt,d  krèttd td tksîn t
  +¡ d d4   krDt,d4  krDtdt d td d d tksJn t
ttfd5d ttfd6d ttfd7d 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sàt
|j/ td¡|j0 td¡  krdksn t
| td¡ttd td dt  d tksDt
|9 td¡ttd dtksft
 1tti¡tdt d dt tkst
tt2fd8d  ttd¡ksŸt
tttt  ttt   tt t t}:|: 1tt#i¡ttt#t  ttt#   tt t# t#ks"t
|: tt#¡tttt  ttt   tt t tks\t
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t
|< ¡ t td tdt d	t  dddddks8t
|= ¡ t tdtddddddksbt
|> ¡ tddddks~t
d S )9Nr   éÿÿÿÿF©Úevaluater)   r+   r(   r,   r.   r-   r*   r0   c                      s
   t   ¡S ©N©r   Úfrom_rational_expressionr4   )Úexpr_2r4   r5   r6      r7   z1test_TransferFunction_functions.<locals>.<lambda>c                      s
   t   ¡S r]   r^   r4   )Úexpr_3r4   r5   r6      r7   c                      s
   t   ¡S r]   r^   r4   )Úexpr_4r4   r5   r6      r7   c                      s   t   t¡S r]   )r   Úfrom_coeff_listsr3   r4   )Úden3Únum3r4   r5   r6   š   r7   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)   ro   )r   Úxr?   Úpolesr4   r4   r4   r5   r6     r7   zq, rT)ÚnegativeÚt)Zpositiveé   g      À?gvž-kÁÆñ?gâ3ìÙ¿c                   S   s(   t ttd  t td  td t ¡ S )Nro   r)   )r   r?   rq   Úzerosr4   r4   r4   r5   r6   /  r7   éþÿÿÿc                      s    t d t  d  S ©Nr)   r(   ©r3   r4   ©rO   r4   r5   r6   B  r7   c                      s    t  S r]   ry   r4   ©rQ   r4   r5   r6   C  r7   c                      s     S r]   r4   r4   ©rO   rQ   r4   r5   r6   D  r7   c                      s      ttdi¡S ©Nr)   )Úxreplacer:   r   r4   ©rN   r4   r5   r6   O  r7   )3r   r	   r3   r:   r   r   ÚtauÚto_exprr_   r   r<   r'   ÚZeroDivisionErrorrI   rc   Zfrom_zpkr   r   rA   r@   r?   rB   rC   rH   ÚexpandÚsubsÚevalfr   Úeval_frequencyÚwnr
   Zdc_gainr   r   rq   Úkrr   Ústrr   ÚNotImplementedErrorZ	is_stablerv   r   ÚdoitÚpowr   r   r;   r=   r~   rJ   )?Zexpr_1Zexpr_5Zexpr_6rK   ÚdelayZexpr_7ÚH1ÚH2Zexpr_8rL   Znum2Zden2rM   rv   rr   ZgainZtf0rD   rE   ÚG1ÚG2ÚcrG   ÚeÚfrF   ÚG3ÚG4Úp1Úp2ZSP1Zexpect1Zexpect1_Úc1Úd0Úd1Úd2Úp3Úp4ZSP2Zexpect2Zexpect2_ZSP3Zexpect3Zexpect3_ZSP4Zexpect4Zexpect4_Ztf_Z_tfZTF_Z_TFÚqÚrrt   Z	stable_tfZ
stable_tf_rR   rS   rT   rU   rV   rW   r4   )rd   r`   ra   rb   re   rN   rO   rQ   r5   Útest_TransferFunction_functions   s    4,$66
6ÿ

.ÿ

&ÿ

.B:$"&$& 4,tXH@>:.(((,&(($(400$(  `0âÿ
  0.ND    þ$$$H,ÿPÿN8ÿ&ÿB0ÿD    þ .$ <@0P\ &2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sjt tkst   t ks t  tksŒt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sRt t kslt  t  kst  t ksªt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©Zcommutativec                      s    t dddg S r9   ©r   r4   ©rL   r4   r5   r6   m  r7   z@test_TransferFunction_addition_and_subtraction.<locals>.<lambda>c                      s     S r]   r4   r4   )r   rM   r4   r5   r6   n  r7   c                      s     S r]   r4   r4   ©rN   rO   r4   r5   r6   o  r7   c                      s    t d  S ©Nr(   ry   r4   r€   r4   r5   r6   p  r7   c                      s    d S ©Nr0   r4   r4   r€   r4   r5   r6   q  r7   c                      s   dt d    S ©Nr(   r,   ©r:   r4   r€   r4   r5   r6   r  r7   c                      s    t dddg S r9   r£   r4   r€   r4   r5   r6   z  r7   c                      s     S r]   r4   r4   r¥   r4   r5   r6   {  r7   c                      s    t d  S rŠ   ry   r4   r€   r4   r5   r6   |  r7   c                      s    d S r§   r4   r4   r€   r4   r5   r6   }  r7   c                      s   t d   S )Nr+   ry   r4   )rM   r4   r5   r6   ~  r7   c                      s   dt d    S )Nr(   r*   r©   r4   r€   r4   r5   r6     r7   )r   r3   r:   r   r<   r   r'   rI   r4   r4   )r   rL   rM   rN   rO   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s t   t  ksºt    t  ksÞt        t     kst tks*t     t t ddttks^t    t tkst ttdks€t  t tdksÆt  ttdksêt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.   rZ   r   Fr¢   c                      s    t dddg S r9   r£   r4   ©r   r4   r5   r6     r7   zCtest_TransferFunction_multiplication_and_division.<locals>.<lambda>c                      s     S r]   r4   r4   )r   r   r4   r5   r6     r7   c                      s     S r]   r4   r4   ©r   ÚG5r4   r5   r6     r7   c                      s    t d  S rŠ   ry   r4   ©r­   r4   r5   r6     r7   c                      s   d  S ©Nr-   r4   r4   r®   r4   r5   r6     r7   c                      s    t dddg S r9   r£   r4   r«   r4   r5   r6   ¡  r7   c                      s    d S r8   r4   r4   )ÚG6r4   r5   r6   ¢  r7   c                      s     S r]   r4   r4   r¬   r4   r5   r6   £  r7   c                      s    d S r}   r4   r4   r®   r4   r5   r6   €  r7   c                      s    t d  S r}   ry   r4   r®   r4   r5   r6   ¥  r7   c                      s   t dt d     S )Nr*   r)   ry   r4   )r   r4   r5   r6   Š  r7   c                      s   d  S r8   r4   r4   )r   r4   r5   r6   §  r7   c                      s   d   S rŠ   r4   r4   )r°   ÚG7r4   r5   r6   š  r7   c                      s      S r]   r4   r4   ©r­   r°   r±   r4   r5   r6   ©  r7   c                      s       S r]   r4   r4   r²   r4   r5   r6   ª  r7   )
r   r3   r:   r   r<   r   r   r   r'   rI   r4   r4   )r   r   r   r   r­   r°   r±   r   r5   Ú1test_TransferFunction_multiplication_and_division  sF    $4ÿ
("$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sÈt|jsÒt|jsÜt|jrætd S ©Nzomega_o, zeta, taur)   r,   r*   )r   r   r3   r:   rD   rE   Ú	is_properr<   )rX   Úzetar   r   r   r   r   r4   r4   r5   Útest_TransferFunction_is_proper­  s    ,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rÈt|jrÒt|jsÜt|jrætd S rŽ   )r   r   r3   r:   rD   rE   Úis_strictly_properr<   )rX   r¶   r   rL   rM   rN   rO   r4   r4   r5   Ú(test_TransferFunction_is_strictly_proper¹  s    ,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sÈt|jsÒt|jrÜt|jrætd S )Nztau, omega_o, zetar)   r,   r*   )r   r   r3   r:   rD   rE   Úis_biproperr<   )r   rX   r¶   rL   rM   rN   rO   r4   r4   r5   Ú!test_TransferFunction_is_biproperÅ  s    ,80


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sÜt|jtksêttt  }|jt  fkst|jtks(tt | dt}t | dt}t t||}|j t||fksptt }|j fkstt||}|j||fks°t|jtksÀttt  }	|	jt  fksött }
||
kst||krt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                      s
   t  S r]   ©r   r4   ©rK   rN   r4   r5   r6   ô  r7   z*test_Series_construction.<locals>.<lambda>c                      s   t  S r]   rŸ   r4   ©rK   rM   rN   rO   r4   r5   r6   õ  r7   c                      s   t   S r]   rŸ   r4   ©rM   rN   r4   r5   r6   ö  r7   c                      s   t d S r}   rŸ   r4   ©rK   rO   r4   r5   r6   ÷  r7   c                      s   t td tt   S r}   )r   r3   r:   r4   rÁ   r4   r5   r6   ø  r7   c                      s   t  tddddgS ©Nr(   r)   r,   r*   )r   r   r4   r   r4   r5   r6   ù  r7   )r   r?   r3   r@   rA   rB   r:   rC   rH   r¶   r   r   r   r>   r<   Úvarr   r'   rI   rJ   )ÚinpÚoutÚs0Ús1Útf3_rP   Ús2Zs3Zs4Zs6Z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t|jtks*t|j|j|jf  krLdksRn tt}|jfkstt|jtkst|j|j|jf  krŠdks¬n ttt | }|jt | fksât|j|j|jf  krdks
n t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 r]   ©r   r4   r4   r4   r5   r6     r7   z.test_MIMOSeries_construction.<locals>.<lambda>c                      s
   t  S r]   rÌ   r4   )Útf_1Útfm_1r4   r5   r6     r7   c                      s   t    S r]   rÌ   r4   ©rÎ   Útfm_2r4   r5   r6     r7   c                      s
   t  S r]   rÌ   r4   )Útfm_3Útfm_5r4   r5   r6      r7   c                      s   t d S 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   $  r7   c                      s   t tdt g S rŠ   )r   r   r:   r4   ©rÑ   r4   r5   r6   %  r7   )r   r?   r3   r@   rA   rB   r:   rC   rH   r¶   r   r   ÚTF3ÚTF2ÚTF1Úfrom_Matrixr   r   r>   r<   rÄ   ÚshapeÚnum_outputsÚ
num_inputsr   r'   rI   rJ   )Ztf_2Ztf_3Útfm_4Zs8Zs9Z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rÖt	t	   krÖt	 t	ksÜn t
   t	 tksüt
  |  tt	 | kst
  |  tt	 |  ksBt
   |  tt	 | ksjt
   |  tt	  |  kst
   |  tt	  | ksÀt
  |   tt	 t	| ksìt
  |   tt	 t	t ddtt	| ks&t
     t	t  ksVt
   t	  ksrt
   t	t ddtt	 kst
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.n t
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ân t
t	  dd	t	   ¡   krTt tt t t  tt t td dt t t  td   tksZn t
t	  d
d	t	   ¡ krt
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t
t	     ¡ t tt t t  tt t td dt t t  td   tksvt
t	  ¡  t t tt t  tt t td dt t t  td   tksÐt
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`t
t	  t ¡t ttd dt t t  td  tkst
t	   t ¡t tt t t  tt t td dt t t  td   tksút
t	t t }|jst
|jr*t
|js6t
t	 }|jsNt
|jsZt
|jrft
t	  t  }|jst
|jst
|jr t
d S )Nr(   r)   rZ   c                      s      S r]   r4   r4   ©rL   rM   rO   r4   r5   r6   <  r7   z'test_Series_functions.<locals>.<lambda>c                      s      S r]   r4   r4   rÞ   r4   r5   r6   =  r7   c                      s    t dddg S r9   r£   r4   r   r4   r5   r6   >  r7   Tr[   F)r   r3   r¶   r   r   rA   r:   r?   r@   r   r<   r   r'   rI   r   Úrewriterµ   rž   rº   )rQ   ZS1ZS2ZS3r4   ©rL   rM   rN   rO   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Œt  |  tt |ksât   | tt  | kst   tt ks8t     tt t kslt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s8t   |  t|   ksZt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rt 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sn t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tt|| 
¡  ¡ t tddtffks²t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n t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	sFn td S )Nc                      s     t  S r]   ©r×   r4   ©Útfm1Útfm2r4   r5   r6   u  r7   z+test_MIMOSeries_functions.<locals>.<lambda>c                      s     t  S r]   )r?   r4   rã   r4   r5   r6   v  r7   c                      s     t d  S rŠ   ry   r4   ©Útfm4Útfm6r4   r5   r6   w  r7   c                      s      d S r§   r4   r4   ræ   r4   r5   r6   x  r7   c                      s   dt d     S )NrZ   r+   r©   r4   rã   r4   r5   r6   y  r7   c                      s       S r]   r4   r4   ©rä   rå   rç   r4   r5   r6   }  r7   c                      s       S r]   r4   r4   ©rä   rå   rç   Útfm5r4   r5   r6   ~  r7   c                      s       S r]   r4   r4   rê   r4   r5   r6     r7   c                      s     t  S r]   râ   r4   ©rç   rë   r4   r5   r6     r7   c                      s     t  S r]   ©r@   r4   rì   r4   r5   r6     r7   c                      s      t d  S r}   ry   r4   rì   r4   r5   r6     r7   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     r7   Tr[   r)   r(   r,   )r   r×   rÖ   rÕ   r   r   r<   r'   rI   rJ   r   r   r   rA   r3   r:   r   r¶   r   rØ   r   rß   )Útfm3Útfm7Úmat_1Úmat_2Ztm_1Ztm_2r4   )rä   rå   rç   rë   rè   r5   Útest_MIMOSeries_functionsf  s     &,*4"""2 ÿ®2 ÿþÿ20Vÿ
,ÿ
2 ÿ°2 ÿþþÿ
~ÿÿ
 ÿp>ÿÿþ
ü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sÜt|jtksêttt  }|jt  fkst|jtks(tt | dt}t | dt}t t|| }|j t|| fksttt }|j fkstt||}|j||fksŽt|jtksÄtt }	||	ksÜt||krêttt }
|
jt fksttt  }|jt  fksRt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
   t  S r]   ©r   r4   r¿   r4   r5   r6   Ï  r7   z,test_Parallel_construction.<locals>.<lambda>c                      s   t  S r]   ró   r4   rÀ   r4   r5   r6   Ð  r7   c                      s   t   S r]   ró   r4   r¥   r4   r5   r6   Ñ  r7   c                      s   t d S r}   ró   r4   rÂ   r4   r5   r6   Ò  r7   c                      s   t td tt   S r}   )r   r3   r:   r4   rÁ   r4   r5   r6   Ó  r7   c                      s   t  tddddgS rÃ   )r   r   r4   r   r4   r5   r6   Ô  r7   )r   r?   r3   r@   rA   rB   r:   rC   rH   r¶   r   r   r   r>   r<   rÄ   r   r'   rI   rJ   )rÅ   rÆ   Úp0r   rÉ   rP   r   r   r   Zp5Zp6Zp7r4   rÀ   r5   Útest_Parallel_construction©  sD    H(


 rõ   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sŠt	|j
tksŽt	|j|j|jf  krÒdksØn t	tt }|jt fkst	|j
tkst	|j|j|jf  kr4dks:n t	t t}|j tfksht	|j
tksxt	|j|j|jf  krdks n t	t }|j fksÂt	|j|j|jf  krädksên t	t|| }|j|| fkst	|j|j|jf  kr*dks0n t	tt  }|jt  fksjt	|j|j|jf  krdksn t	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   ü  r7   z0test_MIMOParallel_construction.<locals>.<lambda>c                      s   t  tS r]   )r   r×   r4   rã   r4   r5   r6   ÿ  r7   c                      s   t  S r]   r÷   r4   )rä   rî   rç   r4   r5   r6     r7   c                      s
   t  S r]   r÷   r4   )rî   rï   r4   r5   r6     r7   c                      s   t d S r}   r÷   r4   )rä   rç   r4   r5   r6     r7   c                      s   t td tt    S r}   )r   r3   r:   r4   )rå   rç   r4   r5   r6   	  r7   )r   r×   rÖ   rÕ   rØ   r   r:   r   r>   r<   rÄ   r3   rÙ   rÚ   rÛ   r   r'   rJ   rI   )rë   rè   Zp8Zp9Zp10Zp11Zp12Zp13r4   )rä   rå   rî   rç   rï   r5   Útest_MIMOParallel_construction×  sD    
$(((
("(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s¬t
   |  t	 | ksÎt
   |  t	  |  ksôt
   t	 tkst
   t	 t ks:t
   t	   ksXt
   tt ddtt	 kst
   tt	 ks¢t
  |   tt	 | ksÊt
  |   tt ddtt	|  ksüt
    |  t	t | ks*t
     t	t  ksZt
  |  |   tt	 | t	| kst
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sDn t
t	 t  dd	t	 t   ¡   krBt 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sHn t
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n t
t	  d
d	t	   ¡ kr>t
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Î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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s8t
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üt
t	  t ¡t ttd dt t t  td   d td dt t t  td  tks^t
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t
t	 t	t	   k	rJt	t	 k	sPn t
t	t t}|j	srt
|j	r~t
|j	st
t	   }|j	sŠt
|j	r²t
|j	sŸt
t	  t }|j	sÞt
|j	rêt
|j	söt
d S )Nr(   r)   rZ   c                      s      S r]   r4   r4   rÞ   r4   r5   r6      r7   z)test_Parallel_functions.<locals>.<lambda>c                      s      S r]   r4   r4   rÞ   r4   r5   r6   !  r7   c                      s    t dddg S r9   r£   r4   r   r4   r5   r6   "  r7   Tr[   F)r   r3   r¶   r   r   rA   r:   r?   r@   r   r<   r   r'   rI   r   rß   rµ   rž   rº   )rQ   ZP1ZP2ZP3r4   rà   r5   Útest_Parallel_functions  sÀ    ($"&"$("(2.02"Jÿÿ
4,ÿ*ÿÿÿÿþÿ*j* ÿÿ&nÿ
l* ÿÿ
j* ÿÿ
hÿ* ÿÿ
Jÿ
^ÿ* ÿÿ
>rù   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n t   t
   ks.t   | |   t
 t| |  ksft     |  t
  t|   kst    t
   ksŒt     t
    ksät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Ât | |  t| |t
 ksðt   ||  tt
||t
   ks&t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rìt
  t¡  krìt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sòn td S )Nr)   c                      s     t  S r]   )rÖ   r4   rã   r4   r5   r6   f  r7   z-test_MIMOParallel_functions.<locals>.<lambda>c                      s     t  S r]   rí   r4   rã   r4   r5   r6   g  r7   c                      s     t d  S rŠ   ry   r4   ©rå   rî   r4   r5   r6   h  r7   c                      s      d S r¯   r4   r4   rú   r4   r5   r6   i  r7   c                      s   dt d     S rš   r©   r4   rú   r4   r5   r6   j  r7   c                      s      S r]   r4   r4   )rå   rî   rï   r4   r5   r6   l  r7   c                      s      S r]   r4   r4   )rä   rå   rï   r4   r5   r6   m  r7   c                      s      S r]   r4   r4   ré   r4   r5   r6   o  r7   c                      s      S r]   r4   r4   ré   r4   r5   r6   p  r7   c                      s     t  S r]   râ   r4   rì   r4   r5   r6   u  r7   c                      s     t  S r]   )rA   r4   rú   r4   r5   r6   v  r7   c                      s     t d  S )Nr.   ry   r4   rú   r4   r5   r6   w  r7   c                      s      d S r8   r4   r4   )rä   rå   rî   r4   r5   r6   x  r7   c                      s   dt d     S rš   r©   r4   )rä   rî   r4   r5   r6   y  r7   c                      s       S r]   r4   r4   )rå   rî   rç   rë   r4   r5   r6   |  r7   c                      s      S r]   r4   r4   )rä   rå   rë   r4   r5   r6   ~  r7   Tr[   r(   )r   r?   r:   r@   r3   rA   r   r×   rÖ   rÕ   r   r<   r   r'   rI   rJ   r   rß   r   r   r¶   )rO   rQ   rè   r4   )rä   rå   rî   rç   rë   rï   r5   Útest_MIMOParallel_functionsT  s^    $:"8.((".6"ÿ
P2 ÿ* ÿÿþ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sât|jt ddtksøt|jt kst|jtks tt	  }|j
 tdfksJt|j ksZt|jtkspt|jtkstt	  }|j
t dfksªt|jt ksÀtt	| }|j
| dfksàt|jksðt|jtks tt	t ddt}|j
t ddtdfks0t|jtks@t|t	ksRtt	t ddt}|j
t ddtdfkst|jtkstt	|  t ddt }|j
tt ddtt| t ddt dfksÞt|jtt ddtt| kst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)   rZ   c                      s   t   S r]   ©r   r4   ©rL   rM   rN   r4   r5   r6   ³  r7   z,test_Feedback_construction.<locals>.<lambda>c                      s   t  tdddgS r9   )r   r   r4   r€   r4   r5   r6   Ž  r7   c                   S   s   t tddttd S rŠ   )r   r   r3   r4   r4   r4   r5   r6   µ  r7   c                   S   s
   t ddS rŠ   rü   r4   r4   r4   r5   r6   ¶  r7   c                      s   t   S r]   rü   r4   )rM   rO   rQ   r4   r5   r6   ž  r7   c                      s   t  dS )Nç      ø?rü   r4   ©rL   rM   r4   r5   r6   ¹  r7   c                      s   t   d  S )NrZ   rü   r4   r€   r4   r5   r6   º  r7   c                      s
   t  S r]   rü   r4   r|   r4   r5   r6   »  r7   )r   r3   r¶   r   r   rA   r:   r?   r@   r   r>   r   r<   Úsys1Úsys2rÄ   r'   rJ   rI   )rR   Úf1Úf2Úf3Úf4Zf5Zf6Zf7r4   )rL   rM   rN   rO   rQ   r5   Útest_Feedback_construction  sR    ($$
4$r  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sÞt  |   t	   ¡ t
|   ¡ dkst    t ks8t      t   ksdt     t kst     t  ksŽt   |    t | ksàt   |     t |  t |  fks&tt ddt|   t|ksNt|  |   t|  ksjt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 tt   jdttt t  tt t td dt t t  td    d  ks~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d dt t t  td   tks"tt jdttt t  tt t td dt t t  td    d  ks|t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*tt | djdt t ttd   tt   tt td dt t t  td    d  ks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øt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tt   ¡ t ddtks t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sLttt ddt t ¡t ttt t tt  t tkstd S )Nr(   r)   rZ   c                      s      d     S rŠ   r4   r4   rý   r4   r5   r6   Ò  r7   z)test_Feedback_functions.<locals>.<lambda>c                      s         S r]   r4   r4   rÀ   r4   r5   r6   Ó  r7   )r   r3   r¶   r   r   rA   r:   r?   r@   r   r   r<   r   r   r'   rJ   rI   Úsensitivityrß   )rQ   rR   r4   )rK   rL   rM   rN   rO   r5   Útest_Feedback_functionsŸ  s    ($.>,((,F(:*ÿÿÿ
Bÿ
:*ÿÿÿÿ
Bÿ
H&ÿÿÿÿ
Nÿ
Pÿ
dÿ
 D&ÿÿÿÿ
ÿr  c            	      C   s  t td dt} t td dt}t| |d}t| |d}t ddt}t|t sRtt|ts`t|j| ksnt|j| ks|t|jt|t|| kst|jt|t||  ks²tt| |}t| |}| | |ksÖt| | |ksæt| 	¡ t td d td td  d tkst| 	¡ t ttd td td  d   d td td  d tksbtt| | |d}|tt| ||kst|j| | ks t|jt|t|t| |ksÂtt | | |t}|t t| ||tksît|j| | kstd S )Nr(   r)   rZ   )
r   r3   r   Ú
isinstancer<   r;   r=   r   r   r   )	rL   rM   Zfd1Úfd2ÚunitrÈ   r   Zfd3rN   r4   r4   r5   Ú!test_Feedback_as_TransferFunctionñ  s0    

2J"r  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*t|jt|t||ksFt|j	t
|t|t|t||ksnt|| 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ètd S )
NzI_b, I_s, m, h, l2, l1T)ÚrealZnonnegativezK_D, K_P, v)r  r)   r(   r,   r   )r   rF   r   r3   r   r   r   r<   r;   r=   r   r   r   )ZIbÚIsÚmÚhÚl2Úl1ÚKDZKPÚvZtau1_sqZtau2Ztau3ÚKZGthetaZGdeltaZGpsiZDcontZPIcontZGunityZGinnerZGouterÚexprr4   r4   r5   Útest_issue_26161  sT     ÿ& (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st|j|ks¬t|j|ksºt|jtksÈt|j	dksÖt|  |ksætt||d}|j||dfkst|j|kst|j|ks(t|jtks8t|j	dksHtt|t
||}	|	j|t
||dfkstt|	j|kst|	jt
||kst|	jtksªt|	j	dksºttddt gddgg}
t |
t¡ }}t||}|j||dfkst|j|j  kr|ks$n td S )Nr(   r,   r)   rZ   r   )r   r3   r   r   r>   r<   r   r  rÄ   Úsignr   r   rØ   )rL   rM   rN   rO   rÎ   rÐ   rÑ   r  r  r  Zmatr   Ú
controllerr  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)   rZ   ©rÄ   c                      s
   t  S r]   ©r   r4   rÿ   r4   r5   r6   o  r7   z*test_MIMOFeedback_errors.<locals>.<lambda>c                      s   t t S r]   )r   r   r4   )rÎ   rÐ   rÑ   r4   r5   r6   p  r7   c                      s   t  dS rŠ   r  r4   )rÎ   Útfm_6r4   r5   r6   r  r7   c                      s
   t   S r]   r  r4   )Útfm_7r4   r5   r6   s  r7   c                      s   t  dS )Nrw   r  r4   rÏ   r4   r5   r6   u  r7   c                      s   t  dS rŠ   r  r4   ©rÜ   rÒ   r4   r5   r6   w  r7   c                      s   t   S r]   r  r4   r  r4   r5   r6   x  r7   c                      s   t   dS rŠ   r  r4   rÔ   r4   r5   r6   y  r7   c                      s   t  dS rŠ   r  r4   )rÎ   Útfm_8r4   r5   r6   |  r7   )	r   r3   r   rØ   r   r'   rJ   rI   r:   )rN   rO   rQ   rR   r4   )
rL   rM   rÎ   rÐ   rÑ   rÜ   rÒ   r  r  r   r5   Útest_MIMOFeedback_errors]  s.    r!  c            
      C   sR  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sÊt|	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s0t| ¡ 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n t|	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sPt|	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t|	 ¡ 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®t|	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s4t| ¡  |  ¡ ksNtd S )Nr(   rZ   r)   r  r   rw   r*   r,   FT)Úcancelr   r+   )r"  )r   )r   r3   r   rØ   r   r   r   r  r   r
   ZHalfr<   r   rß   )
rL   rM   rN   rO   rÎ   rÐ   rÑ   rÜ   ZF_1ZF_2r4   r4   r5   Útest_MIMOFeedback_functions  sv    
"("ÿÿ
ÿ

ÿÿÿ
ý^*ÿ ÿÿ

*"ÿ ÿ*ÿ þÿ
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rldksrn t| jtttt kst| jtkstttt gtgg}|j|j	|j
f  krÐdksÖn t|jttttt ttksüt|jtksttttgt g tgg}|j|j	|j
f  krLdksRn t|jttttttt t tkst|jtkst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(   rö   )r,   r)   c                      s   t tgtg ggS r]   ©r   r×   rÖ   r4   rz   r4   r5   r6   »  r7   z:test_TransferFunctionMatrix_construction.<locals>.<lambda>c                      s   t t gtggS r]   ©r   r×   rÕ   r4   r|   r4   r5   r6   Œ  r7   c                      s   t tgt ggS r]   r%  r4   r{   r4   r5   r6   ¿  r7   c                      s   t ttg ggS r]   r%  r4   r{   r4   r5   r6   À  r7   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   Ã  r7   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   Ä  r7   c                      s   t tt gS r]   r$  r4   r{   r4   r5   r6   Ç  r7   c                   S   s
   t tgS r]   )r   r×   r4   r4   r4   r5   r6   È  r7   )r   r@   r3   rA   r?   r:   r   rÕ   rÙ   rÚ   rÛ   r<   r>   r   rÄ   r×   rÖ   r'   rI   rJ   )Ztfm3_rë   rï   r4   r|   r5   Ú(test_TransferFunctionMatrix_construction§  s,    $$$&(2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üt	t |t¡tt dt d td d tggks2t	t |t¡tt ddtt ddtgt ddtt ddtggksxt	tt
gtgg}| tt
 gt ggksšt	tt
ttg| t
 t gg}| tt
 t t g|  t
tggksðt	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st	| 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øt	| 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\t	| td¡tt dt t ttd  tt dt tt td   tggks®t	| td¡tt tt t td tt tt t td  tggksøt	| td¡tt dttd  tt tt ttd  tggks<t	| tdtdtti¡tt tt td tt tt td  tggkst	| 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$t	| ¡ 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~t	| ¡ tt tt t ttd  tgt tt tt td   tggksÎt	| ¡  ¡ |ksät	| ¡  ¡ |ksút	| ¡ 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t	| ¡ ddgtttt gggks°t	| ¡ 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sVt	| ¡ d	ddgg gg g ggkszt	| ¡ dgdgggkst	| ¡ 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t	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t	|	 ¡ tt ddtd  td  dt  d tggk	sFt	| ¡ 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t	| ¡ t tt t ttd  tt tt tt td   tgk	sàt	| ¡ tt dtd d tt td dt  d dtt dttgk
s,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k
st	| ¡ 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
süt	| tdtdtdi¡ ¡ tt dttt dt dtd  d tffksJt	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ön t	|  ¡ 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sXt	|
  ¡ 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Ÿt	d S )Nr)   r(   r,   r*   r-   r   r+   rw   rZ   rl   rg   rh   gTã¥Ä 	@g
×£p=
@gÖÿ9Ì@gTã¥Ä @)!r   r@   r3   rA   r?   r   r:   r   rØ   r<   r×   rÖ   rÕ   rD   r   r   r   r
   r   r   Z	transposeZ
elem_polesr   r   r¶   Z
elem_zerosr   r   r   Z_flatr   r   r   )rQ   rð   rñ   Zmat_3rä   rå   ZH_1ZH_2ZH_3ZH_4Z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sÎt
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sVt
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ìt
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sÔt
d S )	Nr(   r/   r   r)   g333333Ó?r,   ro   r1   )r   rD   r3   rE   r!   ÚTr
   ÚZeror   r;   r<   )rK   ÚnumZÚdenZÚtf_test_bilinearÚtf_test_manualÚtf_test_forwardÚtf_test_backwardZtf_test_gbtr4   r4   r5   Útest_TransferFunction_gbt2  s(    ,^  ""$,",vr1  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sÌt
d S )Nr(   r   r)   )r   rD   r3   rE   r"   r)  r
   r*  r   r;   r<   )rK   r+  r,  r-  r.  r4   r4   r5   Útest_TransferFunction_bilinearX  s
    ,^r2  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st
d S ©Nr(   r   )r   rD   r3   rE   r#   r)  r
   r*  r   r;   r<   )rK   r+  r,  r/  r.  r4   r4   r5   Ú"test_TransferFunction_forward_diffc  s
     "r4  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st
d S r3  )r   rD   r3   rE   r$   r)  r
   r*  r   r;   r<   )rK   r+  r,  r0  r.  r4   r4   r5   Ú#test_TransferFunction_backward_diffn  s
    ,,r5  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stt|dks€tttfd	d
 tt fdd
 ttfdd
 d S )Nro   r,   r(   r)   iLÿÿÿéŽ   é   r   c                      s   t  S r]   ©r%   r4   rz   r4   r5   r6     r7   z4test_TransferFunction_phase_margin.<locals>.<lambda>c                      s   t  S r]   r8  r4   r   r4   r5   r6     r7   c                      s   t t S r]   )r%   r   r4   ©Útf_mr4   r5   r6     r7   )r   r:   r3   rD   rE   r   r   r   r%   r   r   r   r<   r'   r   rI   rÿ   r4   ©rN   rO   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sŽt
t|tksÄt
ttfdd	 tt fd
d	 ttfdd	 d S )Nr)   r+   r(   ro   iìÿÿÿr1   i  c                      s   t  S r]   ©r&   r4   rz   r4   r5   r6     r7   z3test_TransferFunction_gain_margin.<locals>.<lambda>c                      s   t  S r]   r=  r4   r   r4   r5   r6     r7   c                      s   t t S r]   )r&   r   r4   r9  r4   r5   r6     r7   )r   r3   rD   rE   r   r   r   r&   r   r
   r<   r   r'   r   rI   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sdt|jt ddgkszt|jt ddggkst|jt dgksŠt|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êttt tgt t	gt t
gt tg}|jt tggks(t|jt t	ggks@t|jt t
ggksXt|jt tggkspt|jt tggt t	ggt t
ggt tggfksš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 ddgddgg}|jt ddgddggkst|jt ddgddggks6t|jt ddgddggksVt|jt ddgddggksvt|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sÎt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sLt|jt ttgttggkslt|jt ttgttggkst|jt ttgttggks¬t|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tt }|jt dggt dggt dggt dggfksBtt ddgddgg}t ddg}t||}|jt ddgddggkst|jt ddgks€t|jt ddggksŸt|jt dggksÖt|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t|jdks,t|jdks<t|jdksLt|jdks\t|jdkslt|jdks|t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(   ç      ø¿rw   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   õ  s     ÿ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   ÷  s     ÿ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   & ÿ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 rx   )r    r3   r4   r4   r4   r5   r6     r7   c                   S   s"   t tddgtdgtdgdS )Nr)   r/   rZ   r(   r   )r    r   r4   r4   r4   r5   r6     s    ÿ)r   r    Ústate_matrixr<   Úinput_matrixÚoutput_matrixÚfeedforward_matrixr>   r?   r@   rA   Úa3rB   rC   rH   Úb3Úc0r   Úc2Úc3r   r   r   Úd3Zis_SISOr'   r   rJ   )ÚA1ÚB1ÚC1ÚD1Úss1Úss2Úss3ZA4ZB4ZC4ZD4Úss4Zss5ZA6ZB6Zss6r4   r4   r5   Útest_StateSpace_construction  sº    
D ÿ8ý    ÿÿÿÿú
    ÿÿÿÿú
8
 ÿÿ
û
rS  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 dddgd	ddgdddgg}t ddgddgd
dgg}t dddgdddg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 dddddgdddddgdddddgddd	ddgdddddggt ddgddgddgddgd
dggt dddddgdddddggt ddgddgg}tt dddddgd	ddddgdddddgdddddgdddddggt ddgddgd
dgddgddggt dddddgdd
dddggt ddgd
dgg}	  |ksHt  |	ksZtd tt dggt dggt dggt dggkstd tt dggt dggt dggt dggksÒtd tt dggt dggt dggt dggksttt fdd ttfdd d S )Nr*   r(   r)   ri   r+   éüÿÿÿr   r,   rZ   rw   r.   é   é   i÷ÿÿÿiñÿÿÿiîÿÿÿiöÿÿÿru   iúÿÿÿr/   rþ   ç      @g      @rl   c                      s     S r]   r4   r4   )rO  rQ  r4   r5   r6   B  r7   z%test_StateSpace_add.<locals>.<lambda>c                      s     S r]   r4   r4   )rP  rR  r4   r5   r6   C  r7   )r   r    r<   r'   r   )
rK  rL  rM  rN  ÚA2ÚB2ÚC2ÚD2Zexpected_addZexpected_mulr4   )rO  rP  rQ  rR  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s2t|
 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szt| |kst|
    |
ks td S )NrZ   r(   )r   r?   r@   rA   rE  rB   rC   rH   rF  rG  r   rH  rI  r   r   r   rJ  r    r<   )ÚAÚBÚCÚDÚSSZSS_negZ	state_matZ	input_matZ
output_matZfeedforward_matÚsystemr4   r4   r5   Útest_StateSpace_negationE  sN    

ÿÿ

þ

þùÿ

ÿ
üÿ
rc  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Àt| 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t|  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sht| 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Ðt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Æt| t
dt	dtdtd	tdi¡ ¡ |ksòt| ¡ |kstd S )Nr(   r)   r,   r+   é$   é   ro   r   r*   rh   rg   rj   rk   rl   )r    r   r3   r:   r   r<   r~   rD   rE   r@   r?   rH   rC   rB   r   )rO  rP  r   r   ÚGÚexpectZexpect_r4   r4   r5   Ú!test_SymPy_substitution_functionsg  s$    :F@R@ÿ
Nÿ
JJ(,rh  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s t|j|j ks²t|j|jksÂtt dddgdddgdddgg}	t dddgdddgdddgg}
t dddgdddgdddgg}t dddgdddgdddg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ž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t|d d tdtd  dt  d dtd  dtd   dt  d tksht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sÆt| t¡d d tksâtttdd  d S )Nrp   rZ   r,   r)   r+   r(   r   rf   é;   r.   r0   r?  rw   r/   rh   rg   g      %@rW  g      @g      @g      @rj   c                   S   s6   t ttd  td  tt  t ttd  t t¡S r}   )r   rE   r3   r:   rD   rß   r    r4   r4   r4   r5   r6   ©  r7   z!test_conversion.<locals>.<lambda>)r   r    rß   r   r3   r<   r;   r=   r×   r   r¶   r'   rI   )rK  rL  rM  rN  r   Ztm1Ztm2rL   rM   rX  rY  rZ  r[  r   Ztm3ra  r4   r4   r5   Útest_conversion  sF    

,    
^^R

ÿÿ
ûÿ
rj  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Œt| ¡ d
ksÌt| ¡ t ddgddggksìt| ¡ t ddgddgddgddggkst| ¡ t dgdggt dgdgggksHt| ¡ t dgdgdgdgggkspt| ¡ d	kst| ¡ d
kst| ¡ t ddgddggks¶t| ¡ t ddddgddddggksàt| ¡ t dgdggt dgdgggkst| ¡ t dgdgggks.t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Øt|j|j|j ksðt|j|j|j kst|jt ddddgddddgddddgddddggksDt|jt ddgddgddgddggkspt|jt ddddgddddggkst|jt ddgddggksžtd S )Nr?  rw   r(   r   r/   r*   r)   rZ   TFr,   ri   g      è¿)r   r    Zis_observabler<   Zobservability_matrixZobservable_subspaceZis_controllableZcontrollability_matrixZcontrollable_subspaceÚappendZ
num_statesrÛ   rÚ   rA  rB  rC  rD  )ZA_matZB_matZC_matZD_matZSS1ZSS2ZSS3rK  rL  rM  rN  rO  rP  rQ  r4   r4   r5   Útest_StateSpace_functions¬  sB    
<* ..("*. <
<,(rl  N)|Zsympy.core.addr   Zsympy.core.functionr   Zsympy.core.mulr   Zsympy.core.numbersr   r   r   r   Zsympy.core.powerr	   Zsympy.core.singletonr
   Zsympy.core.symbolr   Z&sympy.functions.elementary.exponentialr   r   Z(sympy.functions.elementary.miscellaneousr   Z(sympy.functions.elementary.trigonometricr   Zsympy.matrices.denser   Zsympy.polys.polytoolsr   Zsympy.polys.rootoftoolsr   Zsympy.simplify.simplifyr   Zsympy.core.containersr   Zsympy.matricesr   r   r   Zsympy.physics.controlr   r   r   r   r   r   r   r   r    r!   r"   r#   r$   r%   r&   Zsympy.testing.pytestr'   rD   rq   rE   r   r3   rF   rG   r:   r   r   r¶   r   r)  r?   r@   rA   rE  rB   rC   rH   rF  rG  r   rH  rI  r   r   r   rJ  r×   rÖ   rÕ   rY   r¡   rª   r³   r·   r¹   r»   rË   rÝ   rá   rò   rõ   rø   rù   rû   r  r  r  r  r  r!  r#  r'  r(  r1  r2  r4  r5  r<  r>  rS  r\  rc  rh  rj  rl  r4   r4   r4   r5   Ú<module>   sv   D"((a a"++,>C.5H463% '"(#h&lA"-