a
    kº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sHJ | jtd td t d tfkslJ t td td t}|jtd ksJ |jtd ks€J |jtd td tfksÀJ t td td d t}|jtd ksêJ |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sJ t ddt}|jdks®J |jdksŸJ |jddtfksÔJ t ddt}	|	jdksðJ |	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,n 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sNJ 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s¢J 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shJ |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   úR/var/www/auris/lib/python3.9/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;   |   r<   c                   S   s    t td dt  d td dS )Nr.   r-   r1   r7   r9   r9   r9   r:   r;   ~   r<   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Úargsr@   Úa0Úa1Úa2Úb0Úb1ÚaÚbÚgÚdÚb2r   r,   Ú
ValueErrorÚ	TypeError)ÚtfÚtf1Útf2Útf3Útf4Útf4_Útf5Ztf5_Útf6Ztf6_Útf7Útf8Ztf7_Ztf8_Útf9Útf10Ztf10_Útf11Ztf12Ztf13Ztf14Ztf15Úomega_oZk_pZk_oZk_iZtf18r9   r9   r:   Ú"test_TransferFunction_construction"   s    $"0:$.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sJJ t |t¡td
td  dtd   d td td
  tksJ ||ksJ t |t¡tdtd  dt  d td d tksÔJ 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sXJ |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rp|ksvn J ttd td  td td  t}	ttd td t}|	 ¡ t|	  krÎ|ksÔn 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s8J | ¡ |ksJJ | ¡ t||tksdJ | ¡ 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sfJ |  ¡ |ksxJ 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	sVJ |) ¡ |(k	shJ 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
sHJ |, ¡ |+k
sZJ ttttd   tt t t}-tdtd  t tt t}.tdtd  t tt t}/|- tdtd i¡|/k
sÎJ |- tdtd i¡ ¡ |.k
sîJ |/ ¡ |.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sJ | ¡ dksJ |	 ¡ tddks¶J |' ¡ dksÈJ |. ¡ dksÚJ |) ¡ dksìJ 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sXJ |1 #¡ t"tdd t"tdd  tt"tdd  t t"tdd  gksŠJ |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snJ 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 sLJ | '¡ d-u s^J |7 '¡ d-u spJ |8 '¡ d-u sJ |2 '¡ du sJ |/ '¡ d u sŠJ |- '¡ d u sžJ | (¡ ddgksÎJ | (¡ g ksàJ |	 (¡ ddgksöJ |/ (¡ 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sZJ t$|+ (¡ t$d2d3t)d4td  d   d3t)d4td  d  d2 gksšJ |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sbJ  tdt d dt tksJ ttd td tttd d dt tttd d d dt d t}(ttd d td d t}  *¡ d   kr$t+d  kr$|ks*n J    *¡ d   krdt+d  krd|(ksjn J d t+d  krtddtksn J t, *¡ d d   krät+d  krättd td tksên J   *¡ d d6   kr@t+d6  kr@tdt d td d d tksFn 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sÜJ |j. td¡|j/ td¡  krdksn J | td¡ttd td dt  d tks@J |9 td¡ttd dtksbJ  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sXJ 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s4J |= ¡ t tdtddddddks^J |> ¡ tddddkszJ d S );Nr   éÿÿÿÿF©Úevaluater.   r0   r-   r1   r3   r2   r/   r5   c                      s
   t   ¡S ©N©r   Úfrom_rational_expressionr9   )Úexpr_2r9   r:   r;      r<   z1test_TransferFunction_functions.<locals>.<lambda>c                      s
   t   ¡S rb   rc   r9   )Úexpr_3r9   r:   r;      r<   c                      s
   t   ¡S rb   rc   r9   )Úexpr_4r9   r:   r;      r<   )r1   r/   r0   r?   c                      s   t   t¡S rb   )r   Úfrom_coeff_listsr8   r9   )Úden3Únum3r9   r:   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.   ru   )r   ÚxrD   Úpolesr9   r9   r9   r:   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 )Nru   r.   )r   rD   rw   Úzerosr9   r9   r9   r:   r;   2  r<   éþÿÿÿc                      s    t d t  d  S ©Nr.   r-   ©r8   r9   ©rT   r9   r:   r;   E  r<   c                      s    t  S rb   r   r9   ©rV   r9   r:   r;   F  r<   c                      s     S rb   r9   r9   ©rT   rV   r9   r:   r;   G  r<   c                      s      ttdi¡S ©Nr.   )Úxreplacer@   r   r9   ©rS   r9   r:   r;   R  r<   )2r   r	   r8   r@   r   r   ÚtauÚto_exprrd   r   r,   ÚZeroDivisionErrorrN   rh   Zfrom_zpkr   r   rF   rE   rD   rG   rH   rM   ÚexpandÚsubsÚevalfr   Úeval_frequencyÚwnr
   Zdc_gainr   r   rw   Úkrx   Ústrr   ÚNotImplementedErrorZ	is_stabler|   r   ÚdoitÚpowr   r   rA   rB   r   rO   )?Zexpr_1Zexpr_5Zexpr_6rP   ÚdelayZexpr_7ÚH1ÚH2Zexpr_8rQ   Znum2Zden2rR   r|   rx   ZgainZtf0rI   rJ   ÚG1ÚG2ÚcrL   ÚeÚfrK   Ú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Úrrz   Z	stable_tfZ
stable_tf_rW   rX   rY   rZ   r[   r\   r9   )ri   re   rf   rg   rj   rS   rT   rV   r:   Útest_TransferFunction_functions   s    4,$66
6ÿ

.ÿ

&ÿ
.B:$"&$& 4,tXH@>:.(((,&(($(400$(  `0âÿ
  0.NDP(þ$$$H,ÿPÿN8ÿ&ÿB0ÿDP(þ .$ <@0P\ &22",(<:,F6*r§   c                      s  t td td tt td td tt td td t d tt tdt t tksjJ  tksJ    t ks J   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sRJ  t kslJ   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                      s    t g d¢ S r>   ©r   r9   ©rQ   r9   r:   r;   q  r<   z@test_TransferFunction_addition_and_subtraction.<locals>.<lambda>c                      s     S rb   r9   r9   )r   rR   r9   r:   r;   r  r<   c                      s     S rb   r9   r9   ©rS   rT   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   ©r@   r9   rª   r9   r:   r;   v  r<   c                      s    t g d¢ S r>   r©   r9   rª   r9   r:   r;   ~  r<   c                      s     S rb   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   )rR   r9   r:   r;     r<   c                      s   dt d    S )Nr-   r/   r¯   r9   rª   r9   r:   r;     r<   )r   r8   r@   r   r   r,   rN   r9   r9   )r   rQ   rR   rS   rT   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s J    t  ksºJ     t  ksÞJ         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                      s    t g d¢ S r>   r©   r9   ©r   r9   r:   r;     r<   zCtest_TransferFunction_multiplication_and_division.<locals>.<lambda>c                      s     S rb   r9   r9   )r   r   r9   r:   r;      r<   c                      s     S rb   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                      s    d S r=   r9   r9   )ÚG6r9   r:   r;   Š  r<   c                      s     S rb   r9   r9   r²   r9   r:   r;   §  r<   c                      s    d S 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 rb   r9   r9   ©r³   r¶   r·   r9   r:   r;   ­  r<   c                      s       S rb   r9   r9   rž   r9   r:   r;   ®  r<   )	r   r8   r@   r   r   r   r   r,   rN   r9   r9   )r   r   r   r   r³   r¶   r·   r   r:   Ú1test_TransferFunction_multiplication_and_division  sF    $4ÿ
("$r¹   c                  C   sê   t d\} }}t| d td t|  | t  | d  | }t|td  |td  |}ttt td  td  tt  t tttd   t}tttd  td  tt  t ttd  t}|jsÈJ |jsÒJ |jsÜJ |jræJ d S ©Nzomega_o, zeta, taur.   r1   r/   )r   r   r8   r@   rI   rJ   Ú	is_proper)r]   Úzetar   r   r   r   r   r9   r9   r:   Útest_TransferFunction_is_proper±  s    ,80


rœ   c                  C   sê   t d\} }}t| d td t|  | t  | d  | }t|td  |td  |}ttt td  td  tt  t tttd   t}tttd  td  tt  t ttd  t}|jrÈJ |jrÒJ |jsÜJ |jræJ d S rº   )r   r   r8   r@   rI   rJ   Úis_strictly_proper)r]   rŒ   r   rQ   rR   rS   rT   r9   r9   r:   Ú(test_TransferFunction_is_strictly_properœ  s    ,80


r¿   c                  C   sê   t d\} }}t|d td t| | t  |d  |}t| td  | td  | }ttt td  td  tt  t tttd   t}tttd  td  tt  t ttd  t}|jsÈJ |jsÒJ |jrÜJ |jræJ d S )Nztau, omega_o, zetar.   r1   r/   )r   r   r8   r@   rI   rJ   Úis_biproper)r   r]   rŒ   rQ   rR   rS   rT   r9   r9   r:   Ú!test_TransferFunction_is_biproperÉ  s    ,80


rÁ   c            
      C   s  t d\} }}}t| |||}t }t|ts2J t|ts@J |t| |||tksXJ |t| ||dtkspJ |j|td  |t |  | | td  |  | t  ks®J |jtd | t ksÈJ |jtksÖJ |j| ksäJ |j	|ksòJ |j
|ksJ |j|ksJ | ¡ t|td  |t |  | | td  |  | t  td | t tkshJ |jdksxJ |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sLJ |	tt| |||ttttd tksxJ | ¡ tttd | t  td |td  |t |  | | td  |  | t    td td | t  tksòJ | ¡ tt|td  |t |  | | td  |  | t   td td | t  tksTJ |	 ¡ 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   rA   rB   ÚvarÚkpÚkiÚkdrP   r   r»   rÀ   rŸ   r   r   r    )
rÄ   rÅ   rÆ   rP   r   r   rQ   Zpar1Úser1Zfed1r9   r9   r:   Útest_PIDControllerÕ  sF    >V.


,,,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sÜJ |jtksêJ 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spJ 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                      s
   t  S rb   ©r   r9   ©rP   rS   r9   r:   r;   #  r<   z*test_Series_construction.<locals>.<lambda>c                      s   t  S rb   rË   r9   ©rP   rR   rS   rT   r9   r:   r;   $  r<   c                      s   t   S rb   rË   r9   ©rR   rS   r9   r:   r;   %  r<   c                      s   t d S r   rË   r9   ©rP   rT   r9   r:   r;   &  r<   c                      s   t td tt   S r   )r   r8   r@   r9   rÎ   r9   r:   r;   '  r<   c                      s   t  tg d¢S ©N)r-   r.   r1   r/   )r   r   r9   r   r9   r:   r;   (  r<   )r   rD   r8   rE   rF   rG   r@   rH   rM   rŒ   r   r   r   rC   rÃ   r   r,   rN   rO   )ÚinpÚoutÚs0Ús1Útf3_rU   Ú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rLdksRn J t}|jfkstJ |jtksJ |j|j|jf  krŠdks¬n J tt | }|jt | fksâJ |j|j|jf  krdks
n 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 rb   ©r"   r9   r9   r9   r:   r;   E  r<   z.test_MIMOSeries_construction.<locals>.<lambda>c                      s
   t  S rb   rØ   r9   )Útf_1Útfm_1r9   r:   r;   H  r<   c                      s   t    S rb   rØ   r9   ©rÚ   Útfm_2r9   r:   r;   L  r<   c                      s
   t  S rb   rØ   r9   )Útfm_3Útfm_5r9   r:   r;   O  r<   c                      s   t d S r   rØ   r9   ©rÜ   rÝ   r9   r:   r;   R  r<   c                      s   t td tt    S r   )r"   r8   r@   r9   rß   r9   r:   r;   S  r<   c                      s   t tdt g S r¬   )r"   r   r@   r9   ©rÝ   r9   r:   r;   T  r<   )r   rD   r8   rE   rF   rG   r@   rH   rM   rŒ   r   r!   ÚTF3ÚTF2ÚTF1Úfrom_Matrixr   r"   rC   rÃ   ÚshapeÚnum_outputsÚ
num_inputsr#   r,   rN   rO   )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rÖt	t	   krÖt	 t	ksÜn J    t	 t
ksüJ   |  t
t	 | ksJ   |  t
t	 |  ksBJ    |  t
t	 | ksjJ    |  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sVJ    t	  ksrJ    t	t dd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  tks.n 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ân J t	  dd	t	   ¡   krTt tt t t  tt t td dt t t  td   tksZn 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svJ 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sJ 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s6J t	 }|jsNJ |jsZJ |jrfJ t	  t
  }|jsJ |jsJ |jr J d S )Nr-   r.   r_   c                      s      S rb   r9   r9   ©rQ   rR   rT   r9   r:   r;   k  r<   z'test_Series_functions.<locals>.<lambda>c                      s      S rb   r9   r9   rê   r9   r:   r;   l  r<   c                      s    t g d¢ S r>   r©   r9   r   r9   r:   r;   m  r<   Tr`   F)r   r8   rŒ   r   r   rF   r@   rD   rE   r   r   r,   rN   r   Úrewriter»   rŸ   rÀ   )rV   ZS1ZS2ZS3r9   ©rQ   rR   rS   rT   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sâJ    | tt  | ksJ    tt ks8J      tt t kslJ 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s8J    |  t|   ksZJ ttfd
d ttfdd ttfdd ttfdd ttfdd t ddt  	¡   krt t
td  tt t d  td dt t t  td  d  t t t tt t  td dt t t  td  d   tt t d  tt t d td dt t t  td  d  tft
td tt t d  td dt t t  td  d  t t t tt t  td dt t t  td    tt t tt t  td dt t t  td    tt t d td dt t t  td  d  tffksn 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n 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	sFn J d S )Nc                      s     t  S rb   ©rã   r9   ©Útfm1Útfm2r9   r:   r;   €  r<   z+test_MIMOSeries_functions.<locals>.<lambda>c                      s     t  S rb   )rD   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;   §  r<   c                      s   dt d     S )Nr_   r0   r¯   r9   rï   r9   r:   r;   š  r<   c                      s       S rb   r9   r9   ©rð   rñ   ró   r9   r:   r;   ¬  r<   c                      s       S rb   r9   r9   ©rð   rñ   ró   Útfm5r9   r:   r;   ­  r<   c                      s       S rb   r9   r9   rö   r9   r:   r;   ®  r<   c                      s     t  S rb   rî   r9   ©ró   r÷   r9   r:   r;   µ  r<   c                      s     t  S rb   ©rE   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;   ¹  r<   Tr`   r.   r-   r1   )r!   rã   râ   rá   r#   r"   r,   rN   rO   r   r   r   rF   r8   r@   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     &,*4"""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sÜJ |jtksêJ 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stJ 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sRJ 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
   t  S rb   ©r   r9   rÌ   r9   r:   r;   þ  r<   z,test_Parallel_construction.<locals>.<lambda>c                      s   t  S rb   rÿ   r9   rÍ   r9   r:   r;   ÿ  r<   c                      s   t   S rb   rÿ   r9   r«   r9   r:   r;      r<   c                      s   t d S r   rÿ   r9   rÏ   r9   r:   r;     r<   c                      s   t td tt   S r   )r   r8   r@   r9   rÎ   r9   r:   r;     r<   c                      s   t  tg d¢S rÐ   )r   r   r9   r   r9   r:   r;     r<   )r   rD   r8   rE   rF   rG   r@   rH   rM   rŒ   r   r   r   rC   rÃ   r   r,   rN   rO   )rÑ   rÒ   Zp0r   rÕ   rU   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sŠJ |j	t
ksŽJ |j|j|jf  krÒdksØn J tt }|jt fksJ |j	t
ksJ |j|j|jf  kr4dks:n J t t}|j tfkshJ |j	t
ksxJ |j|j|jf  krdks n J t }|j fksÂJ |j|j|jf  krädksên J t|| }|j|| fksJ |j|j|jf  kr*dks0n J tt  }|jt  fksjJ |j|j|jf  krdksn 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 rb   )r#   rã   r9   rï   r9   r:   r;   .  r<   c                      s   t  S rb   r  r9   )rð   rú   ró   r9   r:   r;   1  r<   c                      s
   t  S rb   r  r9   )rú   rû   r9   r:   r;   4  r<   c                      s   t d S r   r  r9   )rð   ró   r9   r:   r;   7  r<   c                      s   t td tt    S r   )r#   r8   r@   r9   )rñ   ró   r9   r:   r;   8  r<   )r!   rã   râ   rá   rä   r   r@   r#   rC   rÃ   r8   rå   ræ   rç   r"   r,   rO   rN   )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s¬J    |  t	 | ksÎJ    |  t	  |  ksôJ    t	 t
ksJ    t	 t
 ks:J    t	   ksXJ    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sZJ   |  |   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sDn J t	 t
  dd	t	 t
   ¡   krBt ttt t  td dt t t  td  d  t t t td dt t t  td    tt t td dt t t  td    tt t td dt t t  td  d  tksHn 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n 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s8J 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	rJt	t	 k	sPn J t	t
 t
}|j	srJ |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      S rb   r9   r9   rê   r9   r:   r;   O  r<   z)test_Parallel_functions.<locals>.<lambda>c                      s      S rb   r9   r9   rê   r9   r:   r;   P  r<   c                      s    t g d¢ S r>   r©   r9   r   r9   r:   r;   Q  r<   Tr`   F)r   r8   rŒ   r   r   rF   r@   rD   rE   r   r   r,   rN   r   rë   r»   rŸ   rÀ   )rV   ZP1ZP2ZP3r9   rì   r:   Útest_Parallel_functions;  s¶    ($"&"$("(2.02"Jÿÿ
4,ÿ*ÿÿÿÿþÿ*j,ÿÿ&nÿ
l,ÿÿ
j,ÿÿ
hÿ,ÿÿ
Jÿ
^ÿ,ÿÿ
>r  c               
      sö  t tt tt  t tt} t ttd  tt  t tt t}ttgtgt	gg tt g|gt ggt|g| gtggtt| gt|ggtttgt	| ggtt gg}t| g|  g| gg   t
   krt
t
 ksn J    t
   ks.J    | |   t
 t| |  ksfJ      |  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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
 ddt
  ¡   krìt
  t¡  krìtt t td dt t t  td   d td dt t t  td  tft t ttd   tt  ttt   tt tft t t t tt t td dt t t  td    tt t td dt t t  td   tffksòn J d S )Nr.   c                      s     t  S rb   )râ   r9   rï   r9   r:   r;     r<   z-test_MIMOParallel_functions.<locals>.<lambda>c                      s     t  S rb   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;     r<   c                      s   dt d     S r®   r¯   r9   r  r9   r:   r;     r<   c                      s      S rb   r9   r9   )rñ   rú   rû   r9   r:   r;     r<   c                      s      S rb   r9   r9   )rð   rñ   rû   r9   r:   r;     r<   c                      s      S rb   r9   r9   rõ   r9   r:   r;     r<   c                      s      S rb   r9   r9   rõ   r9   r:   r;     r<   c                      s     t  S rb   rî   r9   rø   r9   r:   r;   €  r<   c                      s     t  S rb   )rF   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 rb   r9   r9   )rñ   rú   ró   r÷   r9   r:   r;   «  r<   c                      s      S rb   r9   r9   )rð   rñ   r÷   r9   r:   r;   ­  r<   Tr`   r-   )r   rD   r@   rE   r8   rF   r!   rã   râ   rá   r#   r"   r,   rN   rO   r   rë   r   r   rŒ   )rT   rV   rô   r9   )rð   rñ   rú   ró   r÷   rû   r:   Útest_MIMOParallel_functions  sZ    $:"8.((".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sâJ |jt ddtksøJ |jt ksJ |jtks J t	  }|j
 tdfksJJ |j ksZJ |jtkspJ |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s0J |jtks@J |t	ksRJ t	t ddt}|j
t ddtdfksJ |jtksJ t	|  t ddt }|j
tt ddtt| t ddt dfksÞJ |jtt ddtt| ksJ 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 rb   ©r    r9   ©rQ   rR   rS   r9   r:   r;   â  r<   z,test_Feedback_construction.<locals>.<lambda>c                      s   t  tg d¢S 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;   å  r<   c                      s   t   S rb   r  r9   )rR   rT   rV   r9   r:   r;   ç  r<   c                      s   t  dS )Nç      ø?r  r9   ©rQ   rR   r9   r:   r;   è  r<   c                      s   t   d  S )Nr_   r  r9   rª   r9   r:   r;   é  r<   c                      s
   t  S rb   r  r9   r   r9   r:   r;   ê  r<   )r   r8   rŒ   r   r   rF   r@   rD   rE   r    rC   r   Úsys1Úsys2rÃ   r,   rO   rN   )rW   Úf1Úf2Úf3Úf4Zf5Zf6Zf7r9   )rQ   rR   rS   rT   rV   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sÞJ   |   t	   ¡ t
|   ¡ dksJ     t ks8J       t   ksdJ      t ksJ      t  ksŽJ    |    t | ksàJ    |     t |  t |  fv s&J t ddt|   t|ksNJ |  |   t|  ksjJ 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s|J 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s J 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sLJ 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;     r<   z)test_Feedback_functions.<locals>.<lambda>c                      s         S rb   r9   r9   rÍ   r9   r:   r;     r<   )r   r8   rŒ   r   r   rF   r@   rD   rE   r   r   r   r    r,   rO   rN   Úsensitivityrë   )rV   rW   r9   )rP   rQ   rR   rS   rT   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sRJ t|ts`J |j| ksnJ |j| ks|J |jt|t|| ksJ |jt|t||  ks²J t| |}t| |}| | |ksÖJ | | |ksæJ | 	¡ 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sbJ 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   rA   rB   r   r   r   )	rQ   rR   Úfd1Úfd2ÚunitrÔ   r   Úfd3rS   r9   r9   r:   Útest_Feedback_with_Series   s0    

2J"r  c                  C   sì  t dddd\} }}}}}t ddd\}}}| ||d   | t | }	|| }
|||  }|d t ||  }t| |
t d  |	td  d t}td|td  tt  t}td|t t}t|t dt}t|tt}tddt}t|| |}t|| | |}|tt|t|||ks*J |jt|t||ksFJ |jt	|t|t|t||ksnJ || 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)r  r.   r-   r1   r   )r   rK   r   r8   r   r    r   rA   rB   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sJ |j|ks¬J |j|ksºJ |jtksÈJ |jdksÖJ |  |ksæJ t||d}|j||dfksJ |j|ksJ |j|ks(J |jtks8J |jdksHJ t|t	||}	|	j|t	||dfkstJ |	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$n J d S )Nr-   r1   r.   r_   r   )r   r8   r!   r$   rC   r  r  rÃ   Úsignr"   r   rä   )rQ   rR   rS   rT   rÚ   rÜ   rÝ   r  r  r  Úmatr  Ú
controllerr  r9   r9   r:   Útest_MIMOFeedback_constructionf  s>    

r'  c                     sš  t dtt t ttd d tt ttd t} t ttd d t}t ddt}t dtd t}t g| |ggt| g| ggtjtdtdt |g||ggt|  | g| |ggt|  ggt| |ggtt fdd ttfdd ttfd	d ttfd
d ttfdd ttfdd ttfdd ttfdd tjtdtd	tt	fdd d S )Nr-   r1   r.   r_   ©rÃ   c                      s
   t  S rb   ©r$   r9   r
  r9   r:   r;     r<   z*test_MIMOFeedback_errors.<locals>.<lambda>c                      s   t t S rb   )r$   r#   r9   )rÚ   rÜ   rÝ   r9   r:   r;      r<   c                      s   t  dS r¬   r)  r9   )rÚ   Útfm_6r9   r:   r;   ¢  r<   c                      s
   t   S rb   r)  r9   )Útfm_7r9   r:   r;   £  r<   c                      s   t  dS )Nr}   r)  r9   rÛ   r9   r:   r;   ¥  r<   c                      s   t  dS r¬   r)  r9   ©rè   rÞ   r9   r:   r;   §  r<   c                      s   t   S rb   r)  r9   r,  r9   r:   r;   š  r<   c                      s   t   dS r¬   r)  r9   rà   r9   r:   r;   ©  r<   c                      s   t  dS r¬   r)  r9   )rÚ   Útfm_8r9   r:   r;   ¬  r<   )	r   r8   r!   rä   r   r,   rO   rN   r@   )rS   rT   rV   rW   r9   )
rQ   rR   rÚ   rÜ   rÝ   rè   rÞ   r*  r+  r-  r:   Útest_MIMOFeedback_errors  s.    r.  c            
      C   sR  t dtt} t ttd t}t ddt}t dtd t}tjtdtd}t| |g||gg}t| |g||gg}t| |g| | gg}t||}t|t|| d}	|jtt	j
dgdt	j
ggksÊJ |	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s0J | ¡ tt ddt tt ddtft ddtt ddtff  kr| t¡ksn 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sPJ |	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s4J | ¡  |  ¡ ksNJ d S )Nr-   r_   r.   r(  r   r}   r/   r1   FT)Úcancelr   r0   )r/  )r   )r   r8   r!   rä   r   r$   r"   r  r   r
   ZHalfr   rë   )
rQ   rR   rS   rT   rÚ   rÜ   rÝ   rè   ZF_1ZF_2r9   r9   r:   Útest_MIMOFeedback_functions¯  st    
"("ÿÿ
ÿ

ÿÿÿ
ý^*ÿ ÿÿ

*"ÿÿ*ÿ þÿ
8&ÿÿÿ

N ÿÿ
r0  c                     s  t ttd  tt  t tt tt tt tt  t tt tt gg} | j| j	| j
f  krldksrn J | jtttt ksJ | jtksJ ttt gtgg}|j|j	|j
f  krÐdksÖn 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rLdksRn 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 rb   ©r!   rã   râ   r9   r   r9   r:   r;   ë  r<   z:test_TransferFunctionMatrix_construction.<locals>.<lambda>c                      s   t t gtggS rb   ©r!   rã   rá   r9   r   r9   r:   r;   ì  r<   c                      s   t tgt ggS rb   r2  r9   r   r9   r:   r;   ï  r<   c                      s   t ttg ggS rb   r2  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 r3  )r!   rã   r   rá   râ   r9   r9   r9   r:   r;   ô  r<   c                      s   t tt gS rb   r1  r9   r   r9   r:   r;   ÷  r<   c                   S   s
   t tgS rb   )r!   rã   r9   r9   r9   r:   r;   ø  r<   )r   rE   r8   rF   rD   r@   r!   rá   rå   ræ   rç   rC   r   rÃ   rã   râ   r,   rN   rO   )Ztfm3_r÷   rû   r9   r   r:   Ú(test_TransferFunctionMatrix_construction×  s,    $$$&(2r4  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s2J t |t¡tt ddtt ddtgt ddtt ddtggksxJ 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sJ | 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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s°J | ¡ 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sVJ | ¡ g d	¢g gg g ggksxJ | ¡ 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	sDJ | ¡ 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
sJ | ¡ tt ttd
  td  td d tt ddtft dt dtt tttffk
súJ | tdtdtdi¡ ¡ tt dttt dt dtd  d tffksHJ tt td td  t ttd  tt td td  td td  tgg}
|
 ¡ t|
  krîtt td  td  d td tt td td tffksôn 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sVJ |
 ¡ 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   rq   rl   rm   gTã¥Ä 	@g
×£p=
@gÖÿ9Ì@gTã¥Ä @) r   rE   r8   rF   rD   r   r@   r!   rä   rã   râ   rá   rI   r   r   r   r
   r   r   Z	transposeZ
elem_polesr   r   rŒ   Z
elem_zerosr   r   r   Z_flatr   r   r   )rV   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ÿr5  c                  C   sØ  t dtt t t} t| td\}}t t|d  |d  t|d  |d  t}t tt dttt d    tdttt d     tt tt d  ttt d    t}tj| ¡ | ¡   ¡ j	ksÎJ 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sVJ 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sÔJ d S )	Nr-   r4   r   r.   g333333Ó?r1   ru   r6   )
r   rI   r8   rJ   r&   ÚTr
   ÚZeror   rA   )rP   Ú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sÌJ d S )Nr-   r   r.   )
r   rI   r8   rJ   r'   r6  r
   r7  r   rA   )rP   r8  r9  r:  r;  r9   r9   r:   Útest_TransferFunction_bilinear  s
    ,^r?  c                  C   s   t dtt t t} t| t\}}t |d t|d  |d  t}t tt tt tt  t  t}tj| ¡ | ¡   ¡ j	ksJ d S ©Nr-   r   )
r   rI   r8   rJ   r(   r6  r
   r7  r   rA   )rP   r8  r9  r<  r;  r9   r9   r:   Ú"test_TransferFunction_forward_diff  s
     "rA  c                  C   s   t dtt t t} t| t\}}t t|d  |d  t|d  |d  t}t tt ttt   ttttt    t}tj| ¡ | ¡   ¡ j	ksJ d S r@  )
r   rI   r8   rJ   r)   r6  r
   r7  r   rA   )rP   r8  r9  r=  r;  r9   r9   r:   Ú#test_TransferFunction_backward_diff  s
    ,,rB  c                     sÞ   t dtd d t} t td dt}t dtt t t t td ttt  td d tt|g ggt| ddt	dt
d  t  ksJ t|dks€J ttfd	d
 tt fdd
 ttfdd
 d S )Nru   r1   r-   r.   iLÿÿÿéŽ   é   r   c                      s   t  S rb   ©r*   r9   r   r9   r:   r;   Ž  r<   z4test_TransferFunction_phase_margin.<locals>.<lambda>c                      s   t  S rb   rE  r9   r   r9   r:   r;   µ  r<   c                      s   t t S rb   )r*   r"   r9   ©Útf_mr9   r:   r;   ¶  r<   )r   r@   r8   rI   rJ   r   r   r!   r*   r   r   r   r,   r   rN   r
  r9   ©rS   rT   rG  r:   Ú"test_TransferFunction_phase_margin©  s    $(rI  c                     sþ   t td dtd  td  td  t} t td dt  d dt}t dtt t t t td ttt  td d tt|g ggt| dtt	dd  td ksŽJ t|t
ksÄJ ttfdd	 tt fd
d	 ttfdd	 d S )Nr.   r0   r-   ru   iìÿÿÿr6   i  c                      s   t  S rb   ©r+   r9   r   r9   r:   r;   Ã  r<   z3test_TransferFunction_gain_margin.<locals>.<lambda>c                      s   t  S rb   rJ  r9   r   r9   r:   r;   Ä  r<   c                      s   t t S rb   )r+   r"   r9   rF  r9   r:   r;   Å  r<   )r   r8   rI   rJ   r   r   r!   r+   r   r
   r   r,   r   rN   r
  r9   rH  r:   Ú!test_TransferFunction_gain_marginž  s    ($(rK  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sdJ |jt ddgkszJ |jt ddggksJ |jt dgksŠJ |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 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sXJ |jt t
ggkspJ |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s6J |jt ddgddggksVJ |jt ddgddggksvJ |jt ddgddggt ddgddggt ddgddggt ddgddggfksÎ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}
t|||	|
}|jt ttgt	t
ggksLJ |jt ttgttggkslJ |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sBJ t ddgddgg}t ddg}t||}|jt ddgddggksJ |jt ddgks€J |jt ddggksŸJ |jt dggksÖJ |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sLJ |jdks\J |jdkslJ |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 r3  ©r%   r   r8   r9   r9   r9   r:   r;   %  s    ÿz.test_StateSpace_construction.<locals>.<lambda>c                   S   s@   t ttgttd td d gttd d gtdt gS )Nr-   r1   r.   rM  r9   r9   r9   r:   r;   '  s    ÿc                   S   sL   t ttgttd gttd d gtd dt  d ggtdt gS r3  rM  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.   rM  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_matrixrC   rD   rE   rF   Úa3rG   rH   rM   Úb3Úc0r   Úc2Úc3r    r¡   r¢   Úd3Zis_SISOr,   r   rO   )ÚA1ÚB1ÚC1ÚD1Úss1Úss2Úss3ÚA4ÚB4ÚC4ZD4Úss4Úss5ÚA6ÚB6Úss6r9   r9   r:   Útest_StateSpace_constructionÈ  sž    
Dÿ8ý    ÿÿÿÿú
    ÿÿÿÿú
8
 ÿÿ
û
rg  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s8J d% tt dggt dggt d&ggt d'ggkstJ d% tt dggt d(ggt dggt d'ggks°J tt fd)d* ttfd+d* d S ),Nr/   r-   r.   rn   r0   éüÿÿÿr   r1   r_   ©rn   r/   r.   ©r_   rn   r   ©r.   r0   r1   r}   ©r/   r.   rn   ©r-   r/   r1   ©r/   r-   r   r   r   ©r.   rn   r   r   r   ©r   r   rn   r/   r.   ©r   r   r_   rn   r   ©r   r   r.   r0   r1   ©r.   rh  r/   r.   rn   ©r   r-   r-   r/   r1   r3   )rn   r/   r.   r   r   )r_   rn   r   r   r   )r.   r0   r1   r   r   )é   é   é÷ÿÿÿr/   r-   )éñÿÿÿéîÿÿÿr   r.   rn   éöÿÿÿru  rx  )r{   r{   rn   r.   rh  )r1   r}   éúÿÿÿr   r-   r{  r{   r4   r	  ç      @g      @rq   c                      s     S rb   r9   r9   )r\  r^  r9   r:   r;   r  r<   z%test_StateSpace_add.<locals>.<lambda>c                      s     S rb   r9   r9   )r]  rb  r9   r:   r;   s  r<   )r   r%   r,   r   )
rX  rY  rZ  r[  ÚA2ÚB2ÚC2ÚD2Zexpected_addZexpected_mulr9   )r\  r]  r^  rb  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s2J |
 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szJ | |ksJ |
    |
ks J d S )Nr_   r-   )r   rD   rE   rF   rR  rG   rH   rM   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shJ | 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sòJ | ¡ |ksJ d S )Nr-   r.   r1   r0   é$   é   ru   r   r/   rm   rl   ro   rp   rq   )r%   r   r8   r@   r   r   rI   rJ   rE   rD   rM   rH   rG   r   )r\  r]  r   r   ÚGÚexpectZexpect_r9   r9   r:   Ú!test_SymPy_substitution_functions  s$    :F@R@ÿ
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sÔJ |	j|j ksæJ |	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s2J |d d tdtd  d!t  d dtd  dtd   dt  d tksJ | 
t¡tt t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 )%Nrv   r_   r1   r.   r0   r-   r   )r  r  rk   é;   r3   r5   )rL  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-   rm   rl   g      %@r|  r4   g      @g      @g      @ro   r}   c                   S   s6   t ttd  td  tt  t ttd  t t¡S r   )r   rJ   r8   r@   rI   rë   r%   r9   r9   r9   r:   r;   Û  r<   z!test_conversion.<locals>.<lambda>)r   r%   rD   rE   rF   rR  rH   rM   r   rU  rë   r   r8   rA   rB   rã   r   rŒ   r,   rN   )rX  rY  rZ  r[  r   ZH3Ztm1Ztm2rQ   rR   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szJ |j|dt d	t|  d
td|   ggks°J 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sôJ 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s°J d S )Nr   r-   r}   rn   r_   r.   rz   )Úinitial_conditionsr5   r2   r1   )r  r  r0   rh  r/   ru   )Úinput_vectorrÃ   ro   gY8bœÉ4@rv   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   rC   r   )2rX  rY  rZ  r[  ZI1rz   r\  r}  r  ZI2r]  ZA3ZB3ZC3ZD3ZU3r^  Úopr_  r`  ra  ZU4rb  Zop4r  rI   Zx0ZA5ZB5ZC5ZI5ZU5rc  Zop5Za11Za12Za21Za22rH   rM   r   rU  Úi1Úi2rd  re  ZC6ZI6rf  Zexpr6r9   r9   r:   Útest_StateSpace_dsolveÞ  sf    
6
H

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sîJ | 
¡ 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sNJ | ¡ 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sªJ | ¡ t tgtggt tt tt  gtt tt  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 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sPJ |jt ddgddgddgddggks|J |jt g d¢g d¢gksJ |jt ddgddggksŒJ |j|j|j ksÔJ |j|j|j ksìJ |j|j|j ksJ |jt ttddgttddgg d¢g d¢gks8J |jt tdgtdgddgddggksdJ |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 )NrL  r}   r-   r   r4   r/   r.   r_   TFr1   rn   r{  rv   g      è¿)r-   r_   r.   r}   ©r   r-   r   r   ©r-   r   r   r   ©r   r   r-   r   ©r   r   r   r-   )r   r%   rD   rE   rF   rR  rH   rM   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^  rb  r9   r9   r:   Útest_StateSpace_functions  sn    
<*2"..(2"". þþþ2<

,,  4,$r  c                   C   sN  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s6J | ¡ 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srJ | ¡ 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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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sLJ | t
¡| t
¡ksfJ 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sJJ d S )"Nr   r-   r  r  ©r   r-   r-   r   r  r.   r1   )r_   r   r   )r   r   r-   )r_   r-   r   r/   rn   r0   rh  r_   ri  rj  rk  r}   rl  rm  rn  ro  )r.   r   rn   r/   r.   )r{  r2   r_   rn   r   )rh  r2   r.   r0   r1   r6   éôÿÿÿrv   )rh  rk   r/   r.   rn   rt  éøÿÿÿ)r   r%   r   r8   r   r   rç   ræ   rë   r"   r!   ) rE   rH   r   r¡   rF   rM   rU  r¢   r\  r]  rQ   rÇ   Zser2Zser3Zser_tfrR  rS  rV  rW  Úa4Úb4Úc4Úd4r^  rb  Zser4Zser5Zser6rR   rS   rT   Útfmr9   r9   r:   Útest_StateSpace_series]  st   
þþüþþüü	üü
ô4

þþüýý
ö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svJ | t¡ttd td d t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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 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/   rn   r0   rh  r1   r_   ri  rj  rk  r}   rl  rm  rn  ro  rp  rq  rr  rs  rt  r3   )	r   r%   r   r   rë   r   r8   r#   r!   )rE   rH   r   r¡   rF   rM   rU  r¢   r\  r]  r   rQ   r   rR  rS  rV  rW  r   r¡  r¢  r£  r^  rb  r£   rR   rS   rT   r€  r€   r9   r9   r:   Útest_StateSpace_parallelè  s:   
>:ÿ
üü
õ,
þþüüýý
ö(
þþþþúýýþþøø
ûûþþñ
þþþþþúýr§  c                  C   s4  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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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s¬J | ¡ 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shJ 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s0J d S )MNr   r-   r_   r  )r-   r_   r_   r   r  r  r1   )r-   r-   r-   r   r.   r  )r-   r_   r-   rŠ  r/   rn   r0   rh  ri  rj  rk  r}   rl  rm  rx  iÛÿÿÿr6   iÙÿÿÿr5   r2   é'   i×ÿÿÿiÓÿÿÿiÍÿÿÿiíÿÿÿrw  é   éI   é«   r  é/   é   éU   rv  é   iëÿÿÿé   i£ÿÿÿiÝÿÿÿrr   ióÿÿÿrD  é>   r  i»ÿÿÿé    iyÿÿÿr{   rz  é)   r  iÿÿÿrk   iÿÿÿrv   iãÿÿÿru   éQ   r3   iïÿÿÿrt   iáÿÿÿiåÿÿÿéW   rs   ry  iõÿÿÿ)	r   r%   r    r   rë   r   r8   r$   r   )rE   rH   r   r¡   rF   rM   rU  r¢   r\  r]  r  r  rQ   r  rR  rS  rV  rW  r   r¡  r¢  r£  r^  rb  Zfd4Zfd5r9   r9   r:   Útest_StateSpace_feedback`  sl   
><ÿ
üüÿÿ÷0
üüÿÿ÷<
þþüû
ýýÿÿù
þþþþúýýþþø
î
&(&((ûû&(þþô
&(&((ûû&(þþô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,   rI   rw   rJ   r   r8   rK   rL   r@   r   r   rŒ   r   r6  rD   rE   rF   rR  rG   rH   rM   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  r  r#  r'  r.  r0  r4  r5  r>  r?  rA  rB  rI  rK  rg  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