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   új/home/www/facesmatcher.com/frenv_anti/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*  Zmatr&  Ú
controllerr+  r9   r9   r:   Útest_MIMOFeedback_constructionf  s>   

(rB  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ś   rD  r9   )rî   Útfm_6r9   r:   r;   ˘  r<   c                      s
   t   S rd   rD  r9   )Útfm_7r9   r:   r;   Ł  rh   c                      rE  )Nr   rD  r9   rď   r9   r:   r;   Ľ  r<   c                      r#  rś   rD  r9   Šrý   rň   r9   r:   r;   §  r<   c                      s   t   S rd   rD  r9   rH  r9   r:   r;   ¨  r<   c                      s   t   dS rś   rD  r9   rő   r9   r:   r;   Š  r<   c                      rE  rś   rD  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ň   rF  rG  rI  r:   Útest_MIMOFeedback_errors  s.   rJ  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.   rC  r   r   r/   r1   FT)Úcancelr   r0   )rK  )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 ˙
˙rL  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   rN  r9   r   r9   r:   r;   ď  r   c                      s   t ttg ggS rd   rN  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 rO  )r!   rř   r   rö   r÷   r9   r9   r9   r:   r;   ô  r-  c                      s   t tt gS rd   rM  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rP  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˙rQ  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&rZ  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'   rR  r
   rS  r   rB   )rQ   rT  rU  rV  rW  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(   rR  r
   rS  r   rB   )rQ   rT  rU  rX  rW  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)   rR  r
   rS  r   rB   )rQ   rT  rU  rY  rW  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                      ra  rd   rb  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   re  r:   Ú"test_TransferFunction_phase_marginŠ  s   $(rg  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                      ra  rd   Šr+   r9   r   r9   r:   r;   Ă  r   z3test_TransferFunction_gain_margin.<locals>.<lambda>c                      ra  rd   rh  r9   r   r9   r:   r;   Ä  r   c                      rc  rd   )r+   r"   r9   rd  r9   r:   r;   Ĺ  r<   )r   r8   rJ   rK   r   r   r!   r+   r   r
   r   r,   r   rO   r%  r9   rf  r:   Ú!test_TransferFunction_gain_margin¸  s   ($(ri  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 rO  Š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.   rk  r9   r9   r9   r:   r;   '  rl  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 rO  rk  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.   rk  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   )
rw  rx  ry  rz  Ú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   rq  rH   rI   rN   rr  rs  rŚ   rt  ru  r§   r¨   rŠ   rv  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   )rj  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   rq  rI   rN   rŚ   rt  r   r   r8   rB   rC   rř   r   rÉ   r,   rO   )rw  rx  ry  rz  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   )2rw  rx  ry  rz  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Ś   rt  Ú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 )Nrj  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   rq  rI   rN   rŚ   rt  Zis_observableZobservability_matrixZobservable_subspacer   Zis_controllableZcontrollability_matrixZcontrollable_subspaceÚappendZ
num_statesrü   rű   rm  rn  ro  rp  )ZA_matZB_matZC_matZD_matZSS1ZSS2ZSS3ZSS4ZQoZQcrw  rx  ry  rz  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   rt  rŠ   r{  r|  rR   rŐ   Zser2Zser3Zser_tfrq  rr  ru  rv  Ú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   rt  rŠ   r{  r|  r¤   rR   rĽ   rq  rr  ru  rv  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ó˙˙˙r`  é>   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   rt  rŠ   r{  r|  r0  r1  rR   r3  rq  rr  ru  rv  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   rR  rE   rF   rG   rq  rH   rI   rN   rr  rs  rŚ   rt  ru  r§   r¨   rŠ   rv  rř   r÷   rö   r_   rŽ   rş   rĆ   rĘ   rÍ   rĎ   rÖ   rë   rţ   r  r  r  r  r  r   r,  r/  r4  r?  rB  rJ  rL  rP  rQ  rZ  r[  r]  r^  rg  ri  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