From aeb8b828f519c198cbfb2f71683f7bc236e9154e Mon Sep 17 00:00:00 2001 From: Anson Date: Mon, 2 Aug 2021 02:43:34 -0700 Subject: [PATCH] Simulink 2 fully implemented #3 --- DancingT.png | Bin 0 -> 33094 bytes Quaternions.jl | 48 ++++++++++++++++++++++++++++---- README.md | 1 + SADC.jl | 74 +++++++++++++++++++++++++++++++++++++++++++++---- 4 files changed, 112 insertions(+), 11 deletions(-) create mode 100644 DancingT.png diff --git a/DancingT.png b/DancingT.png new file mode 100644 index 0000000000000000000000000000000000000000..5ddad21027ae0791dcefb261461881507fb6b9f7 GIT binary patch literal 33094 zcmafbby$>5*zcm!A>G{#g3^L?cXxv#taO*KfJjOs-5ruk!_p-kQqrPG2?&D1nSH`uN#5^~5K4~!rXlthdN;3xkKqo=@sXf~>f za*%uE-~6t!WC(;FqAVw^=bQaA$0w9n{`auu)ozpMB=-`y@%8_zAG zGNhh#-yRo4(XM>5jXD{dEm1w`xxZ^~Zx`Yw*-LsWCNoyjD|P=zbN>PNcxiF5=a2bU zf3MFxzI}e~_sh;S9Zy9?X4r|ty%OX4R^0#d~^6fi#_Q>PEOEX zsv(wCo>WNi?eC+nA@~1O`MNx>)_wRayCoPH3Tm*@($a+dFAkvV?StPA!52^5ca^@@ zH8(ed`%_m^BImaXy?3FEQ@Gfn9&THQwyyHr7;b4&4=Wp4VU@TP#3#Cc=Xcv4X#EzeOQ-1C6nv5pk%#9{g1V55ZwRrjY+Qn8fJoK83Ur2d}vhuod#N!^_#boaEcAOE|( zA|oX=(9U)}R#!DyaZuhl2h>1_x%j+_Y$5cgC^%>>l=~n;h6jNc-YhSF0K#QPDeTeTtL6qEm zyXSWu$iZrB-N_hRUSLv2#=%#7^2kgfmyeSU=l53r{_PrN;+_XuP-yLmMw~(;c)X!? z*V{3Cff^bwSd+W1HUbcqJ%4|xLVR~WCaF`DF^O2OLw@w3)9s3U{`%|Y z^6+2G$E2hN`|-5-`T64FVhp;^U?d<`*QZ+q8}|!_mMFc?mdu!L4EuP%D9`!WAFs7> zrjFhBXmO^Vf1A3xpZ`8rhGO+|`4f2hbBCAR;_9q(GewI3PMVCkiP$tQT7^9JCqn-2 z&_U{L2C=@nESY)WLb9I+Ec!(3!2QnmD;AvYr!+G73%a`{5h2;`Uu2T)+0!M#d=rOI zi+fkMv>3p3^!2^gy_RasJA|a9?$a%U|NK3lio&62aG3lEhDTtt5!xECJrHeZ8FcCM zHRx(5K}zy`LQtdZ94y+Zii)%Kb8si?{)aUS3k%??r2?-{N0ZrvT~;oJrS6{SWlRo( zk67sFGRCdQd+t1z-JRv-2LsVWS8E~XUh6b=!hVOIHX??ABhT_23#PJV;bFPC4ViEH1k~HRQURRUHs6Jmco%i)p z?Q^ga<7vd9J^y~!b>BGsn6DtbYfMp1Zuvbx{QeOvkKmUCA9EzNWT41Au0vy+ZkdKt z^NP)HVGn& z_($Ti;a7Gt1{N?5q^{?#y{Gx>{oNHL^f{9nC>gAg22~OJf(R>(TnUc!adceD-44bH z43u7B)qTH>oiop)6%q(odpu^Mo(K4l3^msK;RbNWTaDHZFtBVL$xA_HjEjrAV`)2X zKZIC~r|}TnourHti7^$Mkz#NZ;o{;JK&KBc!R2|0P=gwPd#&61RQtd*=KXt^rO(ld zzK)K7%>YKXI|f`F zxDApRB1gwcBReh4`=smYCOCD}4x9zGC*)7#l;p!usej)!?-J|}rVEM`;;`=Hstjtj z{enMdi=q$=8#g+j^nxY6Zj;H8F8I_q(%slfC~GTBoY<%*_}^G67kFYz4GjY6<9i(z z=H^HD(h)Kr($l|%$Sb|AC@;qke(t`D0U1b3AxO+kPD(1&Vh>D_m{@}N$ znb(3oPXEBEmo_NUBS)*P=$0o-b$O?Jx2|nwZ@MlQ+r19o$HZ`PadlbLooHTOULvYZ z@=aTz+^f!~JQ5RQV=7xqc_DWckg!JEo2!!^Fv|QTB&A6KSI0bDT<1N!M!dYdc<07g z5y*z6X>6Q{uO~ehQU56M{=#A}7evF<@ zJtvd%r=p_z$Da z>R7MCMfHr3`Nc(ElO}d9uEVmt;9ovg!|^mReelFt3VwnQkNQy2Ol#}vT1{#|bxsPg z#c}@9U_V!?4j$i1r(dH^#f5nYc+fWWtlZ89`ueD5kGmOf`arvir4VF|JSj2TpC)J6 zBP{FgzB>uI7l&LI>VlT*+OghqyMFSwi#~)wm&pW{6C(BRBAGbCML|PD!{6UuTYCm9 z<-c`rR$J76oxIQNk9z#D7qp;%T~;nGF3wdSy8JK0un)8EcE5d#y0-$Y3&rZ{c>PHr zM}!Ow!=BE5@7sq<76C5W+S*gO&+mVw_h67kc(o)Y5#Eaf#IeI?Pa(>r5PL9NGJf~( z7My!-k!+bR(%Rv3_V+X2{m(gvrhLs`oUuu{6Mx5;gdg;q!CkF;3tz(2awRAr8^O0O z)^U2sGxDfpZ3JDdbB}kQ4N-mjoP&K$H0*Zd)QVX`Ba^Pck&btJbrN#(y$q#y%N2A0 zjDSZdRs-({UN2Uk9qk+goTH)f8T2;CsoWq?SS$1@^|Tni@AdJF*>tS$O|V>b~vS{mfdMht&Pw^T$$u>)~+U*+qrQ_%gtD}ZW(l1GXA-QDUex{g6b z2iU`E4erL*eQg7U9(M=b<_7*61!WQKdIjSD`zNwF@PY9dNn{B5cW8-@L!OwFq+@0l z2=E5-qkChiJHrWY21AtF2L799uGgp5)+HMG;H;qS+)iW)f*Ej-$IFmD4xWX_s!J=xWO*kRl7tdj`n%G{{7<@Iy$qz zk5-XSHt^40y137=9%m}@=%Ce#dH-Ctxl;-p86OY0J!%DG`)`0qQ{rrZ2$Gnc-3fY7 ziE8=~sc;Z@@yPpjm@ed}hdJ`f(|OHd3>xI5+(wC+nWAqtBnrqS!3gK&t zJr6J_nt4>bv-J{NGfQ~Utijci}`#sl>!^79FU(e0X22xo8T;(EU3TFC4 zcYgpE!Q92h#`enwR&L#w>xGg$;0iMAo<8?l4&|*)2fPCfX zBS16>0G@&#AcOWD)Z7gK98ywJz@$OXK7CQ3WfMMx!I9!U`KIR|>&wKOgPEe?O8u&G zvSO{`u+PX>;+764r>3O5e&~65HEWa|erfzRT_9VbhZVudfII~;)?Py-w7g7}y9y#p8D8wQ;jZW)$ z&{FOzljoiNp;M~OlX?rCE|5Ru?k~ohmer%C;~}D|g_T8lc9SPp*u-9I_+p7~+ZoENm;PM8Qf$bQY-s5eptzwQ-)Udc!^CAo+!^gisp;AI~Qsfc>S z5~7P2ZvEHpVMzO1x|B$p&U%8+#s$eJ{X)c9f843QexVN7~PTE zD7AB}?+r)4fW@mQx+z-oR<=}0Nvmu)jABwu{zRq3O24(~ zTtj@Vb$H5PHdiPg1BGv)jrOF7A>NhOpHn~PR++-I7b0$auqQO|iG;@wV@Cp_VUEJy zD(y(kH~uhX_1f&Y%4ikM=KhPfjSuu42CF&Chl;ABG|C#QtDONv6d8PVA_2-MAaLYq z6-==h!srWlBh&fhe644`G;F~M369iLLYEB%v9h(ag95{{5jj&c1u30lD71LHN)|0S z3$i6TqOO}SVi8RtNy*6{xaY{Ni#NS)c=0M}lv=Vnxj(u+ojY=7rY&)^QH>XGMl?*E zX}uaYrP=Fxsfl{&qQe*^hERI-=3zgEn{UBlv>jFJFyF9*BueY+OIfIHPYx2VjZ>0vojEU`P;+^Gum>IF@1dWoQn_D;q(V*)XzM;H6i_G^=UeoYJX zD!W>W^}8T{7NWz6i%26@vCei!sUN7=94PW^41C<*;6rrLiu?@61s6$xnBZ znYC^eFW~Xq{UBCrP2^KImR}9kj0lSi@`PboPd@br%7nY%^48n9(6RYi$U|LG+LGod z{yT=;!S1_D{kj*_`-dd&`U9JBK0VYK#}}uMqn;Ue+B2HUw(lK%OEWzFgXCT6T7GX0 zr!ULW2!U$6Q#PUKJb?mCdwg@jFgdLc`)>xnDs*1Jmj?g0;P80^D^IvEwy;yZN^YS| zHSLQeSJ|M=gGr*s%6wi$T}Bv~2_y5peoaSm3Ow7f)5Y@C+*wWUc8uqfs6MF1FLh*k z8nHO?D+*OO@ptq^Fa^JwW#pyJHzcufgzfF^@%8*|=y!W^Oppi^=S(c!Sgys&kW7uN2qmkjua#FR;d!^lk?(zaeMXzY%b-mIhV=fhFn%# zx=s<;h&pgk-BPvDu`#Drok27&*aw?g>t=KoY-LVvH;N)s&Ywox0t!4f#goOd)cdwm z_R%@{`MeLe{l8#kU&};NI^}FiXO9;e#XkmY>F-p?U8Bc04M+yy{74SEpl?_MQ1fUK z%RNvpY8FdVYXS0Fb{2xL^YECrLTQHGAX-{lNIova-6;iz^}TlZDhh*F4ksxNR(apA z2Ao`2mUjQ9pE1)7Ki#O1cRg-f5|gAy^>tWmgePt|^Hy4gbtTO^Lg;daS}9UVKoYa8 zH{E{#72`Il-(2_k`rLIh)TCwuG~Pr8Suafy zp)&Tc*2NX-PWkKg@+q?sk^Wb?lfgvy?n=w5k!S>Qn8ai#r zXe3I;6X%Wb+N@N@?wM!$GL#dM<(Wa@m=SJeeQG^F1pT`oU;QOcvO-M@#=QkVu||&A zGay7kmU}|(1*ar*O-!8l$pP(9RZ)Q;c|HItumz{*;KI4;ItAWrbW#X9=;`Y>=h;3* zuDYVMKbS|p7Q2ZgPJ)pFR2K+Q8hP@4HYQ)=9BMn9r*L|0jy9LVb91+@pDpgptM58g zAZ>n>%~}pS=q-hQ7U`3lITIv2fPa2Ftm~0{HOmZv@1>ba&_0I5l0Vs=uh2bQs-t`6 zG@maW9#1FL?Y1-Y_203NyI+9Yi>C_Y93vF?Ba`GV^=fzgmj1Bhjt|Uh7;#M~?vczJ zwS>LAq7HJRpMASFe*O9-9gc?bLS8-spX?E&+2lD;f&dGOr4ct^O~$2_@clkp0(haz z96~4@)H~^#x@AF`3EbOvZ|QzoOF{+kG)9S1H}^g?LbT0+y@=VD{^} zuKkW+K%Z`AUBDrBu-lQ0#I(gZvwsVISla4;Ru_o8u#G^tP zo)dMK#dzo1hOy%=(@X^Y-gDfYriB+Qng(zCvArL^#1}*BPr3SUra*%^Rg z5=B0yhv-kWza0l?UzP`SH*Bu}+cT24Ni4jvVtXt~Gm z*oB3VJgU%RkAPcSQrnwx#g07*}aSBHq;fsSEmxG}m#Nfc;m!HeX9q zsMuS_H6|F0sX$vBX%oPe+E=EORpoGW&hw0*8pN=fmq-R}tT7CXgtBemc?r^Jbj z$K3GOBFE9uDAMQDsMy6t^Fr?`qfJ2zPx(YqlBD~ovoC*i(Ql%o(bPx8yDZA)Di#z93<^;#SdkZ_p={R7lzm1%ZN%4-FJ`e# zT5RBfh5ClS+#P2Fts<*TXRu-oT+oy65^-uHAIbGY+-KmkRXR_UF+Y074YMve9-7?2 zHOgLBIrsgB^z%D4o4hob6{_{##RwwDL5ovhy6ypbWS*Fp3OpDHKcM(WRG^#G2#JeR z)me>vqVRO1kicwpr^?g#2&1~$P`7FA+EIoR+)a!oyUOkX#Y5ctCj`=E@wLMT5V(Mc zy+G^H<6Hoe!#D4aBkuqi8JY5g^z`(pkI!EBN8+AwxxHMWS2*zQIQ;R$Stx6Gr=qHg zh#L8bq@__Hpfaah_$wF9U?h0$Ph&| z*kIm@?2E9*#k$@W&|77|PsY2JII+8o| zhN3{MJ{bW8!BAIs4lHxp=l&R^*&5#fICu;JMT`j7b}&lF6}DvVWA5UzxjjDefeFy& z2=;sxFE@r*>kOdi^H_9d?7+)yXvygvl@JPQn5{+4azZ=U{ejecRpIVb7jlFu3s3BY zB(*t$Mscw=Q6`RAd4yj{Uo~Wi`<=FQWqHpT4_iQ^y z+^0qVQB%+$hU}o!s%!D)V|Um?L&Q;*hwo5k-r0ctHp5mVJFka5VdPP1Z6kUZT&<8$ zg4*bLem{OXgDC=6+swgI|0kXB8;iqdOUj1SZg!X)k)M3f%z)kS=g%MB^h3-0o267k zi_EMnE;GZbg#^i)1COcaY!hTCR=+NzZiyB@@G9vD}{Xr-aUMG+)Ylxc(+&<;(NkDFlH&hx_0jjVbE*Q_PZm z`T?_3IM6f!kf(Az+lrziWsCrj1XLt26N$XvKc*HrUH)VXc+%^aFX2Zmt2k)kpfKk# z@tU@%i4<3;=g0GlDM{{FiVcy|=`LBAxDGHC~$L%u~7JC4Bx-EgR2TDUw z&kf!iCnDa03SYC{4U7oREd?k#jQILY8E$PgKrLupXVJ1cka(J3)|4No%lEbhCI_gk zXa1sLAg1sD?(O_I+6$I78S*khC}X}uwzdi2jtBI@va|0xt@GBbB^wIKRYtws6C0;D z5%uanKEPhM?*jd4mGlQjQMlQt^gIQ_mzY=OWnD!YTCTZm`D+f|w#y2V4H?TLs6V)~ zXBh$AQv66sxxR@|*6k%rmu^tM_QM!R%N{WHEhOxeFe;I_x?*HjT(04_4%9l)-e z1a-c)(#g7N8fn9UVSn&KS$Pz>=HSHqSjeYZD(9HZnH4OJzc462%t&+!^4gJ&(PP>(iO_jcw8Xn6u9y_iDN*jWX(%j+u_Dt}<`NynNjOYa*mlry^kH!vd|p7jv43OXLy)g~4T<6cbEH+&1y4 zdX_b%wMDdn(x#W-j>!)iMC_o;!wRg)&w=m-RDVVYw;B8t5ErxVz(3W-jX*HJ1cz?% z`fd`*8wU=N6wXSc>y7rND`vEg)l1=B(2!l79iZ`m4=G!^=fO-18}_n_A~pjJd32$Q z;=MW#dkPhAig$qqprNUG^-s)YfHz3P!O4jeqW1BrvJund zeMoWI$lgoa>bH4H_$^o$17{5gZV6(n+UfNLd*3d2x)&{*rM<)vns(EE}`F@ z%6%BXuzQcmJw8`9M`QMOd!UV&{7=*(h5idGV`fkv7U!%i!|HfpjaTw>$$UXXOQ;yah+a%c_GKh9C>(iUg-c=z<6zJVe&{TfS*w7h zqahD<6PxIu^qzvvO`9x{Wz92#5DJUW#E5Hns7~!7eh_(M^df;^+d+x(J;#Zn1UWli zf1sROrX&Y>OIE2t1;)03Bxb8!>Tb)-dF_X&XYY6p=suQ2*qne}b;FojW5U9k)0F@=*yFVcJd z+5@|v=cAa#a9LH~eQ%mOmE|{BRT7JD)}MvIrAI^e>m;VW@!_41Fk??@chO6qI7zM7 zT2lFTQ8#$0fwc?l#ls%P3?UDs+9xrUrB)gm18g%7kO^@2@CYIkjTZ-TlDY5H0(S3I zBKXDPXwiDht;dc6bER)28Ra9`w;bjZJDlVyVWwtbE=GjVJv~H?s0&paQ1QV{#SP3i z(RH?Z*_7^^;{*p8j`jG@-+qrHy!gY^bI~axC&YNZ-En)x0X0OuKY7i3&?t5{b^e*z zIR3csjO?)+G~QAKzd*M01=Rq?{Z-Gsy@SL4?(QS@&ovfZpx$l(%tdO0Cd!^!aK zp`r*ViZtTh$oSJ8nK|zg@bbGs)B{+LNb&27V_r>F)x%H#<)*h>fx&tL`aEbG0J_J8 zyp@;EG1;?=wZ{{ZFeN0=q6{d=Hrm2nA5f^Kj7xILeUIQMNp_JV{XT9`*vyOzO{W zGV+Ev6$x?1UqmWUVhZO*?UOtbc=`gAEj`=PO~(h!kIzRh8(_z~(y6XYOXFBg>2{gm zG3@Q{-*nmn^Agneprs(P@$vEJvw}aRt)MUB92>XA7AVs9=?fl zL!(!pOfl{HB8FM0g1Ei<_pvt23H{`^y8s8H0m}XBv`D^2hpEh%Bx+;ZPnv4!{ZsbXkxr1u{>E+Q_*~`usZ*v- zOlu=?CN2sOyF8|Y@bY>#Gx%mmZ~-!_dqTyUe@)JA4mPaw;}9Hm5mZXBb?y{eAA)6ArIi%(>);Xobd|TNaN~ zyv2;`gr_yyWr9oJwtg`Y_x0%5gKFmI;FNU2b6ZPV7vaN9K5Y-;%D6}dFE=dOd&Qhu zSM3|vKfYV4auw@p*%vK0cCy#b5zl;e)T=x1nUtXaOKFG%B(55S2oq$ z{?p3x;-wTV9z+lEy;L%Zg1s5d;=;d6!_l(?n+7j(hZyCOKhu87xSrFmGIl*;F-&K# zAqj10@rS%Zy6XPfqm@@q(!tN9D(?oOUSf}FalSw7dPrn)h>+*h>G9JkrJ51+GmucqJR#!iQbgbWj@_|Pu zNeuylCkP98Z{>n$&0{K|Y97_^=mjRs_)$s89~qQ)vGp5@LA#8hCgA*`4z0&Eb|U}O z@*-;j0~p9lHKslwJ^-9=7S%LlJpec#FaQ9ta{h;lA%l!Sbo%{$4jlqY)dHY=RkiJ$ zKm?sBlm}^wXi%9LF38bejwQ;i{;myQCsAtoB*;87Xn#{;vkX;vrEIABA2i{NgyZaI zU`pkoYMbpF@g%u~1=L_uII>R?t=Rov-2~Jr0T8xmZx@RhWHhq`I1#A1Le9(lFQ@4( zyJ^BD4e%MxV;q{BmuQh- z(MPDjqp2LnK3l~N#F^}~*|4mQ9m%P{F+p?06c#N7o$|brlx5Atk-8-nm=AHJ(znqB zNfwhM@_Co-lJZBdHymu|14|IIbJ=2sVD&k&9#yWWmW_%Mt`>ip+1j`H; zoy_hmYl({6!0p*(Ww%CO#W)iC#InY%WRbS)#@PY~PS~gL5DT4-VjYpWm+!zh+;Xv;WM~TB5GieNzIZvt75?3RZr;k<)?NtW z1*>^GeS^ge;L}RSsIii^G|6Wf|FlTalm7rzLmi{nxya0_;B==msmTOtIg>Lk?EUn^ z?-pqjOmU}SE`TzpxYBBx4aKwY$_4~#kI>R3wxN7Pj+U@_#+qcD$V9Jc^^(g=ItJAWkC)Qu+ni=nl+TdUJL8OfTGhSiGwjolTO=LuS{IyqHY*5vv& zFyvX*!U_P}nEngV9?6N&Rb1x8sA)igvU*1tGcUi)+^SrI`l0@nujIg(1v-0s8Fx~ueh9WaA+Fls~EUEI;&4MOfH1D_G7n8Mge>GD(-U>cZ=X!HU2Hrbcv zTIE5=0nV6#nJAnvw?MCb>aaD+>AIza97U56bG0Jy4Dwar_JGit((COi5xX0PyRv+8 z%@qYpAYwX@wAb=kA(Pva9iuaT7CV0Oj=FTX>M3!Nnfo`d0q8VBv4Cuv)!vDCKHKK0{SI!TTp?kqj2nmuhU&+<{tAzFyt;Us^}GFk!4RXfQv-I8$c)q za`;d0TXWzLjq|iNnWM-OrdvyfTbq$vcxevQug`aQsV8>tg2jX@^N+*pU3r&c8^R2eg`%a!-aQ->nQ; z(%g#DI6mQjx98I{&i(+$8>>!qOx$PyGY+~VPF2hYq!{&R{zoi^h{@sz^>3%Kg2RXJ zr~}P>3*J>p8?st2Lpl9qfK5rpyjD%Cuu}Rp-T?-we>qdANb6?@cog+g6XG0nZxD=M zKuiHrECzxrtEs%q%l(1wj<6?uHciVnm^l%)s_=yld_alWT+)k1v}01U*VOSqu_w`~ zi*G5S*aE}9P^L)-pa6&@0;kMv^5Zjz7Kp!o{K&bM`0?Y)^76}O6ECrx@(92&K;N6c zA3L9tLdL|ArW)0-8xGM-=pfXw%OZ1-PykgNGsA;4V$ddfzyyMaet0&*otDTalxaQn zv40&OEnW5a*D0y$h<&$R+WIPakz1?jv1g=UE@yg9g(ElVV~A1BB1T?I=x~v%NGEuo z#&dHHzDn$?BkN6(lF9uhSgxZZJ4r-ytz^5A8P*h6jX@Pl6P9YJinh^?NkHgzJaT1bK{w6tQPJ(dx}Jr_J@VF`;@BKJnWj-&oo*U z%F^Y(!c3f{%`=uw6IBK|*3`X>QaXw6{&Li5l*M#Poz2)_|Hz*BXGC_q+CS}%t+M@| z=Luyx5;zN}VgJa$w`Dmdifk z&J5s~vK{D`ADFb&R#b%*d^daFiUX4iC|mVkoJmMX=mJlr-Q5KsAO^7mRx^_H+SnA% zeGfcpURr4{-p0^xF%(F0D_x6k$Wcmkl?Cp5ZnW%=NQq^Ow{}?vXDI$Es-+4yq|8e2 z1rZ9GA+z5=_f=S7Z>J^Qcbe{YkET-&aD#!sa1e-PgDKW*;sWFny~Rf&W-UTSS|ntCdh?){*vh0D0F7SpCZQg3v93f!^vmlmXc zMNbP|VVj8CGMi;IQ0ZD_6=Bl#ng|{ej{UH<_wgSOExyC$Xc!=1-wY0gfZT61sIYy%VX(rCs`Bb)h7( z%%lKJX;B{VOTpa2rc@C`>7z1-Oo4<@^2DF^1g)49&u~iJbXFyQV5erkqqdfo?u*2w z!otF;SC!?pV_Qr9GK-Nz@W5ddfbSpKpeF+7m6OIu5UXKIRfel=_gzm9XgZz1=|M~Z zReN3E(EOR-uU#N?+JMRB#!aPc_bLO+rj>|5jB_A2BAI;s5hR(Evsi-#--=>nnx91E zl^g|SxmOta__)oRk$ypq-4BDIKkZo*2MGKna;K#3*VtzMfSec5jc-AC3%R=}&vzR$ z0i7)()KyP_pFN+_We(bgJ2Ot`^S%^67#P3wV-Ecdx7X8JVBl z&|j_bZacz2BpTW(&IcTMunp|j`8jfv8`%9dnI&A;)YMd5%zPZd{E*2c^%7VQFSa>8 zstoj5d`wZ*k`N2QH!h?lt+_+YkWKg+kGd30CNkl zxo%)Pmz0!%j2#H;xcd8xL5Ru79)_O$^i_cXb@!}=2%u+j{OoAiWA5x_M<`;gKCh%t z08EapK{L)%8{Q0U?@d3?hnLppL36BF6l*&?I~%+rpr zccNZ$c;`T+CI`6O6`=7621J^xEO%QZo7?g(U~IkR(q{7M)NYC*fQ5qnCVuPfULY}_ z0n!$(pFkuVdACiDP;<#?>u&aC4j!drPHjwDylSOw0Xv12(4XdI?*wIv&{kIKqrWjE z)l%~L0*ue#XFQ6@F~1B_+Q1FCwak2Cix%v+!bIRi{IRLNgxi1T!-8Q^n&<+)S5O@M z$-pd%{EZPTg;aL_Hxyou>kWHpG^VJA7X#8n*$$_LvsHJ@M3 zt0Ut8>cW_Yi`B;GNK{p2wfk1mh}AY8!H9!g1JQ5;0h)KvU>pHqM@A%776U(0vR4jvuXyp1 zasr85;b5#M9x?M3tH!{YvY)+P1ol2Q78WU=#m^33a5?{euf73k`7=%`Ydfb5PV|K4 zoOcsSREZK3Y^@3WPQJ`;0Km8Y$9;_{>A|7&%=RBJ#%pDug3!KY+1#J9AhT6SY4JsVarA^>+Q(^*We^IpvGrM>lh2B`D$6B#k40fhmA_H`&^`sGuUrK+kb zlotgrM4*3lP~H%P0~#mO#ofljW_p3Gj^Hs5XW2?CKm)qC`yoT$6U4>EMRj#GShLiS zzm71lol1Zl2c@EzIHVpFi&WSC6%D!SIz~gYq4GYpa|QzhE?+m2ppbenERL(hDBTE+ zH)u-!*`3prI`R|)VUrQJwZ3rlCD0*3I?^#O2<%N7>Pabf-5T6s7JFSVffiJ#<{6J) zuv?+cDyA4_fTu$G!yIP_4Qnls)RQl!Q3Ks-7BSTfX5hRpd2l4N&bi%~bEh^}mm4a! zvWmz_=tQr)7$(wSlpvlbW#-ww%>bKPK1D`G0_8^}M*~7G;mbeXfQ)8o+D9^~n32xm zcR3^NC97Eh61-k5=2@z;o1NH-|1BGeZROlDKQ)W>!Ovu(c zBVo^e+~p+^F^w*ChbVP+^-38|fsfd}4iF}w^UWl2;jIcc_q9j^dJ0d)lgv#K9&&#N#5J(HNF3O3XNGBhKR*p`8wvf&zkCLB<1QfT zHNXKAhDSB7{BEfyB3S8^&nPn9P1iFa0T{UX%on=fFxp(95 zxW=i)JYQ}hA}}#A?`|F_X%e=>`bVUdPBWyWS1ojyN^LN8b#lZ&-Cy_i` zi=nJrk=1Idl~08@wgnjDr}x}ZLIL`P17q4^=4WXC`y4+L^3M(kG{g4C1n7b;K7ieSV2kZv6-9cO(-kQE&t4KjQ}NJ1 zyKDEa=;IdMk4^k(Eb^8Rnn{TUG8|?c(wGEb_<)Fq+&OmU3wAm8x~+k@3$mvD8s|eg z8G6R75+grIv>d{NGghN#@mq*0?DIdIffTXyvW6T~GDbOyo!55$lx?O3E3w0dItsYm zUbCjZ!R%|Er2<~OzIX1-sY8v8U<&%&P~(FSZ?le>YCA3}-Cj-%rl4*cc1<+DlW3xyQvpM%3d2!l6$heIGj2N(g8AKaI!A{$b z<94n)d#uvhTCm3)SY0h%(k4U25q13H^q{=0C6auEW${n+U@4m^45t(*!%@KZ+7Fo( zRv?xL7h$BvqmP&bg#IWHEi641|AQSxcwcXWGjKTmsuJN0y8$CS1Qar`15XONCerw2 zZIQ(Zr6EFte@YE_mKYux30bpYzbA-@fl(ES;>0riIg$CvSCVN}4ZE-;*gFxKF618J zC6Er2yeq+U0KKNe`-qd5SJLM=Tc+Uj>*iNU0t5cU*dk%urW>dE2hh!^33WVJX(w{U zp@8cPT?@k4ZFjA}PaxO;CS9Y$WMyq_E{N->s5D+ggRJaeiIsY_N%Ieo(GViXf$#$% z8*K5b&SCorx(+3}uU{fV^`lLo((owTSHhn^Q)=l$hIylD_nAtKNc>V#SD*ZWjI>(NoU+lrUTPK@ z931Qc+Xf=?z@~lg{RxWr9U}v?x+<_y6YRgf0|J@*BR0y=@mEz9&yRXkbwYRis@PmH z>nbBPo6gwY%XO#tbojp6?Dgd@5;w`m+lboS66p|jEz50tuPYP=cPmT&mZdD_PR;xg z*YR^@7*Wo;MMAg-YOpQO@L55uxNo;{hr7rNgLL2D*-E`*dzT{Nv5mOd{VdRIk4FZ9 z^38MRwh{pvyAEZ;d)p853O*wi^m{QS4}x>QWxwSc(dy4QOp=dVea@?yc=iVYj6Xf4 zi&_0F+t{+dM?ZD=T$_2D&bKZKq*z_&dp1uw>Vl!WiAOQdJM@tz z3aroNDbkuV{eDLlj=l1ZxWghQP-%@a-3q-g>-puHA^{rApH!MqpLk`@(T>5N*OxsD zZ)IiqMOysK<1*Ei(RVTd{WHaNnCF5Ow)NVUTU&T+*$VuZz&`9*A!*8DZ>}3_G-YdB zjs=EZ&7`+m1dE#NsU&X?X|bUrCC>A3+#wvMAIo4@B*Ve@Bcg8<*1*C59x}j`7!W{2 zkCs1yZHIUUT_~YQk_QYGW#vg7Rtu1x2W1fbYiHnXcmKj0)GK_6Q-9vm2sa5|KdpCS zmXVmWpG|%F>KVfW(O!(BHoMUJ((V#az1i)m&TykR{gIy(;0K|#+a8jyT|B3aSax6( z4dpD~rNtv>9T*TE5g9&rh)j-IuilmJL%-{rd>f1T=@Z8YJnCDGHf6dX{!=VM;bXK7 zyzGOXitQJ)7o29)?2JOko1$d>F_eZLYW2e&ehhGQGBxy&lALc6ID9ua&D8vps-Mp{ z_oXS^o;Urnh$o4zR+qq<4B&NR^r`ckmICnaw8dvE3fNih4l-r9|N28r7w zsoBNKHAKUucurmap_P8KCUimKiOIBAML4wEyJeY%5hz%h)g^nV-9dst^#k}s>-rBs zjbb^x^En`ecYmIDPhIc@y=CR=-!DX4lUb^5pDf#$Iy1lh$?CZbwD=i!x_|%oN0(zj zG0T=;a4(wAogXhE%kuB&?cYB3SjTF2{bk0kKYRfa;|Yp4Z*uEi>-}3LdDZ53w|F8! z#TXnJ5oh-8nRaKsG}Rh9_*Q(Zt*R#$;p^~p>NKRRYcBZR1*&LQ=dXLS4{Klxb2U(i z6uUHk#?1RlGo_PF3I=-CeX|^943b_QgMW32MO@QlJwL=^dUoel;4`+*&iSW)YCV9H zDd@J2_EM6zPY~oisfW)b^3FeW3=meF4gdDPqc{`1a7CWsSn18TGUwlc)MtVczzwe| z@|EtF^f`&UrZ}$KjtnG?IQyBf_RYjU68sK=4^**_ss&Ms3xRZnjz^6jywMX9^0hmN zycv=wMDbqUeUyd4V|`;?!S8zk zoAUaWbaixqWcm~kUMeUYrOitH?|04r_Vs|20@$KT%42c?k`v~HjS9GV39NB%14nrJ zKukkOl134s&J|_CR;+{B>M-Qi+C@k7ouL@zHk0V%Ke<}isvmk9h!wn>LZT1wdl10x zms*1=4|n%n_di?xQB{?d;P=RY9mWCR_m_Om66TZKqXN2H$o=it|I^od$5Z|Pf859H z*fV=OvdhTcdnSa)CS;wEos=2b>mZSt?3oc|uZUFk$V|2rqIEq_-_P&*@4Ec)x%t%T zyw7=$*XubRkNb0dIP>rB?fZAP%1WI_vN&TciU^SkYnYubu`nKp9_=5yV!vY)3Ea+vSjcV{jy1+!hWhBeU&`<>8 zGwf3BEtlm5#_k*t)k2~JRpa7%*N=yhY(9E_>==inQ~n)Id9SvA8kLZMtFUH9>m#MC z7yctCGLHUI=+VUWD;LuC9UyJmc$;$qrP?>^hkbGM!|R8T6!zCWp-wC5&p>JBQKfD< z>5w(0$c#wwJvb+F4jqzA=<`B*5WbTyQS)(p40mO9wPem)oS&2k4t{ku=lHO%tGYz4u^C1wa;r0!>W0T~ z>+9)>GmrxQr~0o(;;LD-Rd#0PIRr=o&CGzRF;EtdtR;orkoXDx(}HMO-j=#{A!sW3 zW+jFCQ-r-HhGr6PMdxx~s#j50)Vux^kJZF$JSr%6lQI*)R8h>=G!aDptf&XG`i^?>TZv%k!1M4jxad8%&*e@wkK+O zZBF(eN7R9O6>F(#0o9_8Ygek5U;`EZva?#67q?W4-Fq&!voNUl6Rphg=7pYZ!qK2w zwO&@5hl{{CLDKySq{)`(Rsuxz>$9!Lj~_3+xKbAcRQe-!*BL6FukH;OpBV>ATk=Uhbi5Y9iRg78#jp6WHEF3|b84$^0ELvFWC7ilE{_2KO{nY=`V z;o#fkUW^xa03+pQ)ER1Cgrj+YS4uQ;-dkW#Kw*|+ns<2;O(mzFxcV6m+1&Aa7XT$W zvVP_JXYd39%V;cDj(%W;GyMLbaM00tq%9!|Yh2C8faWiS!8JlUo+4&pNUg95uko%o zBbAmp@)wFZE$C7!#lltI-(3O8VQ4kr(q1V2&e=v zYs@07-LkAM#kJUX96=3hZEX#k$Q{qRvkZvRj}jdRzq<75yD_A;_}Df#Jqvfg5FZ<-T!zXyHZcTj2Cyl?{{k!}6WAvhJ2 zRhG?wOM2?wj^9}UExIeTvLh7sPnN#o#PZib7t+U0?XVtInnQKtaFTlu32m{zyUm|~ zHI*TF9g5zH0M`H*!c$;KKo4WZO3?&q-TO~qj9nX*_u`Yuzo{|s2GS*dAHf#IlI1F0 zH(t)IP`e{UU^xYjlnJPV&CD)<`rE~2puzlW*iu7zS)?T;_M>1E1v>T0$ea|Y?TwH6 zHCqNgadmYCUtl{5?()1gl_hPfe>HCq+^RD0LchP@r#<2hU83-^SrWq?*C}yTxOP@n zzW}^-vj0g5Odj0aMX*^xw0?HF@pRQz+9L*<^bQ_`BAG={$d)fKfrEx`bLI_F)o}`f z|5?=;&-j~qhL^iSSmFU-ihx3P((&i0227B^XMk$k)YMdsd0ipo+Yo=F)3{bt_B8oY z9mSyx2qj(tNVc@k%WwtJ8QiLU8)N1J8Pnv45p2U?G)ojxQ%5LRK$#y9{(_uASSKI^tR_ATNiV zxd4{Du9K3|(yKX=kUn~P3tYF-{%4nLztYz0mSXpr3d)zrNb+x3i9J?wa*>Tvwx)a1 zpTkiz5PTxQWqJ704~#nC+hGn(S<$*iOgGw1H4aML!o^79pT;H5MhE@p2nj^6EqPtC zw2Iw#=su^%GO7(TPqMNYwkCUfd;9w}e{uDlZ(mG+$IfF&<)*!@(CV6i1T^05sQ>3L zHDs4A;Eh)ONtfyR8L7Z_m4QG4tc zlqMg!$1U-MO&HD@!ZZy3EtF4IlE4^hq`NK33fH{^S~UFuI{)ieztH!Y!`G6s)XC?b z|DX43u9(jgx3xlZ`ks0#nGZCF*nc=)5W3{9it!SBMvYcMuHvnruv!rR2+&F6Uy6H- zp45FT(+wyOFRQx?XGsw{=LWXl#nZfUi zV}3CrQ_nLGRX3N2@0e^7F5TAP|K*n)m4q)+ofAd$pN*6}(iu*7e8m6PIO%52FVdmQ zC{ZW)^vqPxazOg4pRm?+35qT9w1uyB!rV}EnGYhF3p3yL#kSro?6bO|^YhNzuTh4= z)SXH;52ryxL!(v;%BG=oubu_d_vDSN4iW|+$8ZZNkg(cIOrH0(2Y(J>|IkPoC|kJ$ zt`oJ!a(=O)yQ|*E!uMpk z84%uP&kk*q5j*Cs=zG+CPJ^J%nI%vC@PGY#uoV8Z4cf>Js0Q;L?0970)wBiK8t~>y ze!nL%r4uF?CJxe$9le3Qqg1C2psjF?D?Z&z&Ph08%SkG?M-2U5OlgWn=0{8qW>PRs z#6v1*hAsAe<~+6x`Ne@K3Eecn8oYZ;6N!K|D->d0au(^Owt65&eO(nW*zXTq05>}P3wxIBEX*j)u3WF@A#3lU zI$UV8A{`JV*Ai;o&EBGmZ(E3;8kT0M6MA>Ed`6sX5hs>2;jVn;!&OJ|yb|ZQ?N^M4 z$^EZvCU?nCYEDN&jyhyZnC6hG`ai)Vy?}Uu(N4!_^Fx z>??GyFC552>z`Ghv_Ja}#!I5B4@a5g)5WEu;9LXVVVe;AO=7UdKJhN42~^}-?J5?` zQWanIie#B>d`3_rn4Q}1)o(cwZsMU6F_ps7L=Xh|{-58m9bZ?1Zzl@Gd-tyrozz5i zJx@75CDSb=(KjF7h>%RGT--u(FVo#2(M=HrVSyX&*zgmBQtllV;d~{cBw%gbCMoQl z_?IsRIDf))RF-v*eK3y`hyT$y-1UMWfe_w{eQesg}bMZdp6h>y08kIC=d zH`7tCehW;d=rS4^p);jA1iue)NnDgai0gkbAJklU?F%nc##d)s{N!h!-4zaZ-sSi+ zMyCxsiMEe<;F-utn^j}KQ^VIM_^-REsxKU{AkFJNb$!{Cuv#*VYa{~l}IT|?-B5tQ{ z&Fu|0&gjxh5OEJz%{3T^q@tJCeS&_Zp8L+bOmrtTGO0zjySrOe@s{mvFaEnnTN&x; zCMG5y;ryfxnV3x$Yg(C^Ve<0WKL735DzEtSYePxPm>Nh+X+w`$3c@Zm2lf28j#+1n zrnsZVAj{A~v(bGIiLHDG9i8zo)yolw zAv*4zZztY<{>Jd`vRk-*KVGCefjxZnb^k`X;*I0$jxRXU66IVnI$Q4FUn%ON`}kNl zZ^0PEyg`I^AA<#Q>#KF%H*+8c-ydvPdw|I*=|H-NA*FA!UzKKxfwM=%gHmf%$EU17W{JUsTj}jClLBNgq8jub9 z(adP5K>h|p8zj5@r`Kt6wH^qhzr_1Ci0iPf4b2d|+IVqD(ywvfqb^GIO!d;Er>JQr zF2a=6auSzt-DcCG)%{I$g)am4ac*Z!=tv{I7O+pHAVs97rM-3eF(g|&;CgDw%gPD_ zB{8t0CYQb1!NADR|Nb_Sfg$3Yop*#HhEkIxS+1%3xc;s{iicy8$GhfNCwhIg3nu43 zsGSRS=m&8?dFs4mnU)J)D2c2w=slKf1f6H&xJ~$I0Yb;F7!r8!^fJ@is^-N~T z`-!!=peTs0%Z>5qyEMX0a$LkpdK22iNxRTSdPlEir8#Y$`iSG=YMPfG@Irhgv`LHT zLi^>Okblb_(EmBBHEl-8uj4JuTGro|ziT@mWr<{N%Z*GGbgkETSRf63mz0ZVSXwOf}FLlPY_JTkd;T`if zc}6Qaw=L|N4xCc3Qg8%t^?&zUFnOD6S!bMI9^2x8rZbbYdv%6&}L@=t+C z`SKYT@yse7h+H3jIp2wUnTto(vD)sPB3Gn0S5g?h1A81+cMGQ@(c8G0nDT~czIC3i zHmf~NU>pMxII@Muv!~B=eWOvYBbkXK{d2JrkQl5^djw3obNrFVRMnwSl3cOw88$z}JyrhNwo3;Fu*b7r7@bB-|E<_tbxH-7X4X3q- zylMGjG0pTQC}30M3=+t;kU1Q3l4^FfWTxH1rW&=8%=MXlVg^3p)McwCAQG+2B5_{h zk_3@v_>Q^@?)3y{evp*~=v@bWl2JF=)NrcINNEZQ$SRe&Conh9x4c8mI{;GU!r(%( z)*csOL4y}0-ehGEHdlN>fdIF4H?0ll<*V3XL#=82)*@A(p4sVDJZf2T>|iNR6!O>augUlnst<6Xsc13$s?mi zU4i${Z^R9L!nYJ0W|iE8&$-)?i*Zwe%Kdhpyj+31QBc|w)KXm&c?xNLxSTUr)9wK1 zwCPo>dpj*VX;Jf(%`_=80;n6^WG6eeVzzwckvWNw+o|li83IQwkm`n01V{o3lcBD+ z7ky8aon*0=$g(?ON#ZYQBpQU7L!2At?gE>7<)yg|qf1SDu))~WNJBe>y=*GeJ( z?<0*f7a*_r_QOMI)b+PHU~k;Wbc+2`Kwf}zY7i=IK%ZJ$S&f;B4;je=uG@wC z$h51|tpZahRHwH|!Iw0l>yFVYUnvSVb8@Z7S4EQCz$Y#+qGk(wfTN>*VgIJHSX%bo z{r#l4)nxrtovhrUp`9_?Z1DieoI7JOKjk|H*3VHyP^07BQV_$sZ(B}yIQ2lIFt+ELJ2UlGJaGEd|u*UB+R$`bGM(;_<_ zUJt+cQPYL!K^uBCHv!3gQ{MMVT>MvqH}2~3T238;=sdkn>*k}^w==m@d2@AIGrCF^U^>Z9f zvD-^VL8-A;(5Dy9$khVnK(crP2e_DTQ5WwBKhJBByQW2rW*z7290cw4%R#CfYP3QW zypza1O5(D?H`Hit=G>VDoUZTWI`aud9w5O-(?CYCsAJWsWJ>*kyBo}B!oZa5jwHf1 z$Gpw}Hwxeq$)a&-fOtd5pnL}AX%9(D2ZxoK`@h{g*iXzH{dq+hgzGP0c;?MNa^ZY; zrHF>#+Aqbr5;gz1gQG!Y5eUsnsB4Zx_6>wdd!PdVyH?{UVf>JIr1~vF zrzI+UK`^dPE3#3AKi&I+O9Y(rVBPx`+(JO3937o>dx3zqH6XLszdE?OZh{p>Na@bE zw^CR@#yAQtzZ$6bE1K==OrK^D^zH zCivC;nEUQ4-KRv#6vwe{%tI*W;xx3yon~agk zL&tV^ed_C*Znt@jB?j^5*XjJ)RQJB}{rYpv(?O5oJD>SD*g`?h{c3o;;Fygq*%oev zuEpZ?V&F~pcY&)C{`e{jv$J$Kt2d5LUp8HKieXyY33Rv|b8$8;@yk(gRACM$yDmp% zSs9jJKg$E$@Qb0qGM|3`p8Ci1Kh5>}_s^f5W$MbxVmE>!x+rJyJ6HPRz8xNRe@$MM z2C9sx;KQxX<*$q@E8!jQHc=(UF>&Drc7p$>1^5u{*h&(uZTzD76>F85NkvJE^q=4a zUhPDZ-30f`g&exx-q!0O(l>AP(qzucm6?&;5T>W6pDYeX>E!y^66W-|1>EH|6$smy zuxHv2SGiDN<8fQ7;9&dc%-z=K_FxP>Q}J=X%&3b9L-o&ls*k%qjaXl!p1?)?kR9%~ z!&fn^mrKIhhzS8LL~e@#fU# z<&-iN)_B`0m0UWT2N&nk7Q?8KyjQU}@in&JC^@;G00Lo4dbkm!lM3$1yx=W8~{#FKMX|ybbc}*MVc8q)k6;QiBr6XIzU#XNil0Eu3_hNfl$uvcTmN+L*uw$<^ zZB$vI_7itut>*N{kG{|ZgI11g?kK=g0e#Mf&`e;;$jyb`0%6V6&o_cKe6){1cZA#N zn51wFq+sAmKpF85bjX6gk89N}s3{l}UoY_Vb#CsXc#*_m?1^9W)0#7VgDU<}Je$y_ zez!1}?}cSLsw}#lzE|Ii?$7HoL@Apa8>i;vv;lZ9K@&(d%;pn!f%pvUoXp3M&)^`T z03|y#*~$%AJO$zaxJ@N^c{g~KPKRnjj|4qV7|v&cJGVzce$sGqcX{x0fB$(EqWmuEb5~soK5{k$G)<{j?gV`*CWyZx6)2yA>K> z_G{n3A(ASRU1?%m+;{tMC*ziQzrO;Da?ZX(>-`6>k7PCIPnl@p6n9(PDO0t+)3qyZ zKatNiZku@ZJNxwRXGeMqDB%{bs(WZzNP^{!$HszeIWW&X!_H8Jb<`ZQaE(VH9M}G? zc9{$cY!2GL@x)=5XlT^#dZ104L8f2lGHl&9JJDUuuoB^u_Gl-;<6 zP|I}0{yg(vIzwSAPWRfTqD9X29L$sSEQGxIcKmOL_AU}?Gw!KGGwriSs1ir++6Sqz zR*)z)PES(72NN8F_1~u`K8QltSt(><9cBY(Lmj6(R0gTH&$jyTTn3D0;;?bAY|2Radc*Zs;9D^%qSdeN0zKkz|$ zFq>^ZTjGe_DEFG|N%b?_Cr~m-*I>OaI_rY#JZztougrkk&Q)Qm+Ro%&)UYc@B*n5? zxyR|?+aXvx-_Yg0fD1+XemwY(NU#NUEsImnLtJ|#1DsX8mgULIjV~CVolw)&g3(DcsDIZ5{@ANBF+QKJhIijt5i9^yjF{d--hA_Z`6W^Kk}y)i$} zd3=`9Y4M9(&wUBoY4O)Dal?ijq$+T_Rp!2oe9>hVX`?xJX6G5*Wh4zdZzO77wHr+# zr_Vd-)<6%tlj7-M!+bku@3`}q#`qRA{ zHmP{#r&*~|BQ3QVJQt=jyc~V2m~N>vG>FW;)ynW{aMW!P4Ix9lfX^$=uT{9lp)*hn zH|!mO-^;kf#E}K|@Rg?pUY0D#E+h`+QNUF2Z^)tjEJVYp3)ON`>~SVIr5~(a$3kv-BMsa>l+` zNkKPiQJ1+WK`cM1+X7aLyu|)($tXO6?sxg4cC~E>J!q+?3q<h$C~u+a-J4q}Xp$t3muEYY-{C94*VGo?sfEw`L#k5M|@a zNz@EquJ&3M8Th3_{fCtA~T<@(1ipuU5YoF8RWB z+X{S%u2DCwdp#dQ?7NH+w!|Vdi^j2a88%s8zGOvcSwZt1%BYAeF{ei?$ehQI(Li_` z8PTtpco}kf7;%om*~R4~;pud0M4O1?L~O8TMcw?{fwF$W=e{b5j6KH_=+`Wr;hbUZP;Wf#|21aFSbS{(Dj-;O+MEI~5cG)3MC$Pc~=rTfY@|-b zWV#&lAbtTmjBM~VsMGe<0B=4+mj?k+*anmlUx7{p8a3)hCVbW8mKnMwD4oJ-*8&eT z`Zd(Sz2L=h9Fcs);T*{DcbE|zd03yYI`y%L5Q7k3@`p>h2 zjnYc5!jBwrOpouqF?&ksY(&(ckfJm0lYus%RM#<%L28tAN0|;@Yc$;4^Io7xyGa&M z&Tlo=b7>KgZz51cg9I6nD5RzSu?sJ`v{VpTFsG=?PWsMgePd%|H@EVh<>U|G8la)3 z4$Rvh>Go`1z+`69Cq4}4IFSETrMKy=n)~#Ft8|p=(kXHu*~t9MFWjsOldj9Z(ofU) z+Ih)Tzz8>XgIh+Pu`-OQ=4#P;hOSE0=Rn2E8Na7vQ+;tH$*!1kdE7B~T4I$Vqi9!o zeZrh#w;GT6nsL4$#nj=$i1?Md;Mk1}mW>Z?6867r?>e`43)838)*GSIdxEGDVk=Qh z?4=VvEWDAYX$Lw8o4d?AogZ0u(n(d}{d=KB}#cE0gTC{(D} zi%pZ7Rw?FbiRQVclq=wlePfWgRckT7pB!jIV?ajSpg_c)u6(P`TnMvxJ4^}e!BEfE z)FedcXNx(3%IWa(CN$q6m}e2a)T5k7!|tXoHd?%uPk0TMcrXU_e=)+zVN0Cgnjx=5 z7w$}^oiN_uW&v;Jmmmv*b3+ih;K(_XZDIH-sUVYGTVDqav`vc_r<7CxFrupS~{Yo)fyce+t<6ko-nGGVD9p{x-p#CrCFpX zhrx-c`38kHb>!5mJsgNK2OkcM7gmx#Iw-8nYg@r+qBt~XZW0z%wy%;FB@4=EzlVL8 z&boD+4_`HH=rSlH0AM}HzWzW81|n^3Z2{PJpsx=)?ZP654Kp6FI{j_B{b}!OAUUfn z)?jl~QUM{dza3*{z&6<-NUdR2uBkiUdx{S3LgLedW zAmFZ{i5FPLg!3;?3L7CKfi?{KslYZEFmnhn?}s6Z!os`RE_q!ig80vi{My(OCp+_{ zX;*!1jkOQtrz(`=K%#%Q9RK%6eg`ty1td33a{Qf)i?pm48qh-Bp&_?u~-A zVf;%%k1UtgXQp68Rf_2^(cx}tWjs*5eEh;7>C`^PfYQ-p6Lnx|NK`h3gb zDU^eRj)}u&dR99tjqfN!q1XR~wpY~0>{n||6sY+^T!QZE+QWA{5lzUaRWFGl#PF%1 z91dg`YU1o_iDZ7XzK@zWtdD}4?t^t#3J)Ej_E5Je2n2`;&hg7OCXC8o4#X*qwKMM~ zvn|UWLMtANac=b~YtJ~0hig4BYYY`A67ScCp!eunTQsh{)Y5`g`4Wf4fax;B)p2h> zHcBsYlG?U{#FwYJ3UITYfERJgOI>KR-WA>#9Dj)b-klxy*!LruNWE zV%B;>qRP0KprN`OBFNhLw_3SMwFR0Mc$f_qWq}({N8YH#Q3$@NZ+jS{8Tp(3+1(ZG zT&E0N>c%BynoRFN6qSq6@Lmk1(wh~=fhPeIoJmgJXE#}tA0?`}eNPL#LJIBCJU7+yVa{$N7&)j9+No$w9cK6 z(jRY6QSG}9>1=Nx88!;9hNy_hP?X7OH@I_0xBjIZlbg2~5P7CKGs$qhxmv1~_S%X9;aoPYzV|oFnP^S$0@w>uaYAkv0o%AcH{A$d6Snlelex2(X1my z_~TWqzk9X;@8e)pS8)D3qLx|2Qh5G-w}u8f9j?&yk@S=r0`cHVk_+<=p~)Ck!klPB zl=axxM};Z`n(cggj8N9f^Q_&>I%4?{NDoJjR%dHWX`eb~nyzxCtxyf$Zp9gsbZF+c zA@laowDA8~_FRLYHJFQOAD1|Cp%YZDel?Zy&&L-Luc zKCwLG+!AcevD2~iDv;pe^hMrp`Ks~t=9sWew-FBraODRoROp1i925%ey&=f+jMiGV zRt~GZ3q?m_a&MuW!qxO!t=;I%G>FyPuJ9HW9pDRBwMJex;O_oI+;)kr#_yU>ayX;Y z`9LGg+YCuN8MAQ*OD*Y2;sYqhRv7p9WTHP<;&v32A&4 za~@v>CbI?ds#x45?`hFGA)uQoFgVH8kbXr5})cr?=6nXnwpc~ zBJVk^N>j}Bq$x7f(~C<=EQIJh;BleU_zD8^<^_MtM%LK#EaX}M2nIb~;jVV27if-h z-U5??(o@R;*N7I|nEbih{O~ly#>U3X%xr3m^ocSO0r9u1yL(&{J-!O1`h9z7^J*5= zZMzVf{5yP1o~nB!_HXpn!jd6kigD$mq=x?&B_mLU2#V`jZ0k?t@6f*hmES_c5GGV}5-{(S{ie_*ZpK#LloT=mB95SVud^0vmtKA`SX z=BRxByan3ja6@axGkl$POoEAE@V#(4!McMb?gDKeJLnSRaZXN75MC(!JSjF;rPXL# zczyQ-eYy%^GB}=?tuqPb%_?5x=PRcc!;nb0r8MSpaR%W`oTFfy7yS+0_#Yq(Tv{1@R6#-EK#GbAI3=%vN!vL9r)nMms)tw#xq+S@jYeHn zm9VJjS3qm~oWb?Oj^X6+2#}JBmam@lM+Ud_{PRGzuT-Yeq(2VF<%h zxd1d6>c@}0Ig;U4f-H~y&%r||ONK)72qOdjc9UEhB$)G%93F!Z6k@RFFJ%xxFkk|1 z7P=olVCEPIbASB)gVln9u>TC4fa~XlxQwM)5|vXVnG$qM>R-N`UH4%mC46y1xYSaJ z4mJ@xv22u%o2RD)_`!fvi#MaIPWN=|1jZnscz8UgYgG(%u3o(lR)aM_?asle;-}P% z1jjDcV|e%pvV2&Vf|jG`*B0~UcApg{s@vP!Lo)|@PPqJ_?rr=v0M`qC0A8~gXU(im z591?YDD7WpGzcKWXFT8;s;a6g(T;y+#hFvg2u zq=4pJSy(jRqmEzem$tXJ0STv;ph=^Rodc$)M}lZs*gM*uy81*q642zLbbO>)fSG}F{s{SgcJ?d@_y*%z>}16-WhEms6Yl{^2Zn-p z%jQBVp7{7aUOF5ZTjB-@F5&>B=?Xym5y$`^Aa+*X-OZ9r#1gD=GvLU8_Yd|sdwFss zqN$jmQ;1cd?~oilI?RZeY-i-~;?{tfsXQyUvL&#UK< zK;R_3xdS6aLG)@Vv#{(n4~vR_+k%H=ja94IO9OHdaB+cVkk5z~k01OZFhc@=eZ~WbyX;bED<`LtC+0RbiZJ;yKAyB;rXB3UHoxwO z2LDutuRVV2%H>yg8iD+y*5CF|xU zKy(M245<3rz5_JDT!=1jJGX2}<8YBMOT>snep!D_swco@r!C*(6-QGXZ zJ7xMRd z$$``@5tG)yq}Jc4;+z-)tN>K3sA*{Mr0NK6-MV#f8UhbP?y2GR>-6mG>_V|@zcOH! z15*M_P-RsL{RNea3y^L8m|XX1wW5rE17*-;+Qr&3eJK-mO~ zAVDbHrkTmfh9)LhNq5Br3=o2RaC~~2A#e$vQ}ANf-l)2e=K3a`A66@J|9y~VL4!9D zh79TGjBonFQXYW+Es~><1UC9F*TlrcJS68n{W)%i$GHZ^0H~5Frt1!s%5Vl5`Bq$ zf`S!AGca)&YXl~r$0;@u0HpX`II|!f=oH7!%*DIE0U+SdN+2_b`;qs`W7;47aQv>T zFn}1=r`LbZwt;pYYpzMD{`Ubqbn0O(EiDjZZNO3n&|5|Soh*p9VAj#6{c{gstAf4r zEr|nIn(5+7o;@Q(z>^H~JJe>|eK%9zI+GXxTna(Sf)u9HTe_W~2^-}F9clz9CmY+A zuKcOe4-P(qS+V8DWx%8Y(;cACzipO2wYh(RQe7zwd-!ayIyr&N7eb}OEbO4>JcYK` zcyLeQBN}F`tgTt!CwAFE$U$ZRdocnwy_ZLvu#&Ca#+&z>vW957thzcBmP$bVkA)sL ztgNiC{m~*Se5#6F4rn_*T}t{Dp5ka35+aYqg_q^$=fiyKwJ|s`jbO9zH`4tK6Gesj z_+UatSUOC_aEA2{=EiSNfV=@pWpEmC`2kz6F)#p5Nw}_XCtpCS!d}_zOcYK8aj^PR zV+#u`Jsx|jL)27RUJkaykFb_wkvqWO8Ei;_p+w;yHnI2+;JwVFPr&e^2nh8~wow3Q zOzxk+P?l|o-~sp4-W~#M|HHix=LOB6WPIsH<8!cvl$X1m97@g^B^-ssrs7(~M8}W5OK$G&cw^Y{s9hU0zOoCjJ>h<0llHQMNmOCCACj}qe*B2K z4^^w%JSY@?=nM3MtP$;c&6;&lXor%W3&dUx!Z)fB2*j|Cx{3*cGH?k_Uf72OwP@0r zmwtjF&FK+<8{GdgssEpy1)IqfC~N=A`TkNo|BRjx2=y?7uZ7UjFi@{n IwG01$0Gr57UH||9 literal 0 HcmV?d00001 diff --git a/Quaternions.jl b/Quaternions.jl index e5e685a..c311dcd 100644 --- a/Quaternions.jl +++ b/Quaternions.jl @@ -1,15 +1,14 @@ - - -export Quaternion - struct Quaternion i::Float64 j::Float64 k::Float64 r::Float64 + # TODO: Determine an acceptable tolerance. ≈ default is too strict. Quaternion(i, j, k, r) = - norm([i, j, k, r]) ≈ 1 ? new(i, j, k, r) : error("Magnitude not equal to 1.") + abs(norm([i, j, k, r]) - 1) < 0.1 ? new(i, j, k, r) : + error("Magnitude not equal to 1: " * string(norm([i, j, k, r]))) + Quaternion(q) = Quaternion(q[1], q[2], q[3], q[4]) Quaternion() = new(0, 0, 0, 1) @@ -52,3 +51,42 @@ Base.isapprox(a::Quaternion, b::Quaternion) = isapprox(collect(a), collect(b)) LinearAlgebra.norm(q::Quaternion) = norm(collect(q)) LinearAlgebra.normalize(q::Quaternion) = collect(q) / norm(q) + +# I dont think we need an actual type for this it might just be easier to keep it as a Vector or something. +# struct EulerAngles +# roll::Float64 +# pitch::Float64 +# yaw::Float64 + +# EulerAngles(r, p, y) = new(r, p, y) +# EulerAngles() = new(0.0, 0.0, 0.0) +# end + + +function q_to_Euler(q::Quaternion) + # https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_code_2 + + # roll (x-axis rotation) + sinr_cosp = 2 * (q.r * q.i + q.j * q.k) + cosr_cosp = 1 - 2 * (q.i * q.i + q.j * q.j) + roll = atan(sinr_cosp, cosr_cosp) + + # pitch (y-axis rotation) + sinp = 2 * (q.r * q.j - q.k * q.i) + if (abs(sinp) >= 1) + pitch = copysign(π / 2, sinp) # use 90 degrees if out of range + else + pitch = asin(sinp) + end + + # yaw (z-axis rotation) + siny_cosp = 2 * (q.r * q.k + q.i * q.j) + cosy_cosp = 1 - 2 * (q.j * q.j + q.k * q.k) + yaw = atan(siny_cosp, cosy_cosp) + + + # return EulerAngles(roll, pitch, yaw) + return (roll = rad2deg(roll), pitch = rad2deg(pitch), yaw = rad2deg(yaw)) + # return (roll = roll, pitch = pitch, yaw = yaw) +end + diff --git a/README.md b/README.md index cb0d868..81aabde 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,3 @@ # SADC.Jl +![](DancingT.png) \ No newline at end of file diff --git a/SADC.jl b/SADC.jl index adce15e..340420e 100644 --- a/SADC.jl +++ b/SADC.jl @@ -1,19 +1,24 @@ using LinearAlgebra +using NumericalIntegration + + include("Quaternions.jl") +dt = 0.005 +time = 0.0:dt:100 + I = [3 0 0; 0 4 0; 0 0 2] T = [0; 0; 0] -dt = 0.005 -ω₀ = [2.001; 0.001; 0.001] -q₀ = [0; 0; 0; 1] +ω = [2.001; 0.001; 0.001] +q = Quaternion([0; 0; 0; 1]) + +ω_last = [0; 0; 0; ω] +ω_temp = similar(ω_last) integrate_ω(T, I, ω) = inv(I) * (T - cross(ω, I * ω)) - -integrate_ω(T, I, ω₀) - function q_step(β) mag = β .^ 2 |> sum |> sqrt @@ -21,3 +26,60 @@ function q_step(β) end +function integrate_vector(v, v_last) + v_new = similar(v) + for i = 1:length(v) + v_new[i] = integrate([0, dt], [v_last[i], v[i]]) + end + return v_new +end + +yaw = [] +pitch = [] +roll = [] +for t in time + + + ω′ = integrate_ω(T, I, ω) + ω_new = integrate_vector([ω; ω′], ω_last) + ω = ω_new[4:6] + + β = ω_new[1:3] .- ω + + q = q * q_step(β) + + y, p, r = q_to_Euler(q) + push!(yaw, y) + push!(pitch, p) + push!(roll, r) + +end + + + +# Plot +# using Plots +# let +# plot(time, yaw, label = "Yaw") +# plot!(time, pitch, label = "Pitch") +# plot!(time, roll, label = "Roll") +# title!("Dancing T Handle") +# xlabel!("Seconds") +# ylabel!("Degrees") +# savefig("DancingT.png") +# end + +# Write to CSV +# using CSV +# using DataFrames + +# ts = 100 +# df = DataFrame( +# time = time[1:ts:end], +# yaw = yaw[1:ts:end], +# roll = roll[1:ts:end], +# pitch = pitch[1:ts:end], +# ) + +# CSV.write("ypr.csv", df, header = false) +# df