From 37b2e073312975389d5cd64b2ea7ad8bf5a436a0 Mon Sep 17 00:00:00 2001 From: Sravan Balaji Date: Sat, 11 Apr 2020 13:24:21 -0400 Subject: [PATCH 1/2] State & Output Comments - Add comments describing states and outputs - Add whitespace - Add SensorModelSim.slx to experiment with modelling sensor --- src/PlantModel.m | 38 +++++++++++++++++++++++++++++++++----- src/SensorModelSim.slx | Bin 0 -> 23918 bytes 2 files changed, 33 insertions(+), 5 deletions(-) create mode 100644 src/SensorModelSim.slx diff --git a/src/PlantModel.m b/src/PlantModel.m index 09e7c6c..da84ed2 100644 --- a/src/PlantModel.m +++ b/src/PlantModel.m @@ -1,19 +1,47 @@ +% Clear workspace +clear all; +close all; +clc; + % Parameters source: https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf -g=9.81; -m = .468; +g = 9.81; +m = 0.468; Ix = 4.856*10^-3; Iy = 4.856*10^-3; Iz = 8.801*10^-3; -% State Space Source: https://arxiv.org/ftp/arxiv/papers/1908/1908.07401.pdf -% X' = Ax+Bu -% y = Cx+Du +% States: +% X1: Position along x axis - x +% X2: Position along y axis - y +% X3: Position along z axis (height) - z +% X4: Velocity along x axis - x' +% X5: Velocity along y axis - y' +% X6: Velocity along z axis - z' +% X7: Roll angle - phi +% X8: Pitch angle - theta +% X9: Yaw angle - psi +% X10: Roll rate - phi' +% X11: Pitch rate - theta' +% X12: Yaw rate - psi' % Inputs: % U1: Total Upward Force on the quad rotor along z-axis % U2: Pitch Torque (about x-axis) % U3: Roll Torque (about y-axis) % U4: Yaw Torque (about z-axis) + +% Outputs: +% Y1: Position along x axis - x +% Y2: Position along y axis - y +% Y3: Position along z axis (height) - z +% Y4: Velocity along x axis - x' +% Y5: Velocity along y axis - y' +% Y6: Velocity along z axis - z' + +% State Space Source: https://arxiv.org/ftp/arxiv/papers/1908/1908.07401.pdf +% X' = Ax + Bu +% y = Cx + Du + A = [0 0 0 1 0 0 0 0 0 0 0 0;... 0 0 0 0 1 0 0 0 0 0 0 0;... 0 0 0 0 0 1 0 0 0 0 0 0;... diff --git a/src/SensorModelSim.slx b/src/SensorModelSim.slx new file mode 100644 index 0000000000000000000000000000000000000000..7b7668dc80e638af39d0db1c07905475fd74eea3 GIT binary patch literal 23918 zcmaI7Q

wx2~DCZQHhO+xpTrD_v>ZcBO6GS!vt0vup1j-DCgzjB#$(#kyMYz7rA8 zT%jZj3Wf#*1Ox?yY!#*Cz@wUX1Plb!4-Eu_`tPl-sDr&Lz~0qR&C3zsqR-%IXFHtg zV6(x5G}0S6zkxit5ydQ#t&)fVQv&NUYJi~5vqAD6M#ecU@HHcSV*W7IFfzzU&hOEC zf^atXxi4LHi0m7`e0XS=nE(dYDw0Vo2`64Xy| zSR6a=SIl;c!v`deLS2e9TdQ5|8lPuIddX3`Us$WB8sIruA! zhf<~0+kz%hkALsE->9<`6nTo9oI8rDI8Nfz-1OO&_$mK6Q%M$cJAW4wDED^~bb9Ny zPA{}oe24OatqQ#wqebUPa$eoE0gPvgjXnQV_NXc$Ip2o?E8$K02%Va!S&+Dd?q0{= zlAobp6(?Ao0)dsSKS>);_pYG&V{Hlo!5|pBBgxOMjFQ5&ZhyBJeR)g%z=ZREDneh8 zt$8*HVr){zs+b&QR4EtPGtHz$4>Gma^5fP{+NPIP12rdh$@hI;+or3^lfx&*2ZKKv zYDo%Efp)WTVQ(Z~e2su1Xe6qS{HF4n=Q<$4@$%#RtD8g)= zJTkkjy;X9|cG}x$wz~~x+ApEIr6k4ym(4C3l!#R?BCZXIUjD)Np38~xQ%ZGM#|-r& z{B2)??2a2dO1t&bham{0^dC`?+A%q1^)D(cz=42Z|3!tNGr-n`k>S6dCCQTtgG@-` z{^a*PPAEtqi%nTUi{L0h_rQP6GPNQI8}!$|`4Pf75L?epGxTP=B|T8$%5)7Ncgo?D zNXVVA5`e@hTqq)wed!G($WYfoRDGfuDld$#Ch6^@BA3 z=#-1?J?0E-0G-ZdDj_a5q-wL9Dp~GAG7P?Um=_lII)77!F?>R0g3@rRXi`j(m3^F% zMHUfgP|iXoOV^tWQ?9pbNh2yt_QIU?ge}THvRbL>bTlJyu2|kEh}vOiq6%r&)ZP(R z_ywfJP2yc(PiCg&d*m*AQ!-p%RcAO%;5t4R`gwZij^-Y}9$#K}D21)>o7GLW9581d z!eypai_dS8t{A}!GS@ya_jIzpZsG;?M|}rj?ERGcHw4F5UO z4gv&(`_B~>$=SgX;OuGzaQUx%jZvAjA7DcIOc{F_au*8Kk&a)F zM3qvPUaI6=(H0T2m24qCyvc$rMsqry&dtcdujSOqHao-bUlpmsnhzjCOrFy&xX0=) zll70~yJ(y6tUwi%wnYPwp0`U`i$u=8m!wx~tW|sHxbs z9}ewO;bmIbMt_+q=UX#TTXMr+OfpzGE_-$9UVl`4f?bM{0m3;R#6MhTi&(@?M(+7t z=wd?w*Q9ma48Mx>Ip8aoYq&rzBZ7jGC+Ei^dzP&{G4@D1!1E(&od3> z<|Sjl)$z4v2fU`5WD6(gyfoTjJmPpC{Y8pjLY>3RVv%&=iW%I~8)!OTtt}F4wc`%P z^V}6Yt}AtV;4y2MZ|v&dLjMN*pT5*8v>S~4^F#8VH~9b47dwyt(-}bAq&)~T+`rtJ z=s`6SuaaA>FRG;a0!q@7V~ao?-edMx#Go@jT}Yk1>-K&L7v8L^Q(T;MYfS_hr#LxV znC$)gZAuHSNd!rZ{Ol}}YGUEqwV`%fRvca8Z8pz0)Qx97Z75TZ&jjLueD0_SIGiah zXIVvNDCekplA!mpYLZt=IJcUxBs$rpUrd$R8k^?3JYe_4=}gCis6JlQt7jw_4?V*r z$=tytL-@%b&aw9g$&SN9!kVrJ|KhW0-fVfR?(zcpPXUPedNsQLEgbBh02Kc>0mME3 z^%WN@2mAkuQT^CX=RqPgaR1PP6JlA3f#5@$3VpIOhu!rBtpJEyQh&m=itP8xHSNaU zj|@HLBkJF|8hR>0{Sus^s)~Yw+uD-S3`lrW%tng-OsWlp(Y{n0=g{qC zrqq)P;kJ^G@$F$O+a^2~_C=VmtE|~dwq{G=D?c0EMxJ7GOYnf-H?_Ux!Vjsk221Ki z9V6i@^Lh+PxH{#+w4KHif#%a1gc&uh_`yX<8m%fzFwvfcRlA07x3K>yzz>qV)X=|$ zbNmy4>i;Hy3cwa%>;jOsH+T3i3EX0t5rdeJLOp&Bj~nRFkdT$wBTz7ixw4^GBIT!J z#0c(aEViX@I=*y!+{9KB_(6J9$g`GDA%gA)s~st^pC`Tz+sB~z5A$<~LOmzJ;-& zN*$><4(D%ProNEe(cU5G=~O*LMQrk#)}xpD5Thg%K`--X5ScQinNHW`;>{O%IGIAZ z%j^}F|M$<=H`sqFk?4PxKl!g+G{6G^Vf;@eTrJ(~Oze%VY#AKwEsoOO1e32YM@I>8 zR8uV*qSeZ&WRbN8Z`l&iP)y-ttS)6!Ge(!S8ptoO%)Dh%>5R<`&E?2{n@vX3ul**o z!rFMbzIpw2+&T&S@b0d0@tZmEoAIOEZVCww%@-j;g0eNXcZLnW`Kb~8>}y>3mM2C6 z`5pG_~yEc0%1d{%Z>+dQl%V~RK#1z0LKFuQIB z@BCovbt)uu#2=%JH4;rD7+KH`CJps>rzIJK?RAxxmXY41^rz1IngVrsC3B-X*f}K) z`Z)o1KW0_5cUDPMI=m5ny~pvVOT+AD0`LGA1#d0o=HzVUWSg+4I%n5w=29iMafM6B z;vcUpF4EaiK28(LT(w)+PUR$LU+S3NjE_;%$mkjv2t1NekDz@P{*f!^)CCl^r$_W7 zdaJ4cTss%5eu~oo1u)k!kudl|zzmpba!x@tr?)&JIgeuf@MtaJ)IVJG2yZ+dLbHpy*m z9?#6ogfS&`*g{d))MO;kkzu3>Do(d*J871vv-p`5hH2+R^$$4GP7CPlr2P{jmJSP+ z!pK-e;;~|7%v*K1>i4^$p~27Q)jS%{9v}z5ct@<70Gq7VaFkxz>FQT_qFp zZ&tFD#U=fsA~>g6DX}+Vt*xzTM~Bt8Z#qQ(y2qE8eqi?Y>uB8D#h96gPNkgb>ialh zwJljmnMEpJXg3By4Jkdz?bF3d;NISA5Xf16e!dBd*CHka9uPDP>mJIYf@b^5ZUgg9 zYg@zpN7ZOiJKSp;3G`n}Ruu~(eFtNgKUGzea|$zp6=VftabhIxGQn?9b0G?8q>zrZ zU?8Oh%uPt(^30}a4HsS{nS(&;Xpf$uhm0kPQ%tbGqz;~2(ci$qo+(y>T=f`HpVtlk zh5<;-4jOQIcw>gh$+!6VJTv4d5_7`1K;u*8(CwIq?ETYhNAr%()9)^BmGUAm;`tBp z6)9}#<`|Q&_zRTeunJU_u^Y@y18R#A1O*g+3lV|rw1&aUN)^)%66l#ba=EF61{yn* zuS1Es52IcVvoo9N->{GLH2o@80{}{Bg_BBB>>nnaOMg?)Zo+PKqco6^laaZqgbDVx zZ`M>6mmX7PAzMEE61zHI8ax^ld9`OyvH}1;^Isew6)`c>Uk3UZdUa#`nrF*tKmw`A zMBx@tIzF4)s0XrK>0u z0e7ZuNN)H!#fo`I&nYdLX$)?Kj7d7W06dzwoN+WYW#xn*k(lYJSM{9CeJr@q-rqW> zlp4Hh_I|RgZC)1|a;ix)#!PL0MYq`LgIj{|96UWbmg$0O)ht>uZBODfs zB9z&MkkV)ga#WvQ+@kqYg!hq!Dw69FJK^Ri%%75r8$1IWz+_M<=P3k8{!`Njd&up? z;O7>2yqLEXsfE7<`~vIl8{yBN+B8jgm|)>=*IJPl3iW2E8I0cQRpZ)u90CP3K`ifa zivZ5LJB$47xo3db?r(KYBRe1eYha@e`DStAaR|sxT+`_SOb`&zFYaE#03Io;_c^h* zZ)hL{#J9rH!`X6g7q9`aJ85&tCP9AQ4<{?DTo*rg@Auw+`xD1k&KZiRNh@ufM&G`v z*)S0PI{nrAf{nqu^}i=xP}Elf#}C=eqSc)M0AM(Hd(*D{4`1zo{VwW(pU9akVuuLsBdrY{miqK{MRourvnHrR2T+Y#@Yy@pSE=OXA+pyj0|pC$X_Xh z68VsOhFx6X@j3$8L%9RvZpC6Hey>LLZy6Opia7)=F2|t!lM|W5zZIMVgO(DOmnD@2 z1h`#-(Kjr$b#*rw`dqKB+I|`OSx#Uh4!?USOTO0Xcecp2_S?A?+kUtgkJ(0(klF{F z*dji!IurW%RNrjz-Yr^)szh79y_m5T-BB6g6-AuHw^cNIb?+W++}OEYqE*AlJ92Hp ze$Jn30ZV1{M(Rb*^k z!QX@-Rs@rI+O(kJ%)6WOd1pYcsiNumC5R<@Vfs46DLQGun6KVbg6ny3-yQ27g zf5aix1~d$Rf6f?w{m}JWY=VQG)8g=m9K>;YdNxl@85lH;m6Mb6^MsHXA?%%*LK$5tGDGbovwN1Cuy0;uFQ4irECMjz`g*W`J0_yhpRu$4PGS~zj+g8Dx#6LjOHpccS>hfX@K<;2_?%8G^);hVgEk|Jy(pN8iN z`seO!%5%U#Dcvx(k+dA@L~@xvfC$=Y@QuBUz}F?-XTayH5!dO+M=smprdR)&*NZG~g{^J=Q^vuaVgy|}!rVlw+p|JDw<+;)$6 zH;|~i%l?)&SngI??1E+3cfRsY3!$Z;PoTck2NcGqghN9u0{*GpPFsyy9Ae%KS1lLs z=krQetJm(_Y>P{&7mk9yx3@Qspo8bx@SBf5d9W4Z1K`=_!hWc;R6bB5i*j*sv5iQ8 zwdV*yitgbB<@fn|eVT&)xM~fx3v=TQnSa11E&I`#Bt?dhe=dx5rYa=l?dO7!wFeqa zR6{|xU*Dw~!@mmqHEo`Bem*p5N85SsC-K$dzg4D0^%ux|#N9nHU_MGx8(WM7%Lwn4 z3*`6TDYR=M0;co?|b!QPA2jF}t6dQ2EGdnQ#WB z!=&EqYdAziMpZdsOlZID?d>r4Cd?1%RV;{+Nfzdjr9z?IwrIE%dq<`EpFv3s+iEqL zI?nkiKBCBM*Wirsd)z$dzzoM9!mxDE8dPO(bZ6kFu0BPqv4O>f!DpcclRZKUEM1d>1wC1>o%SZ#P&aJ<7cHBSD&wi?! zrp=RUvTaBNB(L%={q=zwLMK%8Tm4>fQo z<_R>%4BIx!WNJ4$XntuWy}4Up-eoumq2vjzWvzESdo0*75MNx9k|j|@j8KDRq=8C;npZknm#( zK;9E0iw-Zc4r6Ln740_w`l)w`p39KnPE`RvZ3h-3fia^Y3W&xQvrBX=LP!o75|}$P z*w1j9DA`9Ns!|nN&;t%lfhnevfF6-3dL*-wsw!^O|J{8Xc4RfxyKoT4h8?+RMx|Dg zf#u`H1Eg9~YK{D=S6tvKif(xLa3M_#ZG}=5#1^6i24!AQ;W`meESC;Onxn+N$4Zu} zt{#SQ#^VX(VUjL!?ycL3UFwRASgZP?)6u%2NM&vUS)}|E`&K=4(O3``gxN z*bJY4aD&Z?&z!%J*U;6cqqpk8@dYM`y9`4ZAp}lcmaLg5)L0A(k4LUlO^N)M=qEP_dL2;=Irk|T2D#Fho0~cZYM0sJFd79YypMK??Ft%^P}6Xm^i}iOUnN| z=c?o_YK{F1bpL&5a&i@ta)f%!cSxiVhCz?!;I0caV1+ntx>&4wq zb8IKkxg4lM1buc3hK9fpsGLsO%K}F3?#&-(f3L$ZG~)n!UyE)@8`ftBKySG<#p+(o zI`1BWJpqfo6AT#g^Y+)x6S}&yMfEp!ZN9s-ewruK3x=&%$D|=KdKZb1JRs&ieSB{A=tehvuLKto(|PLZbbHZm8pUVvf{1D1xUux&<~BUv%2L4 zB1}f`GF7g~mlNJsext9G_X6wV7au5 zI>-GlX57{~eQcbc%(l=vT(LZEJCfigSg?HiIUyZF0O=BR(%e#Q1py;0V0g-)aEUBv zT1?cagbHh0u`oG)E$W>CgCB=ZDJ=cCS9n_&Ar`dT{%8`$sYA&i*eJ(J&>-n3(0wu{ zc;cB!Y}sSpNi~19-gF+Oy}{x`g2acOL}X%s(LyKY3$c*2h@~SC&}Ckl@JP*z&6i3D zBk`EPUeK;OU}LYE8+W1v*vKLQg@LiA``$4;?<6HaBFy_Ew_?;Rg*(K(%fB6>PIlvf z4Esz+S0(X)F3acRYV&uvB|kG|bCr@nM%*}Q`9MetpryoVNGkzyY`B#5EiG65IZ

<*Mx+*BW1?KNJL7H2%=Ep=|sz)!v4cvD_| znVfkrw1k;|s$1>FUY_|XtFqdkzJ4~H{&aqv9>blw`64_5d!hfZ3jlv*1%I}VTnP%# zNMJ>R+=v4T(Jzg0T zR9WgtQOC6Gq^Sa=MPWz|4f9i2m~@7!)=7}>7MICdu<<$Yr5gGoGaKhT~?253n8 zu2>Fzo%1A(Qo@tcXW5V>k6F?3HL1d*=~iBLq1wL!b~q*>R43D4umW24)FbIl3Wsr*%+B}#@gIZ-NFHz z+MVvB?hcch-0V~*hPosj_C^#ksFtC;>gSppHsKaFVll?@YV*6!^r6^mQUa(A_fs4; zKb#G}Hz~S8TVOER8d&w5)Q7$T8I}Vor4vF{zJ0$G|FfLsw%p>>C--g4dJyiHb(a2@3{WUfpKqJcdNm?AKE7cB$IWmK> z@1Z@4KaQ`mRqZM%>1zWu7yU>deBuZgGi9@B9!v{(iuTiT81`gF2nV|7)Ux=rPGM00so4hzJCP z`9GI!VrB*~H+Hji{qJeYw5F~D2`AdG9Yf}O(Y#EdN{w@HEvGdqDOn+Gj)V>!b@St? z-TnHq7D}OCesNcUKnLUQC6(3$W2@i3*PJigH!uYJ!3%Pc0`2YBiBMuBsucGE6>{!R ztX_hiE4WS>wiMCFkr=3BmcuawT~UPwT`LeXEsie8TYo{`&4gu@T!w|PtmA>P9cff@ zcHL_mp#m{gSfjGX(Z&KaBwB#dK2U&Dj*Z~TG3F@yqbWcs64_5F%*VNk5x9zIEI_y|UVy3qx9C^5J75GzTKW{tHa0;t{(( zUXJ!kXU3JveRI0U--rbh?hRbz?v{(9qwlte`oJjUIU6kW(sa82NIxn?P;+oc z`}w>BRTORr6a?M#pC8t=WslH`z--o|f7T^=Yx7M#$#g}gh;$+>SCrJNFqWf(`l0xF zmUcO}R!dFFEG``E6FA0(pSe`a9qL6SE>1$`pXuHDb${;Kmws2;I79NtQokjp*w1rk zhB#-$%`eM1_l9*oc^j7H)}ERt*k`t-1xO#v;yW2EI_BV zom&0|nHD-wSmYcv3C|abnI8SBbfFE`P!q)wnWbelT@4cOx8g17@ws<*eTxHH=MK(0B-o_FCb}P7hGrdR)K!WobEnj$8$~(Q(z|AI# z%_H9IMvIfnh;$C|OZw$RVuxu>wFinDlUm=U!l3WB7N@OjylPj|++9&y9gK%8>6(4? zlg{dRtLh}0RjIXyN|{AuWxS)*L9Sd%5}FhD(;iT@yzBAZAl_l0s3~k+2J8{^nu*hL zIm*=t`G?Ck!Dq2S(0(ni={)X+XJTHKx{7k@!XAoC&uW|&RCZigIYCbVR1wg3KPpB} zd&P!}Q?v?GTZrh((R#whK{{h_uKn_v@n${v&Z;DW$a-@W{Oa(Z)xK?Jf3x7<6S>`{&lKI)O3d3>gymJx0d+?mQfS;Nq_T`?X-|;G7bl-# zc<$VZPqZChruEZrmBL(I|950NmYj(y^tr$AKC<*k_X(L#L+{q9vP{>RqHw)y*pGTs z?!32oSsPDZTg6`BKAM!~J7)p9xXb|m| zkBFW74gQ(LAQyMf<{nRxVGD*aS8Pjl&8;P;Z3_Q8ndfIl;y4i0X7U(xB?#)B76*E!p!iE!CH>BEw7YsIv zR3#R4t!phmp9zLDg%(u6DDClCdyOblfa=+RtHUSy@lYZdb(g8gs3kr32|_1f{jdnV z@LjR=2tjM=A;P43jqA~%j{?~C2~Tgf!JFEz8rQwx(lRKCi#sD%1w1|yv^XaHa6m~F zE;TKaNUlT&e@uh+R+)KjrGRU^Xpbh3n5(BdLLu-7kZ1OfR~+wl=oEOOF)$ zNxV)Za}(9BxXtdOEAri`%!uwH{xQe%RaqIo7xz6wD_%=-(;pAHAZwCXvw#RPuoQ&o zA!XqeGX`H{8H-V#sQ@D2X}0Ic8BRllMjM+!t?dXwPxYX!jek!aj6}!yQ(1pdd_kxn z@`0b6DbtG6qNB#JAE^5o8M$5NK{Qr!zbuj5QXNkvDfdhCDwOJ>~ z6NGY|2#6G-s2q^WoZlwqo-cX#nSXT*E!#0mtR#o~Rsuf*CDV#widPM$U+60V)Kd!w z!2x(L5%OFiP+HuC!G=0X#%|_e4onIL*yU<-YLiuE_`zEwW-?lOI0O)WG&uS#9 zM{=cO#iKFlv>D+3hXm2@A<=VVtIMUPJV`-`&t;EBs&|A&`y%%UC#Els&skda_%zvdDc~1a$iuH6%|Czv+-SJ$KH7ZP&#M2ZA!@iJmcN z_fxbxaEudc95O=9P*cFY9CNac(K^=F{#)uvQOpU{=VOsp2~DLZIbxF*t=N@}2e_et zvr`^UM;o0ES!9w+6vE8QWpDZyFtJ2Z^L$GybZ4TxY%#dnAn;=vx;)@C{X~?m%KoMI zu$E>Nx@3%_8D$z%Ro?4%RB3ZEvN;*}(U3KZmBIvPgiEVDF1ZKrSb)L=ziPfBT|;nk zN^G7!h{3ODzEa1m#N$mQiQ&APw(3N)UV05C`%^T!n0f`HWM)YlW}-@1)lOJFX~u%* zTPecvVBw6|{iBpWJ?>POt~@szc|&PobL&o(S8Pum8XiCUA*D4*6a>(qi)3F(F;<#GF8ub{P2efCn;9_M_meI=`e+XlE6|~NH7RvV;nP!D{5>9(m2dVm|0D< zFdTgqyXTG$msf4FP`7r@au(x{$DXy8@Ye7- zs>60hk)yVz<9y>I}X}hT1lX29>aS6hG)}a>!J~YqKG7mx(BxYfUYz6 zNs9&PFCaA=qOJPe6~x_-27ga((D&>=r*;2YI0V*k^NE(Vt1O*mbmM&3P?tVpGJYA< zzH^!E8v&N)NVGzXfvj{64ftztRxz`B@3ADUbbh5-I0b{+?^86-guvc7&$W2|2X%4)?CBROw+vF>!eu+ELuY~W2T{Vuq9yWu8i~U)| zfeQxqFO7$I<-lOSO@r1pQ9C$NSh^Wjg=;}qgkaQVB*E~wI?(*$&!Bg>vZBz2!5W*& z{7Z!Eo_D5=0ZW{%1~|q*$j`Xjd{zwNS?BG`tyx(QE1V;lb?qy)@!Sj9MI@Lq&|xzy ze(sf31JS7T-Kf(W<>@B$(b4gNU07f@>HNyY(&(R+ z(e;NXWH7OtT^k_tkbq_xAoEjXqbHI+ogJJyT8J2-kd?ue!AZC~>$LtV<^fvT_S zJMXY-PRFmer`^gh$1@3FgQrrEKM>GGMMzcXM5Vax@#H4n3D2hf_|yo5MOcL%3vR~^ zGCqc3K#2t=Q*`>7CAci0E&Uutrf3;U(u8S8fI8=KIILQNAWk++d!`q-yHTV3Z6rym z5JNOLa{?H0BFkm3bQLzO*T-#m$@jj>bQ1AYoOqk+T z_AvaASCD;gkPQKlzeQoT6$?%fg|8j9?yrhqE*H!#$;bMNffdc4n;vlt%&4SzLC0gJP_~l_Ywj` zC|#$|d5|`A42(&S2`o%ku9dfK$)z`3rj zFWWW>Grh6F`FtVf;32O&Lq%IM@3d+7{WG&3}wA%dLpqW4 zQ3F_hd|r%T? zAHpO)ZExT1t}|SLRuc}JN~?g#H8%AtJvHVFMe+wtnv5c`MnBR&_A^s>DP~FDQmzVB+iC_AFRegnAI5JEE+2Xn-GW<>!j9*Fydw z@McFvIWQMhjgPzyBQb~&^mh*N72wO?Yl}Kh?#*9&K4+qZn%=*^+bLhp<)be}b|KV0Q~)6wts zLBkQm)2dDt>Uz6exWuz~UfMxwi9wtruvz8mb4L0l)0!vXtaFxYt`}M93aK3*d^{HM zCDNW6Tq#%Qm7_QU-UHowm{XIN&iHG*PV*~PcVM%e(7LmA`yEf&Cc`_jZ6oS-{}eRd>hy}-T6r`l7Iuu-cZ>40lfdll1wdRx@`MZ(3^iT_g* z>7JuQXUuz>2V&XHz1k<5p4|THdMz0_OlD(|&MN6Hf&<-fYKyhvE`y?3Ce!R!p>(K+ z#S7g;8-o6Np}Ta{d0b2REqVitQOSOqfsMZk5NnJ|;4l>w9r?bDv>!{Y0;E)0czN8A zfaxIQDsD+%8g=WlNRm*7cG|S*x!`sd=c$>#lEy^p2DL801Cza?svgo!ZzbvA3Nc~8 zaKbGwl+fgdN$X|pe^1}cG*LN^aDag7L;n}f`v3VI(|@tpY5fbA1CGR>9{*4ocDLrN z%(Rmf>8&B%2HWXVg$DIPK9`hgdQ3SH6w^c{Ar;f3+TLpjsRWd}_{P>W#)#2tLV_5H z-Z>z_uPNzmr0Yiebb+t6aP0hv^5q3@Gh<&+1i)0 z<8Ms@jP6?Y%d#{J#@Y97+`+18rKV7&qbWPXGmGC;aCz>?Db0 zy$5y~-`|6;Jxcz}FSEs;ku<@VoyRY8jBqe4PP}D#et>fgbj$a4cD6;5aKBl7K*h_f zVXv=UAu_JGvw1Q0<_T=V1st&G&6LRB_I0qn( zlV!ib|GOt^H_fp=mCrDWtf3iHCG(+E?WANkne0+fDn}jPl2SIw+ z92lh;7ixx^z@%rwf9tVTyaMo^J!1bc2)EU9lt!}MvH+B(<|IP~)xj|4tdl;_XvLh)z;je0{JA|1l|O zi__*f8rBhG>JVA?5UMlB6#%t5aGtdBu7-h-Hi=ha7*>kpt3__*&VyQ&&S!%)$`HP_ z-Xzk7#>Tv8^8ZE8f>968kZYVoLHmnQ8&4r*Rn1m!jD_*o7d^}U4+)-yZ2iAZ?;M9- zVi!2V&EiqFM#daBqRo`ct?7bbON&sgM@KSj6pfV^^r4WC0DAsULc>#Ntyq+ad2P)&YB8OepvWEHThA*~O~ zQAK>GuyDV1``yUjLILDj&4{AT)BvucZk&NxgIyb>%TakJYzIbnE+nl!km3+LOGGZN zS76HvEDahE2RPe0%uDO`=s~Q5qQw15>?FEw@69pM$1+Vas~>{6QxfF8!E8xDxxQ4e z$udi%E&=$K9MrchnyD_Gw7X^;R5poEWs{-dIS3qt{R#F;@Qf$Qn+Qx|8wJ7Nf>#oFfe#oT-EMA^?X5FsTz+(^!hk17k9l{O0aO;=3{Wymp5vi7jKWF0YdM443 zFoSZgDb?B-_oqRjh@^9*v97?dD?)eN$S-i5Olf$1EyA=awFUXKN!5{Vsx>jh=1nEW zpo*19JvKIb5=ZSQ&DQqM)eCATso%6Xmw2FR6(L1nm~ZyR)04Mso0n6AO8yRqvjSMo z>N)1c{s?kl$0>;~X3c!tuM)Ku4suQ!!*jAd)XQiOL{E}Qq(}q%MO{38W9?FjBS3H! z;`eNUZ@Yb~sgSKzS=LA<8o`-rgPju-Y7Vr`xhrtg-e@&0Rx{Ap(Uo2;7_!z;ockup zXm3PO3^z%>#C7PCpHmorK`P-1DyBG@#)v2FoDAX9Ssd$YkPAiu=_6zYL78S5q|`8% zP=p|J9$zliS3a`T3j~f<02q`PZ-|%mP^!jo=}CLgA7pq$&*p|RVI2U)A#_{u3(1M4 zABkb;l}x7k5LE|Nu-^1=gDHc|>K%7F{H;zuls!r~nLTF$jK4ewuR)>)ReZgdF1_y5 ziJos>gJGy`F~2@4v4p~FDkW0F-VYp3iL?PZR)4!W^qB{zxz=&0l(bwl$mq4#eS0)1 zLHlv#!g2m@EF{yO*H94_^UU4B>ReoI~2Ya*@^eMV8siQE6Q*Px;^+&S2ekjTKjoX_XJl=!mhk81}vBR0yTRar@O%e0Weg0tVx^^*$ocH zx}#8cYYF2E*?B--@b?Mk_k$33Jyp!n;3>JLLb-*p5xsInfqsXZgh>m~!GKq-L z0E;^xZSowsz`8Tygs#PtL-(AQdctr%J-w@pMV5TfpzJAkr2yWoiOr+w+K zE24x7RhL`T!GT0Q7K!%|( zxrnEw;^dOi-KHdK|00;5Thf*@r~*=pTb)hObjt8Rt1 z2AR_MK!7>kMeLp?V5!4Bwf_^SP4F#I#fZl0XRhtx4x&d-^L<2ZS*h5C(H44#MAYig zUuh3cy8Xm?Iy{oIqo0!)Y(Xb+LehvC9rg~A2l>hkOSV=s8e3b}XcR{M0cu=`F2i$V{_DujOp zrUYT!IMs$OGB0tmgOBRXskUiz^l=}hg=Z<+6?9X$(YDaf{k!ES)tOSNriHU`VkvJ8 zJ1NnM4Moc>SVyL*E5}E?*!&%E5Qp0^?KuMBlXK4uW6j>OUPas**a1HuAKOT*bpawi zU-e-lye;h3QKiU9JrYi9&JEE(3E>j4y}(W@Gmhfu8VH*!2Q&}2k}h3A`h`7s8HvdS zav+D793+l~pWXNKbqbk`sGL#r-nUT&e-s{=6V@$G|!;Kis3TOG0kS~fTwP&~w#Au@v;*LOh+ zvT&W>n_l6KTVX^P;$ySF28=;rN^IpR8%n9}YRsF02QkP!yL&RW1&Ok=X-dwM^Vo5~ zPb^C3+g5uqkxY(bA#BTQ+&_PyvKdT#Ef+&LWRrq><_L2Zou7({wQ1@7!EVmfkX7=$ zZwG1W$}kkzKEa8El6qQIY4`6|N|?)mz`^UT*T`MyIMnO(&*9P(Q@|++gSj5US^1Eo zYY!#t^5I9GpCfW?y}(QLUbdYg2ePH{*C|?cJw!oiiCmmwQCcSNHN|-25UDCB^|2AA zfgJ{lq7Z%)DxdL86mTx>Z=H;%S*K<`=1km2K~}Tq78l;pqM}fxVK?)A+Ern?B6_Zf zD3B?tuK~DM3bSH`Svytu<-3OT(JFQc$fnTgj`-3FfQWt@6AdWY*ZozZ!-0~f3K4D+ z=1)D%_ru*m7$)KN)Au|549Ok;&+WBCsVhzHgU@9$MS*E8n{;(&iL~VoV_5Pt{@-Hv z%M6MNn-w;z3%`TEb=%wm#UI_KWdd$5uK8>hHS`tkL)j!<9hic5>O+tps4?dYzL?U+ zI2(@~Rf@;9;a>vHjp!nK@L0xB%XM|s;adwE5}V#c>$)7b*gZ2{T(12~4w385$r%q- zkGYZw6QUEyk~ayudaQIUi={iL%G^+xAiw4gU8Osg9@3256MgQB%6t}P-ap54(2l}e z!v3Tpz5eT!ik=Z}E)6tXX3(~Gwt&^T%2(vf4)4wA0b3k$;kILBXY-N<(bRP_o`!QQ znH;;uAp=dfQ1~G|=}z>@orbrjEhd@$O*0<8Jm`(?i))P1;9)bzz!kwcxM2dz<%2b3*l@!+(*S?*oMuW&4t@qwTT!eE*SkqNQeUfDY z@}*Q5<#L$&DE%);*)5waVJ8kL4Xtt~)XI~&p=7iMi7h7!`4{kulj5uz1?hk&BXT{*j1g>! z6pDYB-#`eI-_cl`%BB!(Bh*|jFdXDrW(JCf{xmivR>(D5Raq9wI#WcGrJPpmFDegc zAgKIdZa^u_)(^FjHD+j0A2YKoQPNDSkoqg4jvvwN9-o1?Xg0%d1zFec(d+WCZJU&o zsQGQs(-qaIm$pzLT}xs8p)9B)LQsWlnDD(msjhxvZL7xCW(2=CCd4`SV>(+_BCm zJq*1bj{19rQ*q8PxuAs;0;19Jwd@1JdbZmdTI7MNe<5L6T{cN!w8^i1zNq%;+4l~< z$z62s=dj`yy^eX85_9jDqxzn;-xIh_{OSu=mJ6CyZI;73ANXW~NbHSLcC5Fx0}Lja zq69-;WS7hrPYy6Wg9>{0oBr+}p3jk+R_32yC=lRY6L)OP{vf)@@JwAK@e9<~C^h&RyH$mh^t3+H9Q^Xh5j z8Cf-F0^QI0ISEvh>iRDRXQ%+;IuPX zjv?m?@c(pj=Fw30?;l5&?1@me>{N`R>?BJVJCp2W9s91a%bKn1WXry0EZHNGEz4xj z9$80<7zW`tJ5P5(`%G5{`S#x&ZVvc zREv(cWg}E%)47sJZWn|yA$oIczfkMmUo>u6Ta|xyOFjEi@wVLxT@}1AyqDX@U}2y8 z;Zo`;_$)oWgT!XmxZrvnn(MmP&IG>}CyiqjGX!>XV2eCvD58+u zmi@xl47=7i)W>wUl$+T6_MJg;Z@t$HMG}%9A^YgtxOndiKr|WWd0wAF3`bzitOy^I z+H{B->8#014S*P+h15~UPV>wps23GF#Zl~}_!7oxS{8Hf_Mlli(YneR!kuTRMCsQD zhK6}`tCVYYovgwbD3dEYodmKh%lDTZ5WPVoabxi=m9^t+BdU}eWl8#a7vFqjzlt}6 zb{6|8=$2DFI4hO-FxyjndInLU;KnR>^qhzPvzfRdy6nD#Nt3%7Dm|ZQV}QjHpLImi z?}nfIp)E2>%S{I>MKYC9jNc+K#)Ku!PrIec`aVK#rtl3F_X>A<03I^4XMP-=s~)>4 zHFY=*jbv+dVF7ndrMDhFke^-_diOJ)F9a}j&vOrhb_p6V| zL8hy-x((9&{&14QyAv*t6$!O<96@2lPFCQwxPVY~;&kI=?d8YxEId~jmqX7r6O_CA zOOK$9w_>Lv$GwcBxeMchEtunM203ZO864}C3eqHCt=m!aWx8)>)Xi}Z`*j(z)|Peg z&RSY#K$8$#vQfu0?1p64?1en0O|JzMi(PPi!*inBgU})z=fa9VyWKavj7WH6r?R?j z-TbzbE1ALKynu@*9LdNca6Rt>*=$)lyir(GuCuN=>64`W7yb=YUU4i|W)0Q;LkjPB z?3`F!c*dS3Sg|`Uy&3K}sP9V3?ba=7m{8M(zq@UJF2O&2#vFAnw&3ZjUZ`6Lxfhz> zuV1rb(Q|nYCoL=&-k%m!!je33^cME4A*xsqw!CpXo2_S%LSYeyZuX0&asM1{?tFXM z#zS`haAox?nZV6J=ZElfPw=&kwp!G2gq_<(=-#;@nQi%Y_VMV;`NjkzX3z~Dbo#=f zxXfouu_cO$HeYWPWP18B5D5BJ-A`U;eT3W?pEJqop{Eh0iaE^l)%I7B-Ixt7Exs<* zTO^-9=4MksrxWb#VED31#-%|(1J;AYgFtkSmktT|i#6`&TvHg-S8q?<2cp!>kz@*eO%oS zq_;q3wT;G|T3?5fs#^5URBobuRp5l`!nX!{8fPd)(mu^CI%zGcFfz-xUque_xQ2G7 z=*MMGY5DRul{Zb&3{SMhn|5}s_EFjl6x1&yhh)=5xs$$KB!rQCY{8#|2~w26Bd5$C zLm6&#&G#olvT?+RYJ(=8_jx2c-uUzwTLEcT_~vaPom#oZ%LmhXqa%qO&)ioI&=sU2 z=i7CrHfLOEs&^iVK~tlnXck&hD@>#HX)V)6ob|uH)AZYY+`MmwZjkn3;Nou#v3EXn z-l>*<)T}Z}jU_i>XyE9maUi5&IN;4t6BU$Zh(E$QRqE?lv#-Ba+44kfQNq}GEZg-i z35Z~}TFQd|LP>Gf_5iZb%$dVSG}-Xz5r|vq)JbhEV zPaGc@isNLq((YJmWYYzDq4t6oK1wl+i5b!RbahNGC0ZuGW|j4}4txgYl`dC=?y$6X zV(~;|E-KD(jac4~e1?bpAv;a&x$=EYO$Ju+RLLq#go1{`3reX%Lo#NM_r-iAVyY?4 z1!f-Z5uZxF8fX=6&pW=Ky|Q1I-tHYe3K8}v}O;IT*t?`kMta+A~Q zRw8A{q`a_9s4;(4%au{YuF2O5NHfFXle-G6*=}s#nCKg}OynoT2Mv-xMYfjuhj?7{oJjwh=}t z1b6+0u2D_0vcJofiOxm}=+Dm-&yOJc#*bjz)4d{Y(*whUN$sxp8skr~q1);jMAwRE zW_-B^WfE#d6pH$D4uu<27YE%<-1Qj9iV8SuN8u9`FL!gJ9Eu0`;E?zY(@G=-no{kc z*)>g`xaoL3&#l|$nx#`Y*7TSpK3Jj?a^vRCcr5{Ur+UCGWVtWE8&>*S<*RLJo!X#O;aoY3)L_;%&|sW%+Rp zIuUmRau%iT_ykxxW`Z_S9`hf~SI_lWkRpXu!}d`~+G=7i*~zCD%0JRc9#SL=GruCw}-G>Z>N-R|o( z3y;4g9!|KF@3HggaBF7#kz}i1|4i#6rrE=H2g_H}7!B=Ln+{1Nnlz#UJ;jfEjsXcJ zMt*JYVVDixS(t_Sf$YSt^W&GX?>pP8D&Spx>Re!+OZ~lwD#*2Ael-pGK5)NkcQ3dW za*0P_s9LSE_CuvpKLXsNM6SZK*$eKe{h*vRf7Jr2y(ojnmj9ODmB$q|Rkyx(7yEv?eo zSE+1z8mnw_zA%xfQYSXoyfO~|&OMsmSHiruYhg`Y)T=M(PwfO0glTB#o1#PdU->Wk zPDbq6^%e}hrGJVap24?y2QLF0Isjq9jFJ4W5*Eq|r5pkw^mNG@p>Y3gtfhc08&{@N z61ku&*WTd!89(jFBI88Lmg6X0@a}*`pSp&f36qE6`^a%G=fL_@DYcR7Dt(bKQO^JptflQzI13 zue)E{Tex4YU`i^CDKGucrL}V~Gq;t2nz~{FYD}DA-+n4peynU0#78B&yLI{2&ehBd z48!B3uIgNNmIgCyPVrYSanz*y8Hv{9r4kU#i0j+K(A(mk9xf{#S*>CwwDqqdYv_2- zNi;DkM3bY+yu~Vyjp$j|F5KNm<1P`=XwSX%pw5A|4NV0T)m~NcQVfKqhrX7!mF(EN zaF*W_NhkM`bz6teuw~NIg+nHKjp5c}50A{U`U~X}*r~|GysC2+gV+oDlqmtM#W7F( zt%RIXa5AjppfNM-B}*n@_Vz7xOhZh^Z~B!AH1fjb`>&`6$FhN*vs{GDMN%zY;mO}s zbS=cc!( zSNRHdF<}?CGNQK%^T8=6RNDaeDpx~p#c5XdAgTt)v8)F>-{Jv25$TL2Uv)5od@+j~ zvwk@4zKfEo)nPS$(KdDo3a5Z=Nf2isK zDe#c}uIAQR?f+cMOPDAFQh=IAj#b&J7!}5t^~=-k`?b_ml9vA8#Src|x&KMa>C5Ab z%i}8qSP4kh65xGxN$2vAAx6hz%mQK7Z`V!R{GO&MCS1+MNY25{9uv6s|GMu9Rh5ZU z`a$wM(H=dZKCr?S>`Uaom#y`g(tmC-t9S{CDnQw2dBl|TFm|N_5-cp*pQU<0?adrK zT)uyYzTVSk4X54ZIB27&_SGBodPvZW(9J^u{Y-sq26N?er^2Nv|@P1f~_dK&vQd4>;Ys$ z=|%XLt-`eZ(Hj*7U=zu*HX&xh*4qhc$jj>0Xj|r)tUlsOAz^DzR8Sd?kK*9L#8Jz8 zBE)jucC$GjvAl|`q4+a}JT~}Q_&h4iR{pMoWO#+gLqRhuNUy4E#9?v_7x)o*zLh+V z=);g=ddAnWn|AbY&v$u*qLnnMu~#mKmlB6?B**d=qo|f6WECq-cS&Q&unZ_%r}?f! zl4Sb6c3hWfutYwAUZ~8nsVrLIj5latW1%S>rs!E&-dp9;aIG%7RLQ}e%|X<@8uQd7opu5cctu{tVdE;Zsi}N8LSg*J@|?Y9)6(Cc`^P;yLiR7kXkf8tzT_ zlL4^kPg$9*S8m&^h&ra`oWUoHbV)T~Bo=)Khet9^x8OY`-R|^v*{0hE*Wpn~`4KVt zm&@w%)a)cC_Lpfgk+$@kEInnsD@}`Y)|f9yFLXZ-Y46-|*iNaYs>`E6 z+*gw~y!>TtQdzw~>(@cVi8KOulfjp7@|PFz?MMYO`mbruVDrszc7W-(|*(_mkXm2*eXW@kt52GuQv4`f+>(rZF9HAH$|Oy#>q zE9!U2)uQ8_ri27&PEAmf5xwI_&GC^6EIk%SgilaSGz8yZGN5jX%Sm2p6)AoB`f{v%L|hW;iHe^>+2m!rH2e)oJdEZk6kek>5unH zO%QQ(8Ijx-rF2pCHeICZ+J&4~&SE?X?Yqw{p9R8`zil|f#&o3roE?jKDq&&zs{g$H zkR%HP0AohK9r-ta8uO2S3LOO=2u#B~Ik525zh+|sLBRON6VOde^y!a~%|C@X0#U#q z#1oVh{=cApiA4l5f!Ts5%v%`i^*_o3Bn<+oz?{Bs)W2U!r+=dUlG+Dk0K-1M9sl>G z17mKGAK#Pr871J|G6TU-Sgijp>AboZgQU`)l_o z5CR;nPaq!@{{;EoqyRa&Y0okH;une0YDhAaX*0_{R#H(Z9EVTY_(6|Me08(1vK7jU_8uo{#$cSi^Yg} LI*Z)-_SgRb_fCzJ literal 0 HcmV?d00001 From 8eb9c26b18cdbd8c7bfa3ef96169ec794ff7d0e4 Mon Sep 17 00:00:00 2001 From: Sravan Balaji Date: Sat, 11 Apr 2020 15:20:25 -0400 Subject: [PATCH 2/2] System Simulation Attempt - Add comments for axes used in roll, pitch, yaw - Attempt to simulate system over time --- src/PlantModel.m | 58 +++++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 52 insertions(+), 6 deletions(-) diff --git a/src/PlantModel.m b/src/PlantModel.m index da84ed2..6ed1a03 100644 --- a/src/PlantModel.m +++ b/src/PlantModel.m @@ -17,12 +17,12 @@ Iz = 8.801*10^-3; % X4: Velocity along x axis - x' % X5: Velocity along y axis - y' % X6: Velocity along z axis - z' -% X7: Roll angle - phi -% X8: Pitch angle - theta -% X9: Yaw angle - psi -% X10: Roll rate - phi' -% X11: Pitch rate - theta' -% X12: Yaw rate - psi' +% X7: Roll angle - phi (y-axis) +% X8: Pitch angle - theta (x-axis) +% X9: Yaw angle - psi (z-axis) +% X10: Roll rate - phi' (y-axis) +% X11: Pitch rate - theta' (x-axis) +% X12: Yaw rate - psi' (z-axis) % Inputs: % U1: Total Upward Force on the quad rotor along z-axis @@ -77,3 +77,49 @@ C = [1 0 0 0 0 0 0 0 0 0 0 0;... 0 0 0 0 0 0 0 0 1 0 0 0]; D = zeros(6,4); + +continuous_system = ss(A, B, C, D); +T_s = 0.05; +discrete_system = c2d(continuous_system, T_s); + +Q = diag([1, 1, 1, ... % x, y, z + 1, 1, 1, ... % x', y', z' + 1, 1, 1, ... % roll, pitch, yaw + 1, 1, 1]); % roll', pitch', yaw' + +R = diag([1e3, 1e3, 1e3, 1e3]); % upward force, pitch torque, roll torque, yaw torque + +% u = -Kx +K = dlqr(discrete_system.A, discrete_system.B, Q, R, 0); + +num_steps = 10; +X = zeros(12, num_steps); +% Initial Condition +X_t = zeros(12,1); +X(:,1) = [0, 0, 0, ... % x, y, z + 0, 0, 0, ... % x', y', z' + deg2rad(5), deg2rad(0), deg2rad(0), ... % roll, pitch, yaw + deg2rad(0), deg2rad(0), deg2rad(0)].'; % roll', pitch', yaw' + +for i = 1:num_steps + X(:,i+1) = (A - B*K) * X(:,i); +end + +Y = C * X; + +figure(1); +t = 1:num_steps; + +subplot(1,2,1); +hold on; +plot(t, Y(1, t), 'r-'); +plot(t, Y(2, t), 'b-'); +plot(t, Y(3, t), 'g-'); +legend('x', 'y', 'z') + +subplot(1,2,2); +hold on; +plot(t, Y(4, t), 'r-'); +plot(t, Y(5, t), 'b-'); +plot(t, Y(6, t), 'g-'); +legend('roll (y-axis)', 'pitch (x-axis)', 'yaw (z-axis)')