o
    ÜÓhk ć                   @   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# d dl$m%Z% d dl&m'Z'm(Z(m)Z) d dlm*Z*m+Z+ d dl,m-Z-m.Z.m/Z/m0Z0m1Z1m2Z2m3Z3m4Z4m5Z5m6Z6m7Z7m8Z8m9Z9m:Z:m;Z;m<Z< d dl=m>Z> ed\Z?Z@ZAZBZCZDZEZFZGZHZIZJZKed\ZLZMZNZOZPZQZRZSZTZUZVZWZXZYZZZ[e-deCd deI eJ eC  eJd  eCZ\e-eGdeCZ]e-eNeF eC eNeC eF eCZ^dd Z_dd Z`dd Zad d! Zbd"d# Zcd$d% Zdd&d' Zed(d) Zfd*d+ Zgd,d- Zhd.d/ Zid0d1 Zjd2d3 Zkd4d5 Zld6d7 Zmd8d9 Znd:d; Zod<d= Zpd>d? Zqd@dA ZrdBdC ZsdDdE ZtdFdG ZudHdI ZvdJdK ZwdLdM ZxdNdO ZydPdQ ZzdRdS Z{dTdU Z|dVdW Z}dXdY Z~dZd[ Zd\d] Zd^d_ Zd`da Zdbdc Zddde Zdfdg Zdhdi Zdjdk ZdlS )mé    )ŚAdd)ŚFunction)ŚMul)ŚIŚpiŚRationalŚoo)ŚPow)ŚS)Śsymbols)ŚexpŚlog)Ś	Heaviside)Śsqrt)Śatan)Śeye)ŚSISOLinearTimeInvariant)Śfactor)ŚCRootOf)Śsimplify)ŚTuple)ŚImmutableMatrixŚMatrixŚ
ShapeError)ŚsinŚcos)ŚTransferFunctionŚPIDControllerŚ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© r9   r9   śZ/var/www/html/ai/venv/lib/python3.10/site-packages/sympy/physics/control/tests/test_lti.pyŚ<lambda>x   ó    z4test_TransferFunction_construction.<locals>.<lambda>c                   S   s   t tdtS ©Nr   r7   r9   r9   r9   r:   r;   y   r<   c                   S   s   t ddtS r=   r7   r9   r9   r9   r:   r;   z   r<   c                   S   s   t tg d¢ttS ©N©r-   r.   r1   )r   r   r8   r9   r9   r9   r:   r;   |   ó    c                   S   s    t td dt  d td dS )Nr.   r-   r1   r7   r9   r9   r9   r:   r;   ~   s     c                   S   s   t td dt dS )Nr-   r0   r/   )r   Śpr9   r9   r9   r:   r;      r@   c                   S   s   t dddS )Nr1   r/   r5   )r   r9   r9   r9   r:   r;      r<   )r   r8   ŚnumŚdenŚargsrA   Ś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Śtf18r9   r9   r:   Ś"test_TransferFunction_construction"   s   $ 08$,4444,DN((2($2",6,64 >rl   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.   r0   r-   r1   r3   r2   r/   r5   c                      ó
   t   ”S ©N©r   Śfrom_rational_expressionr9   )Śexpr_2r9   r:   r;      ó   
 z1test_TransferFunction_functions.<locals>.<lambda>c                      rp   rq   rr   r9   )Śexpr_3r9   r:   r;      ru   c                      rp   rq   rr   r9   )Śexpr_4r9   r:   r;      ru   )r1   r/   r0   r?   c                      s   t   t”S rq   )r   Śfrom_coeff_listsr8   r9   )Śden3Śnum3r9   r:   r;   «   ó    y      šæ       @y      šæ       Ąé   ē      š?ē       @éż’’’ē      @ē      @ē      @zc1, d0:3g      @r6   éł’’’é   é   é
   éū’’’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   ŚxrE   Śpolesr9   r9   r9   r:   r;     ó   ( 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   rE   r   Śzerosr9   r9   r9   r:   r;   2  r   éž’’’c                      s    t d t  d  S ©Nr.   r-   ©r8   r9   ©rU   r9   r:   r;   E  r@   c                      s    t  S rq   r   r9   ©rW   r9   r:   r;   F  ó    c                      s     S rq   r9   r9   ©rU   rW   r9   r:   r;   G  r   c                      s      ttdi”S ©Nr.   )ŚxreplacerA   r   r9   ©rT   r9   r:   r;   R  ó    )2r   r	   r8   rA   r   r   ŚtauŚto_exprrs   r   r,   ŚZeroDivisionErrorrO   rx   Śfrom_zpkr   r   rG   rF   rE   rH   rI   rN   ŚexpandŚsubsŚevalfr   Śeval_frequencyŚwnr
   Śdc_gainr   r   r   Śkr   Śstrr   ŚNotImplementedErrorŚ	is_stabler   r   ŚdoitŚpowr   r   rB   rC   r   rP   )?Śexpr_1Śexpr_5Śexpr_6rQ   ŚdelayŚexpr_7ŚH1ŚH2Śexpr_8rR   Śnum2Śden2rS   r   r   ŚgainŚtf0rJ   rK   ŚG1ŚG2ŚcrM   ŚeŚfrL   Ś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_rY   r[   r\   r_   r`   rb   r9   )ry   rt   rv   rw   rz   rT   rU   rW   r:   Ś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 )Nr3   r0   r1   r-   r.   r¹   F©Ścommutativec                      ó    t g d¢ S r>   ©r   r9   ©rR   r9   r:   r;   q  ó    z@test_TransferFunction_addition_and_subtraction.<locals>.<lambda>c                      s     S rq   r9   r9   )r¹   rS   r9   r:   r;   r  r   c                      ó     S rq   r9   r9   ©rT   rU   r9   r:   r;   s  r   c                      s    t d  S ©Nr-   r   r9   rß   r9   r:   r;   t  r<   c                      s    d S ©Nr5   r9   r9   rß   r9   r:   r;   u  r   c                      s   dt d    S ©Nr-   r1   ©rA   r9   rß   r9   r:   r;   v  rą   c                      s    t g d¢ S r>   rŽ   r9   rß   r9   r:   r;   ~  rą   c                      s     S rq   r9   r9   rā   r9   r:   r;     r   c                      s    t d  S rć   r   r9   rß   r9   r:   r;     r<   c                      s    d S rä   r9   r9   rß   r9   r:   r;     r   c                      s   t d   S )Nr0   r   r9   )rS   r9   r:   r;     r<   c                      s   dt d    S )Nr-   r/   rę   r9   rß   r9   r:   r;     rą   )r   r8   rA   r   r   r,   rO   r9   r9   )r¹   rR   rS   rT   rU   r:   Ś.test_TransferFunction_addition_and_subtractiond  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 )Nr1   r2   r-   r0   r/   r3   rm   r¹   FrŪ   c                      ó    t g d¢ S r>   rŽ   r9   ©r¼   r9   r:   r;     rą   zCtest_TransferFunction_multiplication_and_division.<locals>.<lambda>c                      ó     S rq   r9   r9   )r·   r¹   r9   r:   r;      r   c                      rź   rq   r9   r9   ©r¼   ŚG5r9   r:   r;   ”  r   c                      s    t d  S rć   r   r9   ©rģ   r9   r:   r;   ¢  r<   c                      s   d  S ©Nr2   r9   r9   rķ   r9   r:   r;   £  r   c                      s    t g d¢ S r>   rŽ   r9   ré   r9   r:   r;   „  rą   c                      ó    d S r=   r9   r9   )ŚG6r9   r:   r;   ¦  r   c                      s     S rq   r9   r9   rė   r9   r:   r;   §  r   c                      rļ   r   r9   r9   rķ   r9   r:   r;   Ø  r   c                      s    t d  S r   r   r9   rķ   r9   r:   r;   ©  r<   c                      s   t dt d     S )Nr/   r.   r   r9   )rø   r9   r:   r;   Ŗ  r@   c                      s   d  S r=   r9   r9   )r½   r9   r:   r;   «  r   c                      s   d   S rć   r9   r9   )rš   ŚG7r9   r:   r;   ¬  r<   c                      s      S rq   r9   r9   ©rģ   rš   rń   r9   r:   r;   ­  r<   c                      s       S rq   r9   r9   rņ   r9   r:   r;   ®  rą   )	r   r8   rA   r   r   rŖ   r   r,   rO   r9   r9   )r·   rø   r¼   r½   rģ   rš   rń   r¹   r:   Ś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.   r1   r/   )r   r   r8   rA   rJ   rK   Ś	is_proper)rg   Śzetar   r·   rø   r¼   r½   r9   r9   r:   Ś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   r8   rA   rJ   rK   Śis_strictly_proper)rg   rö   r   rR   rS   rT   rU   r9   r9   r:   Ś(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.   r1   r/   )r   r   r8   rA   rJ   rK   Śis_biproper)r   rg   rö   rR   rS   rT   rU   r9   r9   r:   Ś!test_TransferFunction_is_biproperÉ  rų   rü   c            
      C   s  t d\} }}}t| |||}t }t|tsJ t|ts J |t| |||tks,J |t| ||dtks8J |j|td  |t |  | | td  |  | t  ksWJ |jtd | t ksdJ |jtkskJ |j| ksrJ |j	|ksyJ |j
|ksJ |j|ksJ | ” t|td  |t |  | | td  |  | t  td | t tks±J |jdksøJ |jdksæJ |jdksĘJ | ” t|td  | | t  ttksÜJ tttd t}t||}t||}t||}	|tt| |||ttttd tks	J |tt| |||ttttd tksJ |	tt| |||ttttd tks5J | ” tttd | t  td |td  |t |  | | td  |  | t    td td | t  tksrJ | ” tt|td  |t |  | | td  |  | t   td td | t  tks£J |	 ” ttd td | t  |td  |t |  | | td  |  | t   t|td  |t |  | | td  |  | t   td td | t   td | t  tksJ d S )Nzkp ki kd tfr   r.   TFr-   )r   r   Ś
isinstancer   r8   rB   rC   ŚvarŚkpŚkiŚkdrQ   r©   rõ   rū   rł   r   r   r    )
r’   r   r  rQ   r¾   ræ   rR   Śpar1Śser1Śfed1r9   r9   r:   Śtest_PIDControllerÕ  sF   >T,


,,,V’>’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 )Nr1   r.   r/   r-   ŚX_dŚXc                      ó
   t  S rq   ©r   r9   ©rQ   rT   r9   r:   r;   #  ru   z*test_Series_construction.<locals>.<lambda>c                      ó   t  S rq   r	  r9   ©rQ   rS   rT   rU   r9   r:   r;   $  r{   c                      s   t   S rq   r	  r9   ©rS   rT   r9   r:   r;   %  r<   c                      ó   t d S r   r	  r9   ©rQ   rU   r9   r:   r;   &  r<   c                      ó   t td tt   S r   )r   r8   rA   r9   r  r9   r:   r;   '  ó    c                      ó   t  tg d¢S ©N)r-   r.   r1   r/   )r   r   r9   r   r9   r:   r;   (  r   )r   rE   r8   rF   rG   rH   rA   rI   rN   rö   r£   r   r   rD   rž   r   r,   rO   rP   )ŚinpŚoutŚs0Śs1Śtf3_rV   Śs2Śs3Śs4Śs6Śs7r9   r  r:   Ś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 )Nr1   r.   r/   r-   )r.   r-   c                   S   s   t  S rq   ©r"   r9   r9   r9   r:   r;   E  s    z.test_MIMOSeries_construction.<locals>.<lambda>c                      s
   t  S rq   r  r9   )Śtf_1Śtfm_1r9   r:   r;   H  ru   c                      s   t    S rq   r  r9   ©r!  Śtfm_2r9   r:   r;   L  r{   c                      r  rq   r  r9   )Śtfm_3Śtfm_5r9   r:   r;   O  ru   c                      r  r   r  r9   ©r#  r$  r9   r:   r;   R  r<   c                      s   t td tt    S r   )r"   r8   rA   r9   r&  r9   r:   r;   S  ó    c                      s   t tdt g S rć   )r"   r   rA   r9   ©r$  r9   r:   r;   T  r@   )r   rE   r8   rF   rG   rH   rA   rI   rN   rö   r£   r!   ŚTF3ŚTF2ŚTF1Śfrom_Matrixr   r"   rD   rž   ŚshapeŚnum_outputsŚ
num_inputsr#   r,   rO   rP   )Śtf_2Śtf_3Śtfm_4Śs8Śs9Śs11r9   )r   r!  r#  r$  r%  r:   Śtest_MIMOSeries_construction+  s4   H(
&&&r6  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.   rm   c                      s      S rq   r9   r9   ©rR   rS   rU   r9   r:   r;   k  r<   z'test_Series_functions.<locals>.<lambda>c                      s      S rq   r9   r9   r7  r9   r:   r;   l  r<   c                      rč   r>   rŽ   r9   r   r9   r:   r;   m  rą   Trn   F)r   r8   rö   r£   r„   rG   rA   rE   rF   r   r   r,   rO   r©   Śrewriterõ   rł   rū   )rW   ŚS1ŚS2ŚS3r9   ©rR   rS   rT   rU   r:   Śtest_Series_functionsW  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 rq   ©r+  r9   ©Śtfm1Śtfm2r9   r:   r;   ¤  r<   z+test_MIMOSeries_functions.<locals>.<lambda>c                      r>  rq   )rE   r9   r@  r9   r:   r;   „  r<   c                      s     t d  S rć   r   r9   ©Śtfm4Śtfm6r9   r:   r;   ¦  rą   c                      s      d S rä   r9   r9   rC  r9   r:   r;   §  r{   c                      s   dt d     S )Nrm   r0   rę   r9   r@  r9   r:   r;   Ø  r@   c                      s       S rq   r9   r9   ©rA  rB  rD  r9   r:   r;   ¬  r{   c                      s       S rq   r9   r9   ©rA  rB  rD  Śtfm5r9   r:   r;   ­  rą   c                      s       S rq   r9   r9   rG  r9   r:   r;   ®  rą   c                      ó     t  S rq   r?  r9   ©rD  rH  r9   r:   r;   µ  r<   c                      rI  rq   ©rF   r9   rJ  r9   r:   r;   ¶  r<   c                      s      t d  S r   r   r9   rJ  r9   r:   r;   ·  r   c                      s     d S rī   r9   r9   rJ  r9   r:   r;   ø  r<   c                      s   t d  d    S )Nr1   r-   rę   r9   rJ  r9   r:   r;   ¹  ó    Trn   r.   r-   r1   )r!   r+  r*  r)  r#   r"   r,   rO   rP   r©   r   r„   rG   r8   rA   r£   rö   r   r,  r   r8  )Śtfm3Śtfm7Śmat_1Śmat_2Śtm_1Śtm_2r9   )rA  rB  rD  rH  rE  r:   Śtest_MIMOSeries_functions  s    &*(2"""4’®4’ž’20V
’,
’4’°4’žž
’~’’’p>’’
žürS  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 )Nr1   r.   r/   r-   r  r  c                      r  rq   ©r   r9   r
  r9   r:   r;   ž  ru   z,test_Parallel_construction.<locals>.<lambda>c                      r  rq   rT  r9   r  r9   r:   r;   ’  r{   c                      s   t   S rq   rT  r9   rā   r9   r:   r;      r<   c                      r  r   rT  r9   r  r9   r:   r;     r<   c                      r  r   )r   r8   rA   r9   r  r9   r:   r;     r  c                      r  r  )r   r   r9   r   r9   r:   r;     r   )r   rE   r8   rF   rG   rH   rA   rI   rN   rö   r£   r   r   rD   rž   r   r,   rO   rP   )r  r  Śp0r¾   r  rV   ræ   rĒ   rČ   Śp5Śp6Śp7r9   r  r:   Śtest_Parallel_constructionŲ  sD   H(


 rY  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-   )r1   r-   ©r.   r.   c                   S   s   t dS )Nr9   ©r#   r9   r9   r9   r:   r;   +  r   z0test_MIMOParallel_construction.<locals>.<lambda>c                      s   t  tS rq   )r#   r+  r9   r@  r9   r:   r;   .  r<   c                      s   t  S rq   r[  r9   )rA  rM  rD  r9   r:   r;   1  r<   c                      r  rq   r[  r9   )rM  rN  r9   r:   r;   4  ru   c                      r  r   r[  r9   )rA  rD  r9   r:   r;   7  r<   c                      s   t td tt    S r   )r#   r8   rA   r9   )rB  rD  r9   r:   r;   8  r'  )r!   r+  r*  r)  r,  r   rA   r#   rD   rž   r8   r-  r.  r/  r"   r,   rP   rO   )rH  rE  Śp8Śp9Śp10Śp11Śp12Śp13r9   )rA  rB  rM  rD  rN  r:   Śtest_MIMOParallel_construction  sD   
&&&&
*"*rb  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.   rm   c                      ó      S rq   r9   r9   r7  r9   r:   r;   O  r<   z)test_Parallel_functions.<locals>.<lambda>c                      s      S rq   r9   r9   r7  r9   r:   r;   P  r<   c                      rŻ   r>   rŽ   r9   r   r9   r:   r;   Q  rą   Trn   F)r   r8   rö   r£   r„   rG   rA   rE   rF   r   r   r,   rO   r©   r8  rõ   rł   rū   )rW   ŚP1ŚP2ŚP3r9   r<  r:   Śtest_Parallel_functions;  s¶   ($"& "& &0.02"J’
’4,’*’’’’ž’*j,’’&n
’l,’
’j,’
’h’,’
’J
’^’,’
’@rg  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 rq   )r*  r9   r@  r9   r:   r;     r<   z-test_MIMOParallel_functions.<locals>.<lambda>c                      s     t  S rq   rK  r9   r@  r9   r:   r;     r<   c                      s     t d  S rć   r   r9   ©rB  rM  r9   r:   r;     rą   c                      s      d S rī   r9   r9   rh  r9   r:   r;     r{   c                      s   dt d     S rå   rę   r9   rh  r9   r:   r;     r@   c                      s      S rq   r9   r9   )rB  rM  rN  r9   r:   r;     r<   c                      s      S rq   r9   r9   )rA  rB  rN  r9   r:   r;     r<   c                      rc  rq   r9   r9   rF  r9   r:   r;     r<   c                      s      S rq   r9   r9   rF  r9   r:   r;     r<   c                      s     t  S rq   r?  r9   rJ  r9   r:   r;   ¤  r<   c                      s     t  S rq   )rG   r9   rh  r9   r:   r;   „  r<   c                      s     t d  S )Nr3   r   r9   rh  r9   r:   r;   ¦  rą   c                      s      d S r=   r9   r9   )rA  rB  rM  r9   r:   r;   §  rą   c                      s   dt d     S rå   rę   r9   )rA  rM  r9   r:   r;   Ø  r@   c                      s       S rq   r9   r9   )rB  rM  rD  rH  r9   r:   r;   «  rą   c                      s      S rq   r9   r9   )rA  rB  rH  r9   r:   r;   ­  r<   Trn   r-   )r   rE   rA   rF   r8   rG   r!   r+  r*  r)  r#   r"   r,   rO   rP   r©   r8  r„   r£   rö   )rU   rW   rE  r9   )rA  rB  rM  rD  rH  rN  r:   Śtest_MIMOParallel_functions  sZ   $8 6,&&".6"
’PJ’,’’žri  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.   rm   c                      s   t   S rq   ©r    r9   ©rR   rS   rT   r9   r:   r;   ā  r{   z,test_Feedback_construction.<locals>.<lambda>c                      r  r>   )r    r   r9   rß   r9   r:   r;   ć  r   c                   S   s   t tddttd S rć   )r    r   r8   r9   r9   r9   r:   r;   ä  rL  c                   S   s
   t ddS rć   rj  r9   r9   r9   r:   r;   å  ru   c                      s   t   S rq   rj  r9   )rS   rU   rW   r9   r:   r;   ē  r{   c                      ó   t  dS )Nē      ų?rj  r9   ©rR   rS   r9   r:   r;   č  r<   c                      s   t   d  S )Nrm   rj  r9   rß   r9   r:   r;   é  rą   c                      r  rq   rj  r9   r   r9   r:   r;   ź  ru   )r   r8   rö   r£   r„   rG   rA   rE   rF   r    rD   r   Śsys1Śsys2rž   r,   rP   rO   )rY   Śf1Śf2Śf3Śf4Śf5Śf6Śf7r9   )rR   rS   rT   rU   rW   r:   Śtest_Feedback_construction·  sR   ($$
4$rx  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.   rm   c                      s      d     S rć   r9   r9   rk  r9   r:   r;     ó    z)test_Feedback_functions.<locals>.<lambda>c                      s         S rq   r9   r9   r  r9   r:   r;     r  )r   r8   rö   r£   r„   rG   rA   rE   rF   r   rŖ   r©   r    r,   rP   rO   Śsensitivityr8  )rW   rY   r9   )rQ   rR   rS   rT   rU   r:   Ś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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.   rm   )
r   r8   r    rż   r   rB   rC   r   r   r©   )	rR   rS   Śfd1Śfd2Śunitr  r¾   Śfd3rT   r9   r9   r:   Śtest_Feedback_with_Series   s0   

0H 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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)r  r.   r-   r1   r   )r   rL   r   r8   r¹   r    r   rB   rC   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Śexprr9   r9   r:   Śtest_issue_26161F  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-   r1   r.   rm   r   )r   r8   r!   r$   rD   ro  rp  rž   Śsignr"   r   r,  )rR   rS   rT   rU   r!  r#  r$  rq  rr  rs  Śmatro  Ś
controllerrt  r9   r9   r:   Śtest_MIMOFeedback_constructionf  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-   r1   r.   rm   ©rž   c                      r  rq   ©r$   r9   rn  r9   r:   r;     ru   z*test_MIMOFeedback_errors.<locals>.<lambda>c                      s   t t S rq   )r$   r#   r9   )r!  r#  r$  r9   r:   r;      rą   c                      ó   t  dS rć   r  r9   )r!  Śtfm_6r9   r:   r;   ¢  r<   c                      s
   t   S rq   r  r9   )Śtfm_7r9   r:   r;   £  ru   c                      r   )Nr   r  r9   r"  r9   r:   r;   „  r<   c                      rl  rć   r  r9   ©r2  r%  r9   r:   r;   §  r<   c                      s   t   S rq   r  r9   r£  r9   r:   r;   Ø  r<   c                      s   t   dS rć   r  r9   r(  r9   r:   r;   ©  r<   c                      r   rć   r  r9   )r!  Śtfm_8r9   r:   r;   ¬  r<   )	r   r8   r!   r,  r   r,   rP   rO   rA   )rT   rU   rW   rY   r9   )
rR   rS   r!  r#  r$  r2  r%  r”  r¢  r¤  r:   Ś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-   rm   r.   r  r   r   r/   r1   FT)Ścancelr   r0   )r¦  )r   )r   r8   r!   r,  r   r$   r"   rz  r   r
   ŚHalfr©   r8  )
rR   rS   rT   rU   r!  r#  r$  r2  ŚF_1ŚF_2r9   r9   r:   Ś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-   rZ  )r1   r.   c                      s   t tgtg ggS rq   ©r!   r+  r*  r9   r   r9   r:   r;   ė  r@   z:test_TransferFunctionMatrix_construction.<locals>.<lambda>c                      s   t t gtggS rq   ©r!   r+  r)  r9   r   r9   r:   r;   ģ  r@   c                      s   t tgt ggS rq   r¬  r9   r   r9   r:   r;   ļ  r   c                      s   t ttg ggS rq   r¬  r9   r   r9   r:   r;   š  r   c                   S   s   t ttgttddgggS ©Nr-   r.   )r!   r+  r*  r)  r   r9   r9   r9   r:   r;   ó  ry  c                   S   s   t ttddggttggS r­  )r!   r+  r   r)  r*  r9   r9   r9   r:   r;   ō  ry  c                      s   t tt gS rq   r«  r9   r   r9   r:   r;   ÷  r{   c                   S   s
   t tgS rq   )r!   r+  r9   r9   r9   r:   r;   ų  ru   )r   rF   r8   rG   rE   rA   r!   r)  r-  r.  r/  rD   r   rž   r+  r*  r,   rO   rP   )Śtfm3_rH  rN  r9   r   r:   Ś(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-   r1   r/   r2   r   r0   r   )rm   r   r1   r   r}   r~   gTć„Ä 	@g
×£p=
@gÖ’9Ģ@gTć„Ä @) r   rF   r8   rG   rE   r   rA   r!   r,  r+  r*  r)  rJ   r„   r    r¢   r
   r   r   Ś	transposeŚ
elem_polesr   r£   rö   Ś
elem_zerosr   r   r©   Ś_flatr”   r   r   )rW   rO  rP  Śmat_3rA  rB  ŚH_1ŚH_2ŚH_3ŚH_4ŚH_5r9   r9   r:   Ś%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-   r4   r   r.   g333333Ó?r1   r   r6   )
r   rJ   r8   rK   r&   ŚTr
   ŚZeror   rB   )rQ   ŚnumZŚdenZŚtf_test_bilinearŚtf_test_manualŚtf_test_forwardŚtf_test_backwardŚtf_test_gbtr9   r9   r:   Śtest_TransferFunction_gbtb  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   rJ   r8   rK   r'   r»  r
   r¼  r   rB   )rQ   r½  r¾  ræ  rĄ  r9   r9   r:   Śtest_TransferFunction_bilinear  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   rJ   r8   rK   r(   r»  r
   r¼  r   rB   )rQ   r½  r¾  rĮ  rĄ  r9   r9   r:   Ś"test_TransferFunction_forward_diff  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   rJ   r8   rK   r)   r»  r
   r¼  r   rB   )rQ   r½  r¾  rĀ  rĄ  r9   r9   r:   Ś#test_TransferFunction_backward_diff  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   r1   r-   r.   iL’’’é“   é   r   c                      ó   t  S rq   ©r*   r9   r   r9   r:   r;   “  r   z4test_TransferFunction_phase_margin.<locals>.<lambda>c                      rĖ  rq   rĢ  r9   r   r9   r:   r;   µ  r   c                      ó   t t S rq   )r*   r"   r9   ©Śtf_mr9   r:   r;   ¶  r<   )r   rA   r8   rJ   rK   r   r   r!   r*   r   r   r   r,   r§   rO   rn  r9   ©rT   rU   rĻ  r:   Ś"test_TransferFunction_phase_margin©  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.   r0   r-   r   iģ’’’r6   i  c                      rĖ  rq   ©r+   r9   r   r9   r:   r;   Ć  r   z3test_TransferFunction_gain_margin.<locals>.<lambda>c                      rĖ  rq   rŅ  r9   r   r9   r:   r;   Ä  r   c                      rĶ  rq   )r+   r"   r9   rĪ  r9   r:   r;   Å  r<   )r   r8   rJ   rK   r   r   r!   r+   r   r
   r   r,   r§   rO   rn  r9   rŠ  r:   Ś!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   r4   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   r8   r9   r9   r9   r:   r;   %  ó     ’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-   r1   r.   rÕ  r9   r9   r9   r:   r;   '  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Õ  r9   r9   r9   r:   r;   )  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Õ  r9   r9   r9   r:   r;   +  s
    żc                   S   s   t td td dt dS r   )r%   r8   r9   r9   r9   r:   r;   1  r'  c                   S   s"   t tddgtdgtdgdS )Nr.   r4   rm   r-   r   )r%   r   r9   r9   r9   r:   r;   2  s    
’)r   r%   Śstate_matrixŚinput_matrixŚoutput_matrixŚfeedforward_matrixrD   rE   rF   rG   Śa3rH   rI   rN   Śb3Śc0rĆ   Śc2Śc3rÄ   rÅ   rĘ   Śd3Śis_SISOr,   r   rP   )ŚA1ŚB1ŚC1ŚD1Śss1Śss2Śss3ŚA4ŚB4ŚC4ŚD4Śss4Śss5ŚA6ŚB6Śss6r9   r9   r:   Ś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.   r   r0   éü’’’r   r1   rm   ©r   r/   r.   ©rm   r   r   ©r.   r0   r1   r   ©r/   r.   r   ©r-   r/   r1   ©r/   r-   r   r   r   ©r.   r   r   r   r   ©r   r   r   r/   r.   ©r   r   rm   r   r   ©r   r   r.   r0   r1   ©r.   ró  r/   r.   r   ©r   r-   r-   r/   r1   r3   )r   r/   r.   r   r   )rm   r   r   r   r   )r.   r0   r1   r   r   )é   é   é÷’’’r/   r-   )éń’’’éī’’’r   r.   r   éö’’’r   r  )r   r   r   r.   ró  )r1   r   éś’’’r   r-   r  r   r4   rm  ē      @g      @r   c                      rį   rq   r9   r9   )rę  rč  r9   r:   r;   r  r   z%test_StateSpace_add.<locals>.<lambda>c                      rź   rq   r9   r9   )rē  rķ  r9   r:   r;   s  r   )r   r%   r,   r   )
rā  rć  rä  rå  ŚA2ŚB2ŚC2ŚD2Śexpected_addŚexpected_mulr9   )rę  rē  rč  rķ  r:   Śtest_StateSpace_add4  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 )Nrm   r-   )r   rE   rF   rG   rŪ  rH   rI   rN   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Śsystemr9   r9   r:   Śtest_StateSpace_negationu  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.   r1   r0   é$   é   r   r   r/   r~   r}   r   r   r   )r%   r   r8   rA   r    r   rJ   rK   rF   rE   rN   rI   rH   r”   )rę  rē  r¾   ræ   ŚGŚexpectŚexpect_r9   r9   r:   Ś!test_SymPy_substitution_functions  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 ttgttggt tgtggt tt	ggd}| 
t”}|  
t”}|d d }|d d }	|td	t d
 td dt  d tksjJ |	j|j kssJ |	j|jks{J 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sAJ | 
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gg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sÆJ | 
t”d d tks½J ttd#d$  d S )%Nr   rm   r1   r.   r0   r-   r   )r  r  r|   é;   r3   r5   )rŌ  r   r1   )r-   r   r-   )r.   r-   r-   )r4   r   r-   )r   r-   r.   )r.   r.   r1   ©r   r-   r   )r   r.   r-   )r-   r   r.   )r.   r.   r   )r-   r-   r-   )r1   r.   r-   r~   r}   g      %@r  r4   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   rK   r8   rA   rJ   r8  r%   r9   r9   r9   r:   r;   Ū  s   6 z!test_conversion.<locals>.<lambda>)r   r%   rE   rF   rG   rŪ  rI   rN   rĆ   rŽ  r8  r   r8   rB   rC   r+  r£   rö   r,   rO   )rā  rć  rä  rå  r°   ŚH3Śtm1Śtm2rR   rS   r  r	  r
  r  r±   Śtm3r  r9   r9   r:   Śtest_conversionÆ  sN   
4
,
\^RD*’

’’
ū
’r'  c            2      C   s²  t ddgddgg} t dgdgg}t ddgg}t dg}t dgdgg}td}t| |||}| ” t dggks=J |j|dt d	t|  d
td|   ggksXJ t ddgddgg}tdd}t ddg}	t||d}
|
j|	dt dtd|  gdt|  dtd|   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}t dg}t||||}|j||d}t| ”  ”  	” d tddtd| d  t
td| d   dtd| d  ttd| d   ksJ t ddgddgg}t dgdgg}t ddgg}t dt| gg}t|||}t|j||dd  ”  ”  	” }|tdt| dtd| d  t
td| d  t|  dtd| d  ttd| d  t|  ksyJ td\}}}t ddgddgg}t dgd| gg}t ddgg}t |gdgg}t t| | gg}t|||} | j|||d ” }!|!d jd d ||||   d|d |   t| | |d |   ksēJ td\
}"}#}$}%}&}'}(})}*}+t |"|#g|$|%gg},t |&|'g}-t |(|)gg}.t |*|+g}/t|,|-|.}0|0j|/dd }1|1 |"df|#df|$df|%df|&df|'df|(df|)df|*df|+dfg
”}1|1d	t|  d
td|   ksWJ d S )Nr   r-   r   r   rm   r.   r   )Śinitial_conditionsr5   r2   r1   )r  r  r0   ró  r/   r   )Śinput_vectorrž   r   gY8b½É4@r   r6   )rž   r)  zm a x_0)r(  r)  rž   z+a_11 a_12 a_21 a_22 b_1 b_2 c_1 c_2 i_1 i_2)r   r   r%   Śdsolver   r   r¦   r   r   r”   r   r   r   r   rD   r    )2rā  rć  rä  rå  ŚI1r   rę  r  r
  ŚI2rē  ŚA3ŚB3ŚC3ŚD3ŚU3rč  Śopré  rź  rė  ŚU4rķ  Śop4r  rJ   Śx0ŚA5ŚB5ŚC5ŚI5ŚU5rī  Śop5Śa11Śa12Śa21Śa22rI   rN   rĆ   rŽ  Śi1Śi2rļ  rš  ŚC6ŚI6rń  Śexpr6r9   r9   r:   Śtest_StateSpace_dsolveŽ  sf   
6
F

B&’">.’RF,rE  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}tt ttgttggt tgtggt tt	gg}| 
” d	kswJ | 
” d
ksJ | ” t ddgddggksJ | ” 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 | ”  tdftdftdftdftdft	dfg”}|t ddgdd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sOJ | ” t tgtggt tt tt  gtt tt  gggksrJ | ”  tdftdftdftdftdftdfg”}	|	t ddgdd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s J |j|j|j ksJ |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s8J |jt g d¢g d¢gksHJ |jt ddgddggksXJ |j|j|j ksdJ |j|j|j kspJ |j|j|j ks|J |jt ttddgttddgg d¢g d¢gksJ |jt tdgtdgddgddggks¬J |jt tt	ddgg d¢gks¾J |jt ddgddggksĪJ d S )NrŌ  r   r-   r   r4   r/   r.   rm   TFr1   r   r  r   g      čæ)r-   rm   r.   r   ©r   r-   r   r   ©r-   r   r   r   ©r   r   r-   r   ©r   r   r   r-   )r   r%   rE   rF   rG   rŪ  rI   rN   rĆ   rŽ  Śis_observableŚobservability_matrixŚobservable_subspacer    Śis_controllableŚcontrollability_matrixŚcontrollable_subspaceŚappendŚ
num_statesr/  r.  r×  rŲ  rŁ  rŚ  )ŚA_matŚB_matŚC_matŚD_matŚSS1ŚSS2ŚSS3ŚSS4ŚQoŚQcrā  rć  rä  rå  rę  rē  rč  rķ  r9   r9   r:   Śtest_StateSpace_functions  sn   
<*2 ,,&2"". žžž2<

,,  4,$$r\  c                   C   sD  t ddgddgg} t dgdgg}t ddgg}t dgg}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ttd t}
t||	}|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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 | ” tt g d¢g d¢g d¢g d¢gt dgdgdgdggt g d¢gt dggksČJ |jdksĻJ |jdksÖJ | t”ttd td td  t d tksļJ t|}t||	}| ” | ” ksJ t|
|}|ttttd 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ks4J | ” tt g d	¢g d
¢g d¢gt dgdgdggt g d
¢gt dggks^J | t”ttd td td  t d tksxJ 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 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	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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ks;J | ” 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 |j|jksJ |j|jksJ t	|}t	||}| ” | ” ks”J | t
”| t
”ks®J tdtt}tdtd t}tttd t}t
|
|g||gg}t	||}|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t
tttd ttdttftdtd ttttd tffks J d S )"Nr   r-   rF  rG  ©r   r-   r-   r   rI  r.   r1   )rm   r   r   )r   r   r-   )rm   r-   r   r/   r   r0   ró  rm   rō  rõ  rö  r   r÷  rų  rł  rś  )r.   r   r   r/   r.   )r  r2   rm   r   r   )ró  r2   r.   r0   r1   r6   éō’’’r   )ró  r|   r/   r.   r   r’  éų’’’)r   r%   r   r8   r   r©   r/  r.  r8  r"   r!   ) rF   rI   rĆ   rÅ   rG   rN   rŽ  rĘ   rę  rē  rR   r  Śser2Śser3Śser_tfrŪ  rÜ  rß  rą  Śa4Śb4Śc4Śd4rč  rķ  Śser4Śser5Śser6rS   rT   rU   Śtfmr9   r9   r:   Śtest_StateSpace_series]  st  
žžüžžü
ü	üü

ō2

žžüżż
ö4
žžžžśżżžžųųūūžžš

žžžžśžųrk  c                  C   s  t ddgddgg} t dgdgg}t ddgg}t dgg}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||	}
|
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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 |
 ” tt g d¢g d¢g d¢g d¢gt dgdgdgdggt g d¢gt dggksĄJ |
 t”tttd  td d tksÕJ tttd t}t||}|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tttd tksJ | ” tt g d	¢g d
¢g d¢gt dgdgdggt g d¢gt dggks8J | t”ttd td d tksLJ 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 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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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ksJ | ” 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sOJ tdtt}tdtd t}tttd t}t||g||gg}t||}|tttttd ttdttftdtd ttttd tff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ksĮJ d S )!Nr   r-   rF  rG  rH  rI  r]  r.   r"  )r-   r   r   )r   r   rm   ©r   r-   rm   r/   r   r0   ró  r1   rm   rō  rõ  rö  r   r÷  rų  rł  rś  rū  rü  rż  rž  r’  r3   )	r   r%   r   r©   r8  r   r8   r#   r!   )rF   rI   rĆ   rÅ   rG   rN   rŽ  rĘ   rę  rē  r¾   rR   ræ   rŪ  rÜ  rß  rą  rc  rd  re  rf  rč  rķ  rĒ   rS   rT   rU   rj  rČ   r9   r9   r:   Śtest_StateSpace_parallelč  s:  
>:
’
üü

õ*
žžüüżż
ö(
žžžžśżżžžųų
ūūžžń
žžžžžśżrm  c                  C   s.  t ddgddgg} t dgdgg}t ddgg}t dgg}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||	}
|
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tt ddgddggt dgdggt ddggt dggdksJ |
 ” tt g d¢g d¢g d¢g d¢gt dgdgdgdggt g d¢gt dggksĮJ |
 t”tttd  td t d tksŲJ t||	d}| ” tt g d¢g d	¢g d¢g d¢gt dgdgdgdggt g d¢gt dggksJ | t”tttd  td d
td
   t d tks+J tttd t}t||}|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tttd tdkseJ | ” tt g d¢g d¢g d¢gt dgdgdggt g d¢gt dggks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}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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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dksSJ | ” tt tdtddtddtdd
tdgtdd
tddtddtddtdgtdtddtd dtd!d
td"gtd#d
td$dtd%dtd&dtd'gtdd
td(dtd)dtd*dtd+ggt tddtd,dgtddtd-dgtddtd.dgtddtd/dgtddtd0dggt tdtddtd1dtd-d
td#gtdd
td2dtd2dtd"dtdggt tddtd3dgtddtddggks1J t||d}| ” tt tddtd4dtdtd5td6dgtd7dtd8d9tdd
tdtd'dgtd:dtd;dtdtd<td=dgtd>dtd?d9td@d
td+td&dgtd
dtdAd9tdd
tdBtdCdggt tdDdtdEdgtd#d9tdFd9gtdDdtdGdgtdHd9tdId9gtdd9tdJd9ggt tddtd3dtdtdtddgtddtdFd9tdd
tdtdKdggt tddtd#dgtdd9tdLd9ggksJ d S )MNr   r-   rm   rF  )r-   rm   rm   r   r]  rI  r1   )r-   r-   r-   r   r.   r"  )r-   rm   r-   rl  r/   r   r0   ró  rō  rõ  rö  r   r÷  rų  r  iŪ’’’r6   iŁ’’’r5   r2   é'   i×’’’iÓ’’’iĶ’’’iķ’’’r  é   éI   é«   r  é/   é   éU   r  é   iė’’’é   i£’’’iŻ’’’r   ió’’’rŹ  é>   r_  i»’’’é    iy’’’r   r  é)   r^  i’’’r|   i’’’r   ić’’’r   éQ   r3   iļ’’’r   iį’’’iå’’’éW   r   r  iõ’’’)	r   r%   r    r©   r8  r   r8   r$   r   )rF   rI   rĆ   rÅ   rG   rN   rŽ  rĘ   rę  rē  r|  r}  rR   r  rŪ  rÜ  rß  rą  rc  rd  re  rf  rč  rķ  Śfd4Śfd5r9   r9   r:   Śtest_StateSpace_feedback`  sl  
><
’
üü’’
÷.
üü’’÷<
žžüū
żż’’ł
žžžžśżżžžų
ī
&(&((ūū&(žžō
&(&((ūū&(žžō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.special.delta_functionsr   Ś(sympy.functions.elementary.miscellaneousr   Ś(sympy.functions.elementary.trigonometricr   Śsympy.matrices.denser   Śsympy.physics.control.ltir   Śsympy.polys.polytoolsr   Śsympy.polys.rootoftoolsr   Śsympy.simplify.simplifyr   Śsympy.core.containersr   Śsympy.matricesr   r   r   r   r   Śsympy.physics.controlr   r   r   r   r    r!   r"   r#   r$   r%   r&   r'   r(   r)   r*   r+   Śsympy.testing.pytestr,   rJ   r   rK   r¹   r8   rL   rM   rA   r„   r   rö   r£   r»  rE   rF   rG   rŪ  rH   rI   rN   rÜ  rŻ  rĆ   rŽ  rß  rÄ   rÅ   rĘ   rą  r+  r*  r)  rl   rŚ   rē   ró   r÷   rś   rü   r  r  r6  r=  rS  rY  rb  rg  ri  rx  r{  r  r  r  r„  rŖ  rÆ  rŗ  rÄ  rÅ  rĒ  rČ  rŃ  rÓ  rņ  r  r  r   r'  rE  r\  rk  rm  r~  r9   r9   r9   r:   Ś<module>   s    H"((a b"+++,>C.5H463& '"(#h&lA"/>A x