o
    GZ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   úS/var/www/auris/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_Ú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tf18r9   r9   r:   Ú"test_TransferFunction_construction"   s   $ 08$,4444,DN((2($2",6,64 >r_   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                      rc   rd   re   r9   )Úexpr_3r9   r:   r;      rh   c                      rc   rd   re   r9   )Úexpr_4r9   r:   r;      rh   )r1   r/   r0   r?   c                      s   t   t¡S rd   )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.   ry   )r   ÚxrE   Úpolesr9   r9   r9   r:   r;     ó   ( 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 )Nry   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 rd   r   r9   ©rW   r9   r:   r;   F  ó    c                      s     S rd   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_exprrf   r   r,   ÚZeroDivisionErrorrO   rk   Zfrom_zpkr   r   rG   rF   rE   rH   rI   rN   ÚexpandÚsubsÚevalfr   Úeval_frequencyÚwnr
   Zdc_gainr   r   r{   Úkr|   Ústrr   ÚNotImplementedErrorZ	is_stabler   r   ÚdoitÚpowr   r   rB   rC   r   rP   )?Zexpr_1Zexpr_5Zexpr_6rQ   ÚdelayZexpr_7ÚH1ÚH2Zexpr_8rR   Znum2Zden2rS   r   r|   ZgainZtf0rJ   rK   ÚG1ÚG2ÚcrM   ÚeÚfrL   Ú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Úrr   Z	stable_tfZ
stable_tf_rX   rY   rZ   r[   r\   r]   r9   )rl   rg   ri   rj   rm   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©Zcommutativec                      ó    t g d¢ S r>   ©r   r9   ©rR   r9   r:   r;   q  ó    z@test_TransferFunction_addition_and_subtraction.<locals>.<lambda>c                      s     S rd   r9   r9   )r   rS   r9   r:   r;   r  r   c                      ó     S rd   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 rd   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   r`   r   Fr¯   c                      ó    t g d¢ S r>   r±   r9   ©r¢   r9   r:   r;     r³   zCtest_TransferFunction_multiplication_and_division.<locals>.<lambda>c                      ó     S rd   r9   r9   )r   r   r9   r:   r;      r   c                      rœ   rd   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 rd   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 rd   r9   r9   ©r¿   rÃ   rÄ   r9   r:   r;   ­  r<   c                      s       S rd   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)r^   Ú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)r^   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   r^   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   Zpar1Úser1Z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 rd   ©r   r9   ©rQ   rT   r9   r:   r;   #  rh   z*test_Series_construction.<locals>.<lambda>c                      ó   t  S rd   rÚ   r9   ©rQ   rS   rT   rU   r9   r:   r;   $  rn   c                      s   t   S rd   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   Ús2Zs3Zs4Zs6Z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 rd   ©r"   r9   r9   r9   r:   r;   E  s    z.test_MIMOSeries_construction.<locals>.<lambda>c                      s
   t  S rd   rì   r9   )Útf_1Útfm_1r9   r:   r;   H  rh   c                      s   t    S rd   rì   r9   ©rî   Útfm_2r9   r:   r;   L  rn   c                      rÙ   rd   rì   r9   )Útfm_3Útfm_5r9   r:   r;   O  rh   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   )Ztf_2Ztf_3Útfm_4Zs8Zs9Zs11r9   )rí   rî   rð   rñ   rò   r:   Útest_MIMOSeries_construction+  s4   H(
&&&rþ   c                     s   t dtd dt t t  td  t t tdtt tt t tt t tt tt tt  t ttt ttd  tt  t tt t}    t	   krlt	t	   krlt	 t	ksoJ  J    t	 t
ksJ   |  t
t	 | ksJ   |  t
t	 |  ks J    |  t
t	 | ks³J    |  t
t	  |  ksÈJ    |  t
t	  | ksÜJ   |   t
t	 t	| ksñJ   |   t
t	 t	t ddtt	| ksJ      t	t
  ks&J    t	  ks4J    t	t ddtt	 ksHJ tt fdd tt fdd ttfdd t	 dd	t	  ¡   krt ttd dt t t  td  tksJ  J t	 t
  dd	t	 t
   ¡   krët ttt t t t t td dt t t  td     tt t td dt t t  td  d  tksîJ  J t	  dd	t	   ¡   kr(t tt t t  tt t td dt t t  td   tks+J  J t	  d
d	t	   ¡ kr>J t	t
 t
  ¡ t ttd dt t t  td   d t t ttt t   t  tt t td dt t t  td   tksJ t	     ¡ t tt t t  tt t td dt t t  td   tks¹J t	  ¡  t t tt t  tt t td dt t t  td   tksæJ t	t
   ¡ t ttt t d  ttd dt t t  td   d  tt t d td dt t t  td   tks.J t	  t ¡t ttd dt t t  td  tksMJ t	   t ¡t tt t t  tt t td dt t t  td   tks{J t	t
 t
 }|jsJ |jrJ |jsJ t	 }|js¥J |js«J |jr±J t	  t
  }|jsÂJ |jsÈJ |jrÎJ d S )Nr-   r.   r`   c                      s      S rd   r9   r9   ©rR   rS   rU   r9   r:   r;   k  r<   z'test_Series_functions.<locals>.<lambda>c                      s      S rd   r9   r9   rÿ   r9   r:   r;   l  r<   c                      r»   r>   r±   r9   r   r9   r:   r;   m  r³   Tra   F)r   r8   rÉ   r   r   rG   rA   rE   rF   r   r   r,   rO   r   ÚrewriterÈ   rÌ   rÎ   )rW   ZS1ZS2Z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 rd   ©rø   r9   ©Útfm1Útfm2r9   r:   r;   €  r<   z+test_MIMOSeries_functions.<locals>.<lambda>c                      r  rd   )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   r  r9   r:   r;   §  rn   c                      s   dt d     S )Nr`   r0   r¹   r9   r  r9   r:   r;   š  r@   c                      s       S rd   r9   r9   ©r  r  r	  r9   r:   r;   ¬  rn   c                      s       S rd   r9   r9   ©r  r  r	  Útfm5r9   r:   r;   ­  r³   c                      s       S rd   r9   r9   r  r9   r:   r;   ®  r³   c                      ó     t  S rd   r  r9   ©r	  r  r9   r:   r;   µ  r<   c                      r  rd   ©rF   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  d    S )Nr1   r-   r¹   r9   r  r9   r:   r;   ¹  ó    Tra   r.   r-   r1   )r!   rø   r÷   rö   r#   r"   r,   rO   rP   r   r   r   rG   r8   rA   r   rÉ   r   rù   r   r   )Útfm3Útfm7Úmat_1Úmat_2Ztm_1Ztm_2r9   )r  r  r	  r  r
  r:   Útest_MIMOSeries_functions  s    &*(2"""4ÿ®4ÿþÿ20V
ÿ,
ÿ4ÿ°4ÿþþ
ÿ~ÿÿÿ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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Ù   rd   ©r   r9   rÛ   r9   r:   r;   þ  rh   z,test_Parallel_construction.<locals>.<lambda>c                      rÜ   rd   r  r9   rÝ   r9   r:   r;   ÿ  rn   c                      s   t   S rd   r  r9   rµ   r9   r:   r;      r<   c                      rß   r   r  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«   Zp5Zp6Zp7r9   rÝ   r:   Ú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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 rd   )r#   rø   r9   r  r9   r:   r;   .  r<   c                      s   t  S rd   r  r9   )r  r  r	  r9   r:   r;   1  r<   c                      rÙ   rd   r  r9   )r  r  r9   r:   r;   4  rh   c                      rß   r   r  r9   )r  r	  r9   r:   r;   7  r<   c                      s   t td tt    S r   )r#   r8   rA   r9   )r  r	  r9   r:   r;   8  rô   )r!   rø   r÷   rö   rù   r   rA   r#   rD   rÑ   r8   rú   rû   rü   r"   r,   rP   rO   )r  r
  Zp8Zp9Zp10Zp11Zp12Zp13r9   )r  r  r  r	  r  r:   Ú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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.   r`   c                      ó      S rd   r9   r9   rÿ   r9   r:   r;   O  r<   z)test_Parallel_functions.<locals>.<lambda>c                      s      S rd   r9   r9   rÿ   r9   r:   r;   P  r<   c                      r°   r>   r±   r9   r   r9   r:   r;   Q  r³   Tra   F)r   r8   rÉ   r   r   rG   rA   rE   rF   r   r   r,   rO   r   r   rÈ   rÌ   rÎ   )rW   ZP1ZP2ZP3r9   r  r:   Útest_Parallel_functions;  s¶   ($"& "& &0.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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 rd   )r÷   r9   r  r9   r:   r;     r<   z-test_MIMOParallel_functions.<locals>.<lambda>c                      s     t  S rd   r  r9   r  r9   r:   r;     r<   c                      s     t d  S r¶   r   r9   ©r  r  r9   r:   r;     r³   c                      s      d S rÁ   r9   r9   r  r9   r:   r;     rn   c                      s   dt d     S rž   r¹   r9   r  r9   r:   r;     r@   c                      s      S rd   r9   r9   )r  r  r  r9   r:   r;     r<   c                      s      S rd   r9   r9   )r  r  r  r9   r:   r;     r<   c                      r  rd   r9   r9   r  r9   r:   r;     r<   c                      s      S rd   r9   r9   r  r9   r:   r;     r<   c                      s     t  S rd   r  r9   r  r9   r:   r;   €  r<   c                      s     t  S rd   )rG   r9   r  r9   r:   r;   ¥  r<   c                      s     t d  S )Nr3   r   r9   r  r9   r:   r;   Š  r³   c                      s      d S r=   r9   r9   )r  r  r  r9   r:   r;   §  r³   c                      s   dt d     S rž   r¹   r9   )r  r  r9   r:   r;   š  r@   c                      s       S rd   r9   r9   )r  r  r	  r  r9   r:   r;   «  r³   c                      s      S rd   r9   r9   )r  r  r  r9   r:   r;   ­  r<   Tra   r-   )r   rE   rA   rF   r8   rG   r!   rø   r÷   rö   r#   r"   r,   rO   rP   r   r   r   r   rÉ   )rU   rW   r
  r9   )r  r  r  r	  r  r  r:   Útest_MIMOParallel_functions  sZ   $8 6,&&".6"
ÿPJÿ,ÿÿþr   c                     s  t dtd dt t t  td  t t tdtt tt t tt t tt tt tt  t ttt ttd  tt  t tt tt tt tt t} t	t ddt   }|j
t ddtt dfksqJ |jt ddtks|J |jt ksJ |jtksJ t	  }|j
 tdfks¢J |j ks©J |jtks³J |jtksºJ t	  }|j
t dfksÎJ |jt ksØJ t	| }|j
| dfksçJ |jksîJ |jtksõJ t	t ddt}|j
t ddtdfksJ |jtksJ |t	ksJ t	t ddt}|j
t ddtdfks6J |jtks>J t	|  t ddt }|j
tt ddtt| t ddt dfksdJ |jtt ddtt| ksvJ tt fdd tt fdd ttdd  ttdd  ttfd	d tt fd
d tt fdd ttfdd d S )Nr-   r.   r`   c                      s   t   S rd   ©r    r9   ©rR   rS   rT   r9   r:   r;   â  rn   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;   ä  r  c                   S   s
   t ddS r¶   r!  r9   r9   r9   r:   r;   å  rh   c                      s   t   S rd   r!  r9   )rS   rU   rW   r9   r:   r;   ç  rn   c                      ó   t  dS )Nç      ø?r!  r9   ©rR   rS   r9   r:   r;   è  r<   c                      s   t   d  S )Nr`   r!  r9   r²   r9   r:   r;   é  r³   c                      rÙ   rd   r!  r9   r   r9   r:   r;   ê  rh   )r   r8   rÉ   r   r   rG   rA   rE   rF   r    rD   r   Úsys1Úsys2rÑ   r,   rP   rO   )rX   Úf1Úf2Úf3Úf4Zf5Zf6Zf7r9   )rR   rS   rT   rU   rW   r:   Ú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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.   r`   c                      s      d     S r¶   r9   r9   r"  r9   r:   r;     ó    z)test_Feedback_functions.<locals>.<lambda>c                      s         S rd   r9   r9   rÝ   r9   r:   r;     râ   )r   r8   rÉ   r   r   rG   rA   rE   rF   r   r   r   r    r,   rP   rO   Úsensitivityr   )rW   rX   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.   r`   )
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 r4  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)ÚrealZnonnegativezK_D, K_P, v)r5  r.   r-   r1   r   )r   rL   r   r8   r   r    r   rB   rC   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Ú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.   r`   r   )r   r8   r!   r$   rD   r&  r'  rÑ   Úsignr"   r   rù   )rR   rS   rT   rU   rî   rð   rñ   r(  r)  r*  Úmatr&  Ú
controllerr+  r9   r9   r:   Útest_MIMOFeedback_constructionf  s>   

(rC  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.   r`   ©rÑ   c                      rÙ   rd   ©r$   r9   r%  r9   r:   r;     rh   z*test_MIMOFeedback_errors.<locals>.<lambda>c                      s   t t S rd   )r$   r#   r9   )rî   rð   rñ   r9   r:   r;      r³   c                      ó   t  dS r¶   rE  r9   )rî   Útfm_6r9   r:   r;   ¢  r<   c                      s
   t   S rd   rE  r9   )Útfm_7r9   r:   r;   £  rh   c                      rF  )Nr   rE  r9   rï   r9   r:   r;   ¥  r<   c                      r#  r¶   rE  r9   ©rý   rò   r9   r:   r;   §  r<   c                      s   t   S rd   rE  r9   rI  r9   r:   r;   š  r<   c                      s   t   dS r¶   rE  r9   rõ   r9   r:   r;   ©  r<   c                      rF  r¶   rE  r9   )rî   Útfm_8r9   r:   r;   ¬  r<   )	r   r8   r!   rù   r   r,   rP   rO   rA   )rT   rU   rW   rX   r9   )
rR   rS   rî   rð   rñ   rý   rò   rG  rH  rJ  r:   Útest_MIMOFeedback_errors  s.   rK  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-   r`   r.   rD  r   r   r/   r1   FT)Úcancelr   r0   )rL  )r   )r   r8   r!   rù   r   r$   r"   r.  r   r
   ZHalfr   r   )
rR   rS   rT   rU   rî   rð   rñ   rý   ZF_1ZF_2r9   r9   r:   Útest_MIMOFeedback_functions¯  st   
"("ÿÿ
ÿ

ÿÿÿý^*ÿ ÿ
ÿ
*"ÿÿ*ÿ þ
ÿ8&ÿÿ
ÿ
N ÿ
ÿrM  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-   r  )r1   r.   c                      s   t tgtg ggS rd   ©r!   rø   r÷   r9   r   r9   r:   r;   ë  r@   z:test_TransferFunctionMatrix_construction.<locals>.<lambda>c                      s   t t gtggS rd   ©r!   rø   rö   r9   r   r9   r:   r;   ì  r@   c                      s   t tgt ggS rd   rO  r9   r   r9   r:   r;   ï  r   c                      s   t ttg ggS rd   rO  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;   ó  r-  c                   S   s   t ttddggttggS rP  )r!   rø   r   rö   r÷   r9   r9   r9   r:   r;   ô  r-  c                      s   t tt gS rd   rN  r9   r   r9   r:   r;   ÷  rn   c                   S   s
   t tgS rd   )r!   rø   r9   r9   r9   r:   r;   ø  rh   )r   rF   r8   rG   rE   rA   r!   rö   rú   rû   rü   rD   r   rÑ   rø   r÷   r,   rO   rP   )Ztfm3_r  r  r9   r   r:   Ú(test_TransferFunctionMatrix_construction×  s,   $&&&&0rQ  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   )r`   r   r1   ru   rp   rq   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   Z	transposeZ
elem_polesr   r   rÉ   Z
elem_zerosr   r   r   Z_flatr   r   r   )rW   r  r  Zmat_3r  r  ZH_1ZH_2ZH_3ZH_4Z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ÿrR  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   ry   r6   )
r   rJ   r8   rK   r&   ÚTr
   ÚZeror   rB   )rQ   ÚnumZÚdenZÚtf_test_bilinearÚtf_test_manualÚtf_test_forwardÚtf_test_backwardZ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'   rS  r
   rT  r   rB   )rQ   rU  rV  rW  rX  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(   rS  r
   rT  r   rB   )rQ   rU  rV  rY  rX  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)   rS  r
   rT  r   rB   )rQ   rU  rV  rZ  rX  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 )Nry   r1   r-   r.   iLÿÿÿéŽ   é   r   c                      ó   t  S rd   ©r*   r9   r   r9   r:   r;   Ž  r   z4test_TransferFunction_phase_margin.<locals>.<lambda>c                      rb  rd   rc  r9   r   r9   r:   r;   µ  r   c                      ó   t t S rd   )r*   r"   r9   ©Útf_mr9   r:   r;   ¶  r<   )r   rA   r8   rJ   rK   r   r   r!   r*   r   r   r   r,   r   rO   r%  r9   ©rT   rU   rf  r:   Ú"test_TransferFunction_phase_margin©  s   $(rh  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-   ry   iìÿÿÿr6   i  c                      rb  rd   ©r+   r9   r   r9   r:   r;   Ã  r   z3test_TransferFunction_gain_margin.<locals>.<lambda>c                      rb  rd   ri  r9   r   r9   r:   r;   Ä  r   c                      rd  rd   )r+   r"   r9   re  r9   r:   r;   Å  r<   )r   r8   rJ   rK   r   r   r!   r+   r   r
   r   r,   r   rO   r%  r9   rg  r:   Ú!test_TransferFunction_gain_marginž  s   ($(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| |||}|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 rP  ©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.   rl  r9   r9   r9   r:   r;   '  rm  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 rP  rl  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.   rl  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   r`   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©   Úd3Zis_SISOr,   r   rP   )ÚA1ÚB1ÚC1ÚD1Úss1Úss2Úss3ÚA4ÚB4ÚC4Z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.   rr   r0   éüÿÿÿr   r1   r`   ©rr   r/   r.   ©r`   rr   r   ©r.   r0   r1   r   ©r/   r.   rr   ©r-   r/   r1   ©r/   r-   r   r   r   ©r.   rr   r   r   r   ©r   r   rr   r/   r.   ©r   r   r`   rr   r   ©r   r   r.   r0   r1   ©r.   r  r/   r.   rr   ©r   r-   r-   r/   r1   r3   )rr   r/   r.   r   r   )r`   rr   r   r   r   )r.   r0   r1   r   r   )é   é   é÷ÿÿÿr/   r-   )éñÿÿÿéîÿÿÿr   r.   rr   éöÿÿÿr  r  )r   r   rr   r.   r  )r1   r   éúÿÿÿr   r-   r  r   r4   r$  ç      @g      @ru   c                      rŽ   rd   r9   r9   )r|  r~  r9   r:   r;   r  r   z%test_StateSpace_add.<locals>.<lambda>c                      rœ   rd   r9   r9   )r}  r  r9   r:   r;   s  r   )r   r%   r,   r   )
rx  ry  rz  r{  ÚA2ÚB2ÚC2ÚD2Zexpected_addZ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 )Nr`   r-   )r   rE   rF   rG   rr  rH   rI   rN   rs  rt  rŠ   ru  rv  r§   rš   r©   rw  r%   )ÚAÚBÚCÚDÚSSZSS_negZ	state_matZ	input_matZ
output_matZ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   é$   é   ry   r   r/   rq   rp   rs   rt   ru   )r%   r   r8   rA   r   r   rJ   rK   rF   rE   rN   rI   rH   r   )r|  r}  r€   r¥   ÚGÚexpectZ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 )%Nrz   r`   r1   r.   r0   r-   r   )r£  r€  ro   é;   r3   r5   )rk  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-   rq   rp   g      %@r  r4   g      @g      @g      @rs   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   r   r%   r9   r9   r9   r:   r;   Û  s   6 z!test_conversion.<locals>.<lambda>)r   r%   rE   rF   rG   rr  rI   rN   rŠ   ru  r   r   r8   rB   rC   rø   r   rÉ   r,   rO   )rx  ry  rz  r{  r   ZH3Ztm1Ztm2rR   rS   r  r  r  r   r   Z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   rr   r`   r.   r   )Úinitial_conditionsr5   r2   r1   )r¢  r€  r0   r  r/   ry   )Úinput_vectorrÑ   rs   gY8bœÉ4@rz   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%   Zdsolver   r   r   r   r   r   r   r   r   r   rD   r   )2rx  ry  rz  r{  ZI1r   r|  r  r  ZI2r}  ZA3ZB3ZC3ZD3ZU3r~  Úopr  r  r  ZU4r  Zop4r7  rJ   Zx0ZA5ZB5ZC5ZI5ZU5r  Zop5Za11Za12Za21Za22rI   rN   rŠ   ru  Úi1Úi2r  r  ZC6ZI6r  Zexpr6r9   r9   r:   Útest_StateSpace_dsolveÞ  sf   
6
F

B&ÿ">.ÿRF,r¶  c               	   C   s   t ddgddgg} t ddg}t ddgg}t dg}t| |||}tt ddgddggt ddgddggt ddgddgg}tt ddgddggt ddgddgg}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 )Nrk  r   r-   r   r4   r/   r.   r`   TFr1   rr   r  rz   g      è¿)r-   r`   r.   r   ©r   r-   r   r   ©r-   r   r   r   ©r   r   r-   r   ©r   r   r   r-   )r   r%   rE   rF   rG   rr  rI   rN   rŠ   ru  Zis_observableZobservability_matrixZobservable_subspacer   Zis_controllableZcontrollability_matrixZcontrollable_subspaceÚappendZ
num_statesrü   rû   rn  ro  rp  rq  )ZA_matZB_matZC_matZD_matZSS1ZSS2ZSS3ZSS4ZQoZQcrx  ry  rz  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-   r·  rž  ©r   r-   r-   r   rº  r.   r1   )r`   r   r   )r   r   r-   )r`   r-   r   r/   rr   r0   r  r`   r  r  r  r   r  r  r  r  )r.   r   rr   r/   r.   )r  r2   r`   rr   r   )r  r2   r.   r0   r1   r6   éôÿÿÿrz   )r  ro   r/   r.   rr   r  éøÿÿÿ)r   r%   r   r8   r   r   rü   rû   r   r"   r!   ) rF   rI   rŠ   rš   rG   rN   ru  r©   r|  r}  rR   rÕ   Zser2Zser3Zser_tfrr  rs  rv  rw  Úa4Úb4Úc4Úd4r~  r  Zser4Zser5Zser6rS   rT   rU   Útfmr9   r9   r:   Útest_StateSpace_series]  st  
þþüþþü
ü	üü

ô2

þþüýý
ö4
þþþþúýýþþøøûûþþð

þþþþúþørÅ  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-   r·  rž  r¹  rº  rœ  r.   r¯  )r-   r   r   )r   r   r`   ©r   r-   r`   r/   rr   r0   r  r1   r`   r  r  r  r   r  r  r  r  r  r  r  r  r  r3   )	r   r%   r   r   r   r   r8   r#   r!   )rF   rI   rŠ   rš   rG   rN   ru  r©   r|  r}  r€   rR   r¥   rr  rs  rv  rw  rÀ  rÁ  rÂ  rÃ  r~  r  rª   rS   rT   rU   rÄ  r«   r9   r9   r:   Útest_StateSpace_parallelè  s:  
>:
ÿ
üü

õ*
þþüüýý
ö(
þþþþúýýþþøø
ûûþþñ
þþþþþúýrÇ  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-   r`   r·  )r-   r`   r`   r   rœ  rº  r1   )r-   r-   r-   r   r.   r¯  )r-   r`   r-   rÆ  r/   rr   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Ýÿÿÿrv   ióÿÿÿra  é>   r¿  i»ÿÿÿé    iyÿÿÿr   r  é)   rŸ  iÿÿÿro   iÿÿÿrz   iãÿÿÿry   éQ   r3   iïÿÿÿrx   iáÿÿÿiåÿÿÿéW   rw   r  iõÿÿÿ)	r   r%   r    r   r   r   r8   r$   r   )rF   rI   rŠ   rš   rG   rN   ru  r©   r|  r}  r0  r1  rR   r3  rr  rs  rv  rw  rÀ  rÁ  rÂ  rÃ  r~  r  Zfd4Zfd5r9   r9   r:   Útest_StateSpace_feedback`  sl  
><
ÿ
üüÿÿ
÷.
üüÿÿ÷<
þþüû
ýýÿÿù
þþþþúýýþþø
î
&(&((ûû&(þþô
&(&((ûû&(þþôrÖ  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.special.delta_functionsr   Z(sympy.functions.elementary.miscellaneousr   Z(sympy.functions.elementary.trigonometricr   Zsympy.matrices.denser   Zsympy.physics.control.ltir   Zsympy.polys.polytoolsr   Zsympy.polys.rootoftoolsr   Zsympy.simplify.simplifyr   Zsympy.core.containersr   Zsympy.matricesr   r   r   r   r   Zsympy.physics.controlr   r   r   r   r    r!   r"   r#   r$   r%   r&   r'   r(   r)   r*   r+   Zsympy.testing.pytestr,   rJ   r{   rK   r   r8   rL   rM   rA   r   r   rÉ   r   rS  rE   rF   rG   rr  rH   rI   rN   rs  rt  rŠ   ru  rv  r§   rš   r©   rw  rø   r÷   rö   r_   r®   rº   rÆ   rÊ   rÍ   rÏ   rÖ   rë   rþ   r  r  r  r  r  r   r,  r/  r4  r?  rC  rK  rM  rQ  rR  r[  r\  r^  r_  rh  rj  r  r¡  rš  r­  r°  r¶  rŒ  rÅ  rÇ  rÖ  r9   r9   r9   r:   Ú<module>   s    H"((a b"+++,>C.5H463& '"(#h&lA"/>A x