a2nr
/
FormationControlSimulation
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

prepare for second order system

master
a2nr 2019-12-18 14:23:12 +07:00
parent a0faa211e1
commit 06667063ba
20 changed files with 477 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
DOC/Oh_et_al-2014-International_Journal_of_Robust_and_Nonlinear_Control.pdf Normal file
View File

Binary file not shown.

BIN
DOC/RP_2015_Rozenheck.pdf
View File

Binary file not shown.

BIN
DOC/related_jurnal/[2016][Pickem][The Robotarium: Swarm robotics research testbed].pdf Normal file
View File

Binary file not shown.

BIN
DOC/related_jurnal/[2018][Khaledyan][Maneuvering Nonholonomic Formation].pdf Normal file
View File

Binary file not shown.

BIN
DOC/related_jurnal/[20XX][Dimos][Stability_Distance-Based_multi-robot_formation].pdf Normal file
View File

Binary file not shown.

48
SOURCE/DataErrorEdge.data Normal file
View File

@ -0,0 +1,48 @@
# Created by Octave 5.1.0, Tue Dec 17 11:11:50 2019 WIB <adnr@morilin.fedo>
# name: yans
# type: matrix
# rows: 41
# columns: 20
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0

48
SOURCE/DataOutMotion.data Normal file
View File

@ -0,0 +1,48 @@
# Created by Octave 5.1.0, Tue Dec 17 11:11:50 2019 WIB <adnr@morilin.fedo>
# name: y
# type: matrix
# rows: 41
# columns: 22
15 17 20 25 25 28 25 20 22 10 15 2 0 0 0 0 0 0 0 0 0 0
19.529862137488188 16.56176457594966 19.906444483047473 18.001085941039847 21.198177331740656 19.153602853443438 21.27675851798281 17.426283712706095 20.903730539596474 15.991222664982077 19.185026990144419 14.866040251878882 -0.06709058730158815 0.33080317362073541 -0.28645433846856583 0.17109275001568031 0.11717275700439803 -0.31460890316849532 0.12618498581823895 -0.11712457002714245 0.31231905316224834 0.096581562223019224
19.533052104244934 16.56209345047445 19.906996505587504 17.998528776440455 21.194386272780505 19.154878457186459 21.279232171052811 17.426582425509348 20.904459575674647 15.989630531110455 19.181873370659559 14.868286359278759 -0.30665260415434836 0.56850156667217222 -0.5243721722838196 0.16981272871406747 0.1163197069471254 -0.55250346261667005 0.12473776159025082 -0.35722451853671378 0.55014540886397878 0.09542209470016863
19.534231155874046 16.563071267926308 19.907528854709994 17.99640085784452 21.194340992531082 19.155393431203645 21.278232125469703 17.425742082413578 20.904090293315253 15.991708988790702 19.181576578099953 14.86768337182121 -0.54680140552892209 0.80623089979552853 -0.76256937374947165 0.16828176375914186 0.11521783021694242 -0.79016729710110745 0.12355856154039856 -0.59663982837092444 0.78799066497068204 0.094523048645858129
19.533413198318009 16.562231314630566 19.907109482535457 17.997778545826506 21.194036453169403 19.155102442110174 21.278979455583826 17.426499172783323 20.904381212495828 15.99033539270572 19.182080197897434 14.868053131943681 -0.78704609446161444 1.043877339882872 -1.0007957928151547 0.16677077436125443 0.11414174569658438 -1.0278604755361287 0.12240840898998923 -0.83611410601742786 1.0257053518849215 0.093665849674228613
19.533920768904252 16.562560636237283 19.907227315413778 17.997046229034641 21.193996437160028 19.155453166213949 21.278520159960497 17.426232757345186 20.904334852837934 15.99108142774306 19.182000465723473 14.86762578342586 -1.027274985347401 1.2815628145121634 -1.2389982754776816 0.16523628737990317 0.11305223837303724 -1.2655367388489358 0.12124125250471506 -1.0755799581724388 1.2634460742574478 0.092787261154695599
19.539124792531361 16.567132131259136 19.910683353306666 17.986127789999244 21.194657696055152 19.157327121452344 21.27362369284165 17.422132009301059 20.901203043315906 16.00198779201672 19.180707421949226 14.865293155971509 -1.267266870208718 1.5194421693798885 -1.4771293094298661 0.16367850265252926 0.111925709451206 -1.5031368182711538 0.12005671965456424 -1.3148318325742308 1.5015915782145615 0.091872346644752259
19.530217050227705 16.559111265227227 19.904653416179375 18.004843419152728 21.193224709539383 19.154505707761928 21.282015130802272 17.429406241916524 20.906693282519885 15.983411509181504 19.183196410731391 14.868721856760109 -1.5076589396134348 1.7568783866492814 -1.7155162152082362 0.1623409725132732 0.11097611807754977 -1.7410103470303218 0.11903225197600566 -1.5544773785542731 1.7388570514280204 0.091130591700147395
19.534470196846449 16.562899383626384 19.907361852036871 17.995887912216428 21.193614535896494 19.155813422654532 21.27800898728756 17.425989657531737 20.904252492257115 15.99229654310763 19.182291935675394 14.867113080863222 -1.7477341198814975 1.9946951443273093 -1.9536578970205367 0.16079322453249068 0.10987940911869665 -1.9786445416781431 0.11785899820275733 -1.7938170999382737 1.9768492108082971 0.090235818701222936
19.536541211163041 16.564578741120169 19.908558283334564 17.991982576127654 21.193898138018767 19.156789553071963 21.276055135503285 17.424497867828453 20.903213351398669 15.996224666129466 19.181733880581842 14.86592659572247 -1.9878247922786916 2.2324761900141503 -2.1918410272056539 0.15929269479036226 0.10880661329587761 -2.2163165573527093 0.11672272781512204 -2.0331689541951765 2.2147702148612125 0.089369214503153244
19.534855853903814 16.563069091705572 19.907576244944234 17.994937774756856 21.193397282653489 19.155998046857945 21.277609574530356 17.425851224866999 20.904032473855217 15.99325128219443 19.182528570112837 14.866892579618204 -2.22799524099185 2.4701340436587911 -2.430094383606809 0.15787348409998309 0.10780318530436155 -2.4540756225607163 0.11564597715444139 -2.2726145609949664 2.4524836097647347 0.088565954232588201
19.532513523278467 16.56075190521566 19.906024152617977 17.999867300984835 21.19295517121828 19.155246404012271 21.279790250160225 17.427924977942535 20.905369956843185 15.988293109184374 19.183346945881809 14.867916302660309 -2.4682206011424261 2.7077578952567722 -2.6683416802786475 0.15643889338350656 0.10679583266174239 -2.6918353166761388 0.11456490330531782 -2.5120990594366903 2.6901426116880587 0.087760667167804174
19.534050760685801 16.56209375925015 19.906843759392569 17.997010176925933 21.193152767246321 19.156035165635014 21.278346115532536 17.426758202604308 20.904715140824674 15.991227214258842 19.182891456318057 14.866875481325744 -2.7082524829694545 2.9455561357737605 -2.9065479696866046 0.15499428707904644 0.10576259796471371 -2.9295455009821416 0.11347205042348905 -2.7514168915772661 2.9280779366038501 0.08692505073083015
19.532951924023756 16.561034232361298 19.9061472981907 17.999058167335704 21.192795513314604 19.155569335187906 21.279311590973162 17.42769913639729 20.905284994009378 15.989191072827033 19.183508679488366 14.867448055890776 -2.9483237217167497 3.1832846233221566 -3.1448146029566657 0.15363817262761076 0.10478909103398854 -3.1673232389513024 0.11243757247818505 -2.9907939696975121 3.1658565338936513 0.086151397979168945
19.530321435338642 16.558552883788025 19.904400810429809 18.004504274254167 21.192344126187415 19.154821692564656 21.281830045622655 17.429973743826242 20.906855352740827 15.983749100189957 19.184248229680634 14.868398305376928 -3.1884116908840103 3.4209659437351965 -3.3830871936210087 0.15228560009845568 0.10382328895067988 -3.4051227914102791 0.1114117539967926 -3.230192614270377 3.4036162392550948 0.085375795217757428
19.533444942138765 16.561363672715423 19.906337022907152 17.998113817745416 21.192660804768899 19.155819932086953 21.278889049316508 17.427433070116706 20.905151005708881 15.990112116082182 19.183517175159785 14.867157391253302 -3.428410363644252 3.6587921006076316 -3.6212482211898136 0.15081475763422897 0.102791456059593 -3.6428166450209813 0.11030956625741965 -3.4694856382539316 3.6416432271700643 0.084529267061345156
19.533229878620112 16.561107439840818 19.906133207471299 17.998607043836785 21.192552889515095 19.155784111069945 21.279079260490867 17.42766968805681 20.905340131206774 15.989619922270665 19.183664632695841 14.867211794924959 -3.6684916342673706 3.8965198026746015 -3.8594677736263474 0.14940409941326122 0.10180360115835484 -3.8805746941940296 0.10925372593496556 -3.7088590009831148 3.8794830198964636 0.083729272722899289
19.532906894412442 16.560828184581467 19.905868494634081 17.999237658585788 21.192404992315204 19.155678920066933 21.279366123976114 17.427933173113562 20.905611469400455 15.989016220261233 19.183842025261729 14.867305843391021 -3.9085655144446587 4.1342483399957741 -4.0976933650967542 0.1480066078407655 0.10082832786384537 -4.1183439418389582 0.10820864459807007 -3.9482306748045723 4.1173138206455704 0.082938526208017851
19.535323587171291 16.562963866686268 19.907154645880237 17.99467813098153 21.192679950259123 19.15674087621878 21.277069126325461 17.426044987690659 20.904561245082498 15.993657620563789 19.183211445281415 14.865914517858936 -4.148528344146647 4.3721036046548987 -4.3359064836571877 0.14663487863654176 0.099850756162462861 -4.3560907636168604 0.10716720759070313 -4.187525156710616 4.3552410784456761 0.082149289316856625
19.535894715595283 16.563431758768047 19.907478312867106 17.993479299808989 21.19266824203785 19.156987604273571 21.276582926722035 17.425648923935899 20.90428539865632 15.994830532588809 19.183090404121444 14.865621880624643 -4.3885384355891137 4.6098737783659933 -4.5741140100922228 0.14525003556433511 0.098882721146198185 -4.5938588564197564 0.10612935638497165 -4.4268607403878795 4.593149364322251 0.081358689958545949
19.534779458145557 16.562333529504741 19.906761296576011 17.995752095918082 21.192408529316427 19.156591992070478 21.277602957957566 17.426627374186907 20.904894335807658 15.992551263991853 19.183553422196798 14.866143744327873 -4.6285912371749287 4.8475969487530577 -4.8123552692429215 0.14390273705574846 0.097943324293409045 -4.8316608204016998 0.10512256571536489 -4.6662302364085013 4.8309604219743942 0.080601370201814251
19.537102390943232 16.564388875020164 19.908151984276262 17.991144962869527 21.19271504062575 19.157466587629695 21.2754253405196 17.424785750344597 20.903683959432531 15.997157930076231 19.182921284202617 14.865055894059713 -4.8685261779059203 5.0854388531464521 -5.0505359489165143 0.14250745530072551 0.096966411619696588 -5.0694058913872908 0.10408332327908171 -4.9054919326667052 5.0689958847059682 0.079800559524590434
19.534943167285906 16.562394624112045 19.906798517863049 17.995451532302205 21.192309411589317 19.156758521727443 21.277441335639686 17.426592523477229 20.90486609535078 15.992877338760049 19.183641472271258 14.865925459620991 -5.1085833265124103 5.3231399696373396 -5.2887999247616309 0.14119985455676368 0.096058138757231515 -5.3072420472979722 0.10310680280401477 -5.1448781353666444 5.3067510442712784 0.079071379995198546
19.535463171546819 16.562869583546906 19.907018134498216 17.994489889334574 21.192321194598552 19.156955715140597 21.2769380618371 17.426173448507967 20.904723421482352 15.993853133260561 19.183536016036953 14.865658230209371 -5.3485438827822263 5.5609361860304771 -5.5270183988444712 0.13985729842398181 0.095122492633918851 -5.5450364026238201 0.10210728612599287 -5.3841775373214711 5.5446818037566254 0.078307386933810577
19.535531252797071 16.562885154884608 19.907091520153056 17.994251991972884 21.192276931038194 19.15704593028839 21.276878007924402 17.426173526756077 20.904637816782429 15.99410339365107 19.183584471304837 14.865540002446934 -5.5885078879202821 5.7987205963065289 -5.7652433899452378 0.13853465300813483 0.094201707892327929 -5.7828436211670571 0.10112211935866332 -5.623486006266174 5.7825924782820781 0.077560668182756462
19.534672586245282 16.56203653373408 19.906531099887083 17.996006546561734 21.192068390579927 19.156736480107867 21.277657619028332 17.426928895847219 20.9051113409602 15.992345672802649 19.183958963299396 14.865945870946607 -5.8284808474591863 6.0364849876458484 -6.0034885722824063 0.13724237862391253 0.093302497405866353 -6.0206759525661955 0.10015919164934092 -5.8628100934451099 6.0204533015876409 0.076834112512851091
19.533675289017051 16.56099649061148 19.905929853245194 17.998048602409668 21.191917397878736 19.15655012706387 21.278617830640716 17.427886741929893 20.905598908881473 15.990302070168097 19.184260720336813 14.866215967816942 -6.0684439570096549 6.2742500577010212 -6.2417309606808704 0.13595749701278168 0.092407516132985631 -6.2585127199643136 0.099202635891691773 -6.1021309514263615 6.2583248928549269 0.076111509002276923
19.534670647549831 16.561954876732432 19.906495865134097 17.995995534414593 21.191955318727384 19.156826253853279 21.277649611751965 17.427018885849165 20.905136285387261 15.992378216208657 19.184092271449472 14.865826232941791 -6.3083419644481422 6.5120833958302429 -6.479950590306335 0.13465699974313788 0.091505041708630358 -6.4963284092030777 0.098235871717537754 -6.3413944954998733 6.4962944619384482 0.075374037925620044
19.534319837407892 16.561573190883866 19.906257097607426 17.996707565762307 21.19184582457639 19.156710593009098 21.277957958162258 17.427358841957712 20.905338797222878 15.9916579425233 19.184280485023212 14.865991865863693 -6.5482575370500218 6.7498824420816259 -6.7181895068032818 0.13338789664183817 0.090622450478350006 -6.7341703945816231 0.09729247598247856 -6.5806820687669081 6.7342111577943031 0.074657467525546306
19.534984424047089 16.562153453787474 19.906608886239656 17.995373614217407 21.191846662186727 19.157064197292076 21.277328654301552 17.426865369704572 20.905044666963835 15.993053677638811 19.184186706261144 14.865489687359604 -6.7881341814403422 6.9877306747208285 -6.9564360957695879 0.13215050488486743 0.089754860704971975 -6.9720171169002834 0.096361104433855191 -6.8199556159317778 6.9721192332085913 0.073958542814968997
19.536926891261999 16.563867126290702 19.907634272085101 17.991691172469228 21.192052801694935 19.157797802167465 21.275475333389931 17.425328079075054 20.904216765224369 15.996748503437527 19.183693936343676 14.864567316559945 -7.027916463348304 7.2256474111547453 -7.1946649939761453 0.13091561936170062 0.088878937179074496 -7.2098494314000021 0.095431000804541311 -7.0591555656108795 7.2101588884058962 0.073243373121429839
19.534415294065518 16.561421612699299 19.906131249359472 17.996580860228821 21.19150693802224 19.156946203791275 21.277815738368314 17.42753163969272 20.90545712052813 15.991847806661038 19.184673659656326 14.865671876926742 -7.2678223459260431 7.4634223965208024 -7.4329766660316769 0.12978306887798682 0.088078923458137864 -7.4477748523406335 0.094568236448219281 -7.2984923079901076 7.4479040348965118 0.072612559748053099
19.538895498779656 16.565444946457813 19.909077695233556 17.987207398302516 21.19206085660273 19.158405531839406 21.273598525991819 17.423892234669754 20.902807021871627 16.001193941638313 19.183560401520598 14.863855947092079 -7.5074846797849686 7.7014282921201724 -7.6711507941501011 0.12852160859428949 0.087185597993416575 -7.6855666208881566 0.093625223234403768 -7.5375836270361711 7.686178432154434 0.071875352458418945
19.531487924283446 16.55859877435989 19.90430657602753 18.002356153703527 21.190881502543657 19.15600402630383 21.280545245249439 17.430093276477884 20.907009109105708 15.98607554997241 19.185769642790245 14.866872219182357 -7.747497610119181 7.9390615633246187 -7.9095462032489863 0.12747254596392413 0.086449815874867025 -7.9235891445112729 0.092829095898788294 -7.7770276654616692 7.9236701194097847 0.071306466323020998
19.534039129422226 16.560892344443257 19.905818684246963 17.997336948405604 21.191171949903925 19.156982550258011 21.278136471849042 17.428031028492661 20.90570114099862 15.991149833545553 19.185132623579154 14.865607294854755 -7.9872125102243023 8.1770205385832391 -8.1477704360601653 0.12627192890097616 0.085601378211356355 -8.1614361207734447 0.091926765844464922 -8.0161821514507015 8.1617704007739533 0.070615207219965295
19.533844320985306 16.560653420269244 19.905630056245055 17.997720042171277 21.191037390214031 19.156898219212962 21.278319316758509 17.428255281195721 20.905884699513745 15.990733165293701 19.185284216283335 14.865739871857025 -8.2269959553615397 8.414885380121591 -8.3860307079140064 0.12511206835957309 0.08478720837684417 -8.3993355777462835 0.09105634020228881 -8.2554145096803975 8.3997298797861806 0.06995281445191899
19.533341512108194 16.560233973308566 19.905248843657077 17.998628159863269 21.190809758297341 19.156654642404991 21.278735297806563 17.428634986590893 20.906257494424732 15.989862949951366 19.185607093706015 14.865985287880795 -8.4667764542102244 8.6527535385895131 -8.6243148938813352 0.1239858623999419 0.083995612000674347 -8.6372549908895859 0.0902086583473201 -8.494647805010894 8.6376441855951676 0.069315401527552209
19.530432487303663 16.55743337989659 19.903306302393219 18.004880351466042 21.190435848265061 19.156032418791668 21.281563055150698 17.431225622320447 20.907992777646406 15.98363255642133 19.186269529240878 14.866795671103818 -8.7066069160595561 8.8905470463881784 -8.8626140096877322 0.12287372718740405 0.083216989534306895 -8.8752070446104945 0.089376033146873715 -8.733932124945559 8.8754845984758539 0.068689134800338975
19.534434066092555 16.561082422388989 19.905802422090083 17.996661515035761 21.190898132465218 19.157247290125465 21.277741575675904 17.427902170090416 20.905792752395527 15.991835725532342 19.18533105128061 14.865270876826902 -8.9462183467532927 9.1285714058831644 -9.1008116622012611 0.12168533817143985 0.082378122352154678 -9.1130494139189704 0.088489579544776528 -8.9730073978281144 9.113733373518965 0.067996917774731352
19.534724154133745 16.56128031484544 19.905987835688396 17.996139743120651 21.19091452699962 19.157494813771397 21.277483980680778 17.427738657658399 20.905612750499593 15.992398734604581 19.185276751997947 14.864947735999543 -9.1859278815789267 9.3664820146360945 -9.3390677383001055 0.12055534432894008 0.081584865431017894 -9.3509593037783425 0.087646392730475878 -9.2121793516739245 9.3517751816662553 0.067354028114848821
19.53455528100038 16.561119485460313 19.905833641544959 17.996405366474541 21.190788708650093 19.157342787602925 21.277616095330256 17.427880283125031 20.905766047974879 15.992105129603086 19.185440225499409 14.865146947734031 -9.4256431810058956 9.6043755702955735 -9.5773317554254724 0.11944003213548021 0.080805120407522052 -9.5888869120694569 0.086815204385511741 -9.4513682122134064 9.5897857083993436 0.066717763337873937

BIN
SOURCE/IMG/circle_form.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 28 KiB

BIN
SOURCE/IMG/sin_form.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 30 KiB

62
SOURCE/RP_2015_Rozenheck.tex
View File

@ -92,6 +92,7 @@ dan menyingkat perhitungannya dan menghasilkan persamaan
\section{Kendali}
\subsection{Orde Satu}
Error jarak dinotasikan dengan $\delta \in \mathbb{R}^m$ dimana
\begin{equation}
\delta_k = || e_k ||^2 - d_k^2, k \in \{1, \dots, m\}
@ -149,14 +150,71 @@ lalu kombinasi persamaan tersebut diperoleh state- space close loop
\begin{bmatrix} B \\ 0 \end{bmatrix}.v_{ref}
\end{equation}
\subsection{Orde Dua}
Definisi persamaan orde dua
\begin{align}
\begin{cases}
\dot{x}_1 = x_2\\
\dot{x}_2 = u
\end{cases}
\end{align}
Dengan fungsi potensial
\begin{equation}
\Phi(e) =
\frac{1}{2}\sum_{i\in V} || x_{2i} ||^2 + \frac{1}{2} \sum_{k=1}^{m} \delta_k^2
\end{equation}
Maka persamaan state space nya
\begin{align}
\begin{cases}
\dot{x}_1 &= \triangledown_{x_2} \Phi = x_{2}\\
\dot{x}_2 &= c(-\triangledown_{x_2}\phi -\triangledown_{x_1}\phi)\\
&= c(-x_2(t) - R(x_1)^T(R(x_1)x_1(t) - d ))
\end{cases}
\end{align}
Dengan menerapkan kendali PI
\begin{align}
\dot{x}_2 =& C(-x_2(t) - R(x_1)^T(R(x_1)x_1(t) - d )) \\
=& -K_{p1}x_2(t) -R(x_1)^T K_{p2}(R(x_1)x_1(t) - d )) \\
& -K_{i1} \int_0^\tau x_2(\tau) d\tau
-R(x_1)^T K_{i2} \int_0^{\tau} (R(x_1)x_1(\tau) - d )) d\tau \\
\end{align}
Menghasilkan state baru dan Ditulis ulang
\begin{align}
\dot{x}_1 =& x_{2}(t)\\
\dot{x}_2 =& -k_{p1}x_2(t) -R(x_1)^T k_{p2}(R(x_1)x_1(t) - d )) - \xi_1 - R(x_1) \xi_2 \\
\dot{\xi}_1 =& k_{i1} x_2(t)\\
\dot{\xi}_2 =& k_{i2} (R(x_1)x_1(t) - d)
\end{align}
Dalam bentuk matrix
\begin{align}
\begin{bmatrix}
\dot{x}_1 \\ \dot{x}_2 \\ \dot{\xi}_1 \\ \dot{\xi}_2
\end{bmatrix} =
\begin{bmatrix}
0 & 1 & 0 & 0\\
-k_{p2}R(x_1)^T R(x_1) & -k_{p1} & -1 & -R(x_1) \\
0 & k_{i1} & 0 & 0 \\
k_{i2}R(x_1) & 0 & 0 & 0 \\
\end{bmatrix}
\begin{bmatrix}
x_1 \\ x_2 \\ \xi_1 \\ \xi_2
\end{bmatrix} +
\begin{bmatrix}
0 \\ k_{p2}R(x_1)^T \\ 0 \\ -k_{i2}
\end{bmatrix} d
\end{align}
\subsection{Implementasi}
Pada seksi implementasi ini akan dibahas mengenai code yang digunakan
untuk menyimulasi persamaan yang diciptakan oleh peneliti.
\begin{figure}
\centering
\includegraphics[scale=.5]{./IMG/SIM_FIG1.png}
\includegraphics[scale=.5]{./IMG/SIM_FIG2.png}
\includegraphics[scale=.3]{./IMG/SIM_FIG1.png}
\includegraphics[scale=.3]{./IMG/SIM_FIG2.png}
\includegraphics[scale=.3]{./IMG/circle_form.png}
\includegraphics[scale=.3]{./IMG/sin_form.png}
\caption{Hasil Membuat Ulang Simulasi}
\label{sim_fig1}
\end{figure}

118
SOURCE/SM_2015_Rozenheck.m
View File

@ -1,21 +1,32 @@
clear -all
close all
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang kene
conRobot = [1 2 1;
1 4 1;
%% 2 1 -1;
1 5 1;
%% 5 1 -1;
1 6 1;
%% 6 1 -1;
2 3 1;
%% 3 2 -1;
2 4 1;
%% 4 2 -1;
3 4 1;
%% 4 3 -1;
4 5 1;
%% 5 4 -1;
5 6 1;
%% 6 5 -1;
1 4 1;
%% 4 1 -1;
5 2 1;
%% 2 5 -1
%% 6 3 1;
%% 6 4 1;
%% 6 2 1;
%% 3 1 1;
%% 3 5 1;
];
length_d = 3;
dRobot = [ 1;
1;
1;
@ -24,25 +35,21 @@ dRobot = [ 1;
1;
1;
1;
1;
1;
%% 1;
%% 2;
%% 2;
%% 2;
%% 2;
1.41;
1.41;
%% 2;
%% 3.014;
%% 3.014;
%% 3.014;
%% 3.014;
];
corRobot = [2; 2.5;
2.7; 2.6;
3; 2.3;
2.5; 1.5;
1.8; 1.5;
1.5; 2;
]; % xy xy xy
%% figure(1)
[R,K,d] = rigidityMatrixFnc(conRobot);
vref = [-5; 5];
corRobot = [1.5; 1.7;
2; 2.5;
2.5; 2.8;
2.5; 2;
2.2; 1;
1.5; 0.2;
]*10; % xy xy xy
B = [
1 0; 0 1;
0 0; 0 0;
@ -51,30 +58,46 @@ B = [
0 0; 0 0;
0 0; 0 0;
];
kp = 10;
kp = 30;
ki = 3;
%% fvref = fncSpeedRef('ysin',100,2);
%% fvref = fncSpeedRef('xsin',50,2);
%% fvref = fncSpeedRef('cw',-100,1);
fvref = fncSpeedRef('s',0,0);
fvrefans = fncSpeedRef('s',0,0);
tspan = 1:0.1:5;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang nduwur
[R,K,d] = rigidityMatrixFnc(conRobot);
_zero = zeros(size(R(corRobot,K),1),1);
sInit = [corRobot; _zero;];
sAnsInit =[_zero; _zero;];
%% tspan = linspace(1,0.1,30);
tspan = 1:0.01:10;
dydt = @(t, y) systm_robot(t, y,(dRobot*6), R,K, kp, ki, B, vref);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% start solving
printf("Mulai memecakan masalah \n\n");
startExe = tic;
dydt = @(t, y) systm_robot(t, y,(dRobot*length_d), R,K, kp, ki, B, fvref(t));
[t,y] = ode45(dydt, tspan, sInit);
cntr = 1;
bypassCntr = @(c) cntr;
plusPlusCntr = @(c) cntr++;
dyansdt = @(t, yin) systm_anlys_robot(t, yin, y(:,1:(2*numNodes(edgeL2adj(conRobot)))) ,bypassCntr, plusPlusCntr ,(dRobot*6), R, K, kp, ki, B, vref);
dyansdt = @(t, yin) systm_anlys_robot(t, yin, y(:,1:(2*numNodes(edgeL2adj(conRobot)))) ,bypassCntr, plusPlusCntr ,(dRobot*6), R, K, kp, ki, B, fvref(t));
[t,yans] = ode45(dyansdt, tspan, sAnsInit);
ddYans = zeros(size(yans,1),1);
for i = 1:size(yans,1)
ddYans(i) = norm(yans(i,1:(size(yans,2)/2)));
endfor
endExe = toc(startExe);
printf("Membutuhkan waktu %i menit, %i detik \n untuk memecahkan masalah mu \n\n",
floor(endExe/60), rem(endExe,60))
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% End Solving
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% start plot it
close all
figure(1)
% plot trayektori dari setiap robot
str_tmp = "plot(";
for i = 1:length(corRobot)-1
str_tmp = strcat(str_tmp,sprintf("y'(%i,:),",i));
@ -83,6 +106,8 @@ str_tmp = strcat(str_tmp,sprintf("y'(%i,:));",i+1));
eval(str_tmp);
hold on
% membuat fungsi plot robot di waktu
% tertentu
str_tmp = "@(t) plot( [";
for i = 1:2:length(corRobot)
str_tmp = strcat( str_tmp, sprintf("y'(%i,t), ",i));
@ -97,6 +122,8 @@ plot_rb = eval(str_tmp);
xrb = 1:2:length(corRobot);
yrb = 2:2:length(corRobot);
% fungsi untuk plot coneksi di waktu
% tertentu
function plot_con (pltRb, yOut, conIn,xm,ym, time)
pltRb(time);
for i = 1:length(conIn)
@ -106,23 +133,44 @@ function plot_con (pltRb, yOut, conIn,xm,ym, time)
endfunction
plot_con(plot_rb, y', conRobot, xrb, yrb, 1);
plot_con(plot_rb, y', conRobot, xrb, yrb, round(length(tspan)/2));
plot_con(plot_rb, y', conRobot, xrb, yrb, length(tspan));
str_tmp = "legend(";
for i = 1:numNodes(edgeL2adjL(conRobot))-1;
str_tmp =strcat(str_tmp,sprintf("\"Robot %i \", ",i));
str_tmp =strcat(str_tmp,sprintf("\"R%i \", ",i));
endfor
str_tmp =strcat(str_tmp,sprintf("\"Robot %i \" )",++i));
str_tmp =strcat(str_tmp,sprintf("\"R%i \" )",++i));
eval(str_tmp)
title("Motion dari Robot");
figure(2)
% Plot analisis error secara keseluruhan
ddYans = zeros(size(yans,1),1);
for i = 1:size(yans,1)
ddYans(i) = norm(yans(i,1:(size(yans,2)/2)));
endfor
plot([1:length(ddYans)],ddYans )
hold on
% Plot analisis error setiap robot
str_tmp = "plot(";
for i = 1:length(dRobot)-1
str_tmp = strcat(str_tmp,sprintf("[1:length(yans'(%i,:))], yans'(%i,:),",i,i));
endfor
str_tmp = strcat(str_tmp,sprintf("[1:length(yans'(%i,:))], yans'(%i,:));",i+1,i+1));
eval(str_tmp);
%% plot_rb(20);
%% plot_rb(40);
%% plot_rb(60);
%% plot_rb(80);
%% plot_rb(100);
str_tmp = "legend(";
str_tmp = strcat(str_tmp,sprintf("\"All\", "));
for i = 1:numNodes(edgeL2adjL(conRobot))-1;
str_tmp =strcat(str_tmp,sprintf("\"R%i \", ",i));
endfor
str_tmp =strcat(str_tmp,sprintf("\"R%i \" )",++i));
eval(str_tmp)
title("Norm error setiap edge robot")
save DataOutMotion.data y
save DataErrorEdge.data yans

48
SOURCE/circleDataErrorEdge.data Normal file
View File

@ -0,0 +1,48 @@
# Created by Octave 4.4.1, Tue Jul 23 08:17:46 2019 WIB <adnr@morilin.fedo>
# name: yans
# type: matrix
# rows: 41
# columns: 18
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
-0.096911478315354516 -0.075730185298846384 -0.012818693778102748 0.02494319515544317 -0.071455460399853357 0.058061517393821691 -0.00046049687370259293 0.031894441863279527 -0.00020103212281644091 -0.023675953022652619 -0.020957934876851849 -0.0036309652191249529 0.0093563014520160383 -0.013149718234368087 0.01202648390515818 -0.0012963378978132847 0.0062554711548290943 -0.0022820491653563553
-0.10425265654265486 -0.074438472824579799 -0.013020183069880772 0.020937547114184728 -0.085196296071277988 0.061012542456736242 0.0061415215844350711 0.037731753417632005 0.0051921291614990791 -0.054166923794470918 -0.043490768870371808 -0.0075028693060688115 0.016084571794996859 -0.037198913299163625 0.030088371215826977 -0.00023775623244392841 0.016935638830019823 -0.0013201073157883807
-0.10496977223679899 -0.073423053303207697 -0.013052667938678717 0.019981714893807078 -0.087406749208225401 0.060803658484664999 0.0076919183959292259 0.038627371912961526 0.0062162636973524497 -0.085616578743486924 -0.065661095427974769 -0.011418819655041261 0.02219524096238892 -0.063204202538559573 0.048378181459469968 0.0018967953279961551 0.028437807258383522 0.00043298687365519631
-0.10430390951433496 -0.07262905261624597 -0.012956881974499325 0.019642588586123517 -0.087186661092282874 0.060268414355689706 0.0079645869608754253 0.038506981507778149 0.0063842529304324949 -0.1170217651945822 -0.087566442340655359 -0.015321451258742397 0.02813360911223722 -0.089417611490451535 0.06654164850612454 0.0042584180904954331 0.040017904201910121 0.0023318424946289448
-0.10334147914108427 -0.071890644056646885 -0.012837287263367293 0.01941026499079718 -0.086453982873853016 0.059681403532539722 0.0079584985700906045 0.038179070629303639 0.0063652323550624761 -0.14817120045783436 -0.10924360269998434 -0.01919095456577553 0.033989838629414508 -0.11546877580916949 0.084534540689276488 0.0066496709541252093 0.051522904026464054 0.0042459112311731603
-0.10232423391632046 -0.071170339812851133 -0.01271312996813039 0.019203510495762285 -0.085618466735949181 0.059088545258519268 0.0078933508743804573 0.037809920295536606 0.0063074949195453308 -0.17902201588564792 -0.1307027547139536 -0.023023148977446957 0.039783180832449104 -0.14128049582445348 0.10234998043734644 0.0090280797875413234 0.062921441869126793 0.006147782712668152
-0.10130754803357367 -0.07046171928648827 -0.012586017213071123 0.019015448790432432 -0.084767342205032042 0.058498964492245682 0.0078165770317406933 0.037433693418042077 0.0062463917487940003 -0.20956744422835819 -0.15194777134134443 -0.026817452589599135 0.045518097024653321 -0.16683831431615148 0.11998795385251554 0.011384743609812422 0.074207750518145349 0.0080317426809297689
-0.10030238107483322 -0.069762919234439086 -0.012456854398391658 0.018842363812125278 -0.083919272631581027 0.057914575253997569 0.0077382472243889304 0.037057893767409936 0.0061886305240407566 -0.23980845668789455 -0.17298115325004518 -0.03057403387500315 0.051196003398394509 -0.19214116983885116 0.13744985344656438 0.013717953067043036 0.08538149189880298 0.0098967339617101451
-0.09929137717680335 -0.069063159697639612 -0.012339706914387513 0.018628612991921142 -0.083080149085847474 0.057336503895833273 0.0076588431584823065 0.036690041831801606 0.0061152081648894775 -0.2697478192651081 -0.19380517636794001 -0.034293102965045494 0.056818120643506637 -0.21719082808867268 0.15473735011688239 0.016027548531943162 0.096443428612796769 0.0117428634735276
-0.098305451820153683 -0.068378347698565253 -0.012213016308279097 0.01845928506017859 -0.082247681517417348 0.056763593445683658 0.0075814058876084783 0.036321196331405847 0.0060581985655958235 -0.29938818339316536 -0.21442180238978531 -0.03797521164732149 0.062384297386103592 -0.24198968659740433 0.17185217888386631 0.018313659468684265 0.10739469341608321 0.013569991298129441
-0.097324407851270472 -0.067697762204744868 -0.012091075668594438 0.018277852652965072 -0.081423996409197869 0.05619659255680852 0.0075043113830484346 0.035957518262439744 0.0059966187669349197 -0.32873268891300572 -0.23483319193106297 -0.041620602233278035 0.067895584126475769 -0.26654020419750896 0.18879605392622331 0.020576520732586191 0.11823632678180396 0.01537848189046446
-0.096351808129880367 -0.067023246380468901 -0.011971300232215064 0.018094459653289499 -0.080608684345088333 0.05563530645003191 0.007427881347577275 0.03559788366996524 0.0059342654418439453 -0.35778425380223833 -0.2550413725871149 -0.045229662319721711 0.073352437412908145 -0.29084486349598621 0.20557068325930572 0.022816362101858733 0.128969432572417 0.017168490526842543
-0.095391690774902282 -0.066356906198406243 -0.011850755423780956 0.017920632518532403 -0.079801256546093033 0.055079518502223347 0.0073524628000205304 0.035241007328194178 0.005875437133398425 -0.38654576569947591 -0.27504834873050632 -0.048802779252144601 0.078755295006975173 -0.31490612861832135 0.22217775864089445 0.025033413795319332 0.13959510739945957 0.018940168115629503
-0.094438283987377455 -0.065695687242967954 -0.011733445460030818 0.017740489090924603 -0.079002202611461694 0.054529394018248509 0.0072775672215087232 0.034888574366505234 0.0058142170787666217 -0.41502016942907699 -0.2948561495372935 -0.052340276245531538 0.084104828428519296 -0.33872643171979794 0.23861895174123923 0.027227911603922014 0.150114411559292 0.020693753265862095
-0.09349677548800539 -0.065042304111926463 -0.011615620248619739 0.017568778568175687 -0.078210911781531264 0.0539846717680681 0.0072036352726408317 0.034538947774994909 0.0057561011719709215 -0.44321028535744422 -0.314466733838975 -0.055842540680070094 0.089401436365884726 -0.36230819042406681 0.25489592127070754 0.029400080988186361 0.16052842370460682 0.022429383097785732
-0.092563134733509883 -0.064394619863379701 -0.011500052775227127 0.017394483632749775 -0.077427698935402939 0.053445450191880094 0.0071303253366386344 0.0341932851951696 0.00569698455729624 -0.47111898577745048 -0.33388208309230971 -0.059309899439033149 0.094645737303602442 -0.38565379016866091 0.27101030625905792 0.031550151938285559 0.17083818745029433 0.024147276703630979
-0.091638201672378072 -0.063753061203606937 -0.011386070645042929 0.017220197378951925 -0.076652391029200517 0.052911638566220574 0.0070577090033772244 0.033851268561362953 0.0056378336000888467 -0.49874907213044606 -0.3531041368228735 -0.062742705996814738 0.09983822570212679 -0.40876559674772134 0.28696373100908862 0.033678348533939355 0.18104474875567214 0.025847607020257145
-0.090725561571805891 -0.063119499930138753 -0.011271059773990191 0.017056183496389269 -0.075884540401184433 0.052383039795339988 0.0069860907824611717 0.033511748291924388 0.0055824727289923133 -0.52610329384293997 -0.37213480258902998 -0.066141327321374094 0.10497932392394951 -0.43164595467929651 0.30275780453623818 0.035784890537305755 0.191149150400482 0.027530520172737102
-0.089819104780775635 -0.062490705299568336 -0.0111592665301291 0.016885606460261798 -0.075124673148911567 0.051859835043388806 0.0069149526092565031 0.033176514090574767 0.0055246291866872296 -0.5531844572470459 -0.39097601335578425 -0.069506067038775807 0.11006968593227111 -0.45429717653825891 0.31839411649802096 0.037870002632849779 0.20115239915787314 0.029196248631887193
-0.088919547307814192 -0.061867039242927427 -0.011050108519851028 0.016710705705754399 -0.074372631287090901 0.051341940133807523 0.0068443555441815929 0.032845287334410965 0.0054651375585167356 -0.57999528158177116 -0.40962965174946347 -0.072837267550234799 0.11510979409807094 -0.47672155766868823 0.33387424297496332 0.039933902208943314 0.21105550962961325 0.030844960984227867
-0.088036150550069289 -0.061253361502996394 -0.010936957778640993 0.016557602204343363 -0.073627400165010931 0.050828938547917257 0.0067750856843791776 0.032515199795196098 0.005413730706159239 -0.60653838424881179 -0.42809754234188652 -0.076135320692436961 0.12009991727337481 -0.49892137792334357 0.34919974721448549 0.041976798106313017 0.22085950890086933 0.032476746310111583
-0.087159258992185074 -0.060644575496098599 -0.010826571835070834 0.016399542786057698 -0.072889864292960571 0.050321151273217644 0.0067063241086391129 0.032189117822336487 0.0053604405517984829 -0.63281652091481011 -0.44638157806650708 -0.079400498077884352 0.12504078115415215 -0.52089887782683064 0.36437217044499548 0.04399891098912443 0.23056536394581825 0.034091863961459225
-0.08628121190539513 -0.060036634691951925 -0.010724297539014289 0.016215347272798632 -0.072160708850014937 0.049818822136935385 0.0066374211428539432 0.031869303722409738 0.0052973755183077119 -0.65883240315663616 -0.46448362428381784 -0.082633080344284057 0.12993305790502094 -0.54265627712966702 0.37939303914604894 0.046000457872464054 0.24017403708376545 0.035690553395671905
-0.085420283234529784 -0.0594391428090753 -0.010617132282300131 0.016056319238962717 -0.07143801731399968 0.049321185169954332 0.0065699275538551082 0.031550159254672999 0.005243651522509031 -0.6845884896585559 -0.48240540892598266 -0.085833505115181258 0.13477678186989084 -0.56419579601385306 0.39426387334718821 0.047981634405818499 0.24968654945021845 0.037272816469109793
-0.084571031650916992 -0.058849221730717195 -0.010508854029010688 0.015907477018287611 -0.070722253812695771 0.048828398823458616 0.0065033819703790045 0.031233275185037979 0.0051936865573156479 -0.7100874200730577 -0.50014875089195177 -0.089002060238900241 0.13957256150100306 -0.58551961180478973 0.4089861698887764 0.049942651683732145 0.2591038496662505 0.038838869002716651
-0.08372425098587262 -0.058262011945203161 -0.010405905291596311 0.015743314149223451 -0.070014261094614505 0.048340768750843553 0.0064370035893650162 0.030921384038168381 0.0051379770702163841 -0.73533184105297222 -0.51771546852580408 -0.092139007097162162 0.14432109244528532 -0.60662987667614932 0.4235614093604978 0.051883721484881448 0.2684268667470539 0.040388959357739965
-0.082890595217752966 -0.057683107001195645 -0.010300677160589636 0.015593830005896397 -0.069312889759339441 0.047857827439423829 0.0063716966519173192 0.030611199043533443 0.0050877010196461502 -0.76032420019079971 -0.5351072712033873 -0.095244726618936598 0.14902258093983387 -0.62752873857277791 0.43799106428849521 0.053805038788218303 0.27765657273648237 0.041923153592566383
-0.082062619207454335 -0.057108574539667459 -0.010198367541421416 0.01543843565325353 -0.068618807451949093 0.047379812991172436 0.0063068297671294285 0.030304920088027102 0.0050351871270291147 -0.7850670777759835 -0.5523259339863773 -0.098319484756699604 0.153677670387313 -0.64821830816124648 0.45227658636903628 0.055706809920595776 0.2867938823200607 0.043441680705373964
-0.08124375404816242 -0.05654020678683707 -0.010096479511706951 0.015286938462653178 -0.067931591185984797 0.046906540319474624 0.0062426943427241052 0.03000145089597582 0.0049840897976267959 -0.80956293400392942 -0.56937316461269616 -0.10136361156025454 0.15828673415609446 -0.6687006844372515 0.46641941647295915 0.057589231108053772 0.29583972962062688 0.044944669131044925
-0.080433222831066672 -0.055977581320657291 -0.0099954942303436944 0.015137382484611038 -0.067251241034564882 0.046437988783226754 0.0061792247543052924 0.029700971255683811 0.0049336804157604081 -0.83381424095861201 -0.58625067300306055 -0.10437740800021646 0.16285024380914409 -0.68897794171868421 0.48042097965886188 0.059452499673447949 0.30479502836226735 0.04643228394352867
-0.07962916490062627 -0.055419705447811739 -0.0098966603501107562 0.014984780601273754 -0.066577866604276753 0.045974180469520766 0.0061162634246432071 0.029403990010444202 0.0048821009505284427 -0.85782345557637485 -0.60296015707131567 -0.10736116530386329 0.1673686928901969 -0.70905213259381095 0.49428268637966555 0.06129681174203562 0.31366068023326465 0.04790469845855358
-0.078835477340095106 -0.054868617057499985 -0.0097971532877799035 0.014840236143643966 -0.065911000486035889 0.045514913663559566 0.006054141293552623 0.029109271307628422 0.0048334886829030132 -0.88159296014436483 -0.6195032714469928 -0.11031520725994207 0.17184242997740753 -0.72892529412499296 0.50800593495240509 0.063122357192520198 0.32243759308805631 0.049362032029334285
-0.078049145456413282 -0.054322716318647594 -0.0096990395882857287 0.014695532278008206 -0.065250869629981237 0.045060256412252024 0.0059926028654176261 0.028817674891870908 0.0047847821878763668 -0.90512517318775798 -0.63588168587760885 -0.11323981196210724 0.17627196797119538 -0.74859943695161335 0.5215921073886437 0.064929329064834243 0.3311266475004227 0.050804465287446678
-0.077267479219871898 -0.05378057440854548 -0.0096041567434393665 0.01454335749898656 -0.064597669162941712 0.044610264607928211 0.0059314201891869384 0.028529962638411028 0.0047332573664141094 -0.92842248651414172 -0.65209705213248148 -0.11613525655359859 0.18065780750677934 -0.76807655183576973 0.53504257199580574 0.066717918218125699 0.3397287160091042 0.052232174346238322
-0.076497014988677628 -0.053245618725066925 -0.009507814731082774 0.014402185506929874 -0.063950669095063473 0.044164635197295528 0.0058711481304487105 0.028244100316467544 0.0046857977040683065 -0.95148718771242624 -0.66815096325049395 -0.1190018722616402 0.18500021971914871 -0.78735861766532822 0.54835868654367947 0.068488306754626235 0.34824468677072101 0.053645249858923945
-0.075737719328861336 -0.052717818937292944 -0.0094099726649668307 0.01427212572793842 -0.063309799454970839 0.043723321551243428 0.0058117841352212426 0.027960045616367411 0.0046424441457712049 -0.97432159833827492 -0.68404502630900765 -0.12183994681990945 0.18929963184688431 -0.80644758781061654 0.56154179302690432 0.07024067974357151 0.35667542183361578 0.055043840694827019
-0.074977090981127703 -0.052190582693141467 -0.00931941196745684 0.01411843863884637 -0.062676242148472103 0.043286763230772178 0.0057522678078575588 0.027681535916618275 0.004590252895098494 -0.99692812565583278 -0.69978088969478613 -0.12464968805268256 0.19355677123645465 -0.82534538515349787 0.57459321579361267 0.07197522961075549 0.36502174161234857 0.056428207595474317
-0.074223977672831329 -0.051668554302265648 -0.0092298091296132596 0.013966052015665007 -0.062049037442718315 0.042854568574467088 0.0056933501184034908 0.027405837952757472 0.00453851223039401 -1.0193089675439722 -0.71536008782584259 -0.12743143286199735 0.19777183996554148 -0.84405393191907141 0.58751427318435778 0.073692131100625669 0.37328451449105504 0.057798415638699863
-0.073487967962877307 -0.051156768116773395 -0.0091343170952663515 0.013841962233461862 -0.061427151982057407 0.042426317529440434 0.0056358448915314928 0.027129994418325878 0.0044972811913220829 -1.0414662926981169 -0.73078413603357184 -0.13018551991542621 0.20194501789879879 -0.86257513180938739 0.60030627065343489 0.075391556565113593 0.38146460235682217 0.059154521677540234
-0.072753449817725011 -0.05064699487123054 -0.0090439202229316874 0.013702729357244314 -0.060812081996140306 0.0420025722514554 0.005578427791130232 0.026858687150697766 0.0044503742021749677 -1.063402467405685 -0.7460546499574463 -0.13291212937859129 0.20607709592040346 -0.88091084746134929 0.61297049207442245 0.077073695204032497 0.38956279201443739 0.060496810283082111

48
SOURCE/circleDataOutMotion.data Normal file
View File

@ -0,0 +1,48 @@
# Created by Octave 4.4.1, Tue Jul 23 08:17:46 2019 WIB <adnr@morilin.fedo>
# name: y
# type: matrix
# rows: 41
# columns: 21
1.5 1.7 2 2.5 2.5 2.7999999999999998 2.5 2 2.2000000000000002 1 1.5 0.20000000000000001 0 0 0 0 0 0 0 0 0
-0.43048669459415201 0.36608547128153784 -0.00274340997035172 2.2036081728581323 1.6367933159008652 2.9869878079154888 1.2544878861930742 1.2945193648661997 1.3556863339134229 -0.25292255363669053 -0.28435587569906517 -1.3719157886385323 0.12631227950736779 0.20547645709384318 0.14409722585169149 -0.013440667885702993 0.030639553634200917 -0.19973824312305685 -0.045265405564663699 -0.25783517766765779 -0.12418546989735015
-1.9681776323507973 -0.28432242922549233 -1.5001795366193131 1.534298459953251 0.15493530341690115 2.2784066893646235 -0.2607702260097014 0.5971172907511676 -0.18567493732665158 -0.9444295209740533 -1.8345881102570369 -2.0378806058024641 0.28884287759903909 0.41457678108866275 0.32279402561594328 0.0075031276256240162 0.11976427408286253 -0.37651685591467132 -0.043833221566996254 -0.44019230144195715 -0.21516636472344586
-3.5752085981502315 -0.74656151389622338 -3.0095199784102147 1.0379514400560226 -1.3170488235393407 1.6874447564011932 -1.8212908336785081 0.032607253216396653 -1.828261483241441 -1.5078618528249126 -3.5290180073354982 -2.5123001438821109 0.44331630506937481 0.62092920530265228 0.50985128928885626 0.039819232585568735 0.20660987720595511 -0.55095524409606278 -0.045115956883930559 -0.62716366891309216 -0.31202757130731085
-5.2276776518565127 -1.0281335753651402 -4.5333268969429694 0.70642935955840946 -2.7985560756568226 1.2269742015157721 -3.4209069417878206 -0.38577878933416787 -3.5419369557983273 -1.9210994590283195 -5.3111117747473999 -2.7962662554029047 0.5923288722287382 0.82479970618698562 0.70057853658873048 0.078133449384510348 0.29165018054872549 -0.72299938808840092 -0.047937931457146477 -0.81559114450089798 -0.41154246654684867
-6.9032611102512353 -1.1324942862395682 -6.0668709078470515 0.53433031951665477 -4.2948307524879805 0.9047587236204252 -5.0498560772278154 -0.65005307885702479 -5.3019950795248727 -2.1697584346032968 -7.1396964927047044 -2.8891834230529647 0.73760137313442653 1.0265687993981205 0.89255751037832898 0.11949079967116057 0.37526406480348412 -0.89285444886731014 -0.051629474782076711 -1.0038967394995875 -0.51202705446559038
-8.582770791708354 -1.0632725347638099 -7.6006067539886955 0.51900702662873011 -5.8028636070161053 0.72575352928721493 -6.6946464917148631 -0.7547707958623876 -7.0862073494384887 -2.2457714529904402 -8.9830878230764117 -2.7912075956601652 0.88015502748743191 1.2264793142618797 1.0844987735364737 0.1622813091928807 0.45772191919388427 -1.0607238588449406 -0.055804084390416571 -1.1912904322132976 -0.61258508794401745
-10.248145123458425 -0.82371346556928227 -9.1226260808254835 0.65817908361824329 -7.3143375686721965 0.69317413801391758 -8.3403520681016801 -0.697303716195662 -8.8739701498435331 -2.1453326683703025 -10.81524902546508 -2.5043859359541045 1.0206027585465176 1.424698812862796 1.2757035139275292 0.20561211330483511 0.53919297420576418 -1.226756523362543 -0.060243804790855879 -1.3773575232185016 -0.71272897768421306
-11.881813297718081 -0.41741077883859012 -10.619919685119593 0.9490351878573059 -8.8178293638885226 0.80850222223513357 -9.9715406158589932 -0.47741790863382949 -10.645720655895403 -1.8677707464791777 -12.613616437642076 -2.0326025831706938 1.1593087950339334 1.6213377587021365 1.4657925550556283 0.2489795267861119 0.61977511924303896 -1.3910496089078075 -0.064830564168817453 -1.5618784042486735 -0.81219444878602409
-13.466864668132125 0.15091076882204019 -12.078905454422161 1.3879298010965746 -10.300281551923849 1.0714250066801863 -11.572546379927887 -0.096807541335834552 -12.382707315893709 -1.4149543371257618 -14.357881902864568 -1.3814139860889878 1.2964891183618446 1.8164677462309926 1.654560232130686 0.29209985532885263 0.69952634354968313 -1.5536666134036283 -0.069499131640954406 -1.7447383884762919 -0.91083790370102946
-14.985448041804901 0.87559546980755332 -13.486698669371449 1.9697744374511938 -11.747379391027696 1.4799811376900978 -13.128812819742882 0.4409569660788078 -14.066886299058568 -0.79067600887481626 -16.029689020522802 -0.55827620393133082 1.4323141023872825 2.0101782024379089 1.8419023336806082 0.33479439912849474 0.77846875552845163 -1.7146510188448532 -0.074227774100264682 -1.9258666053480906 -1.0085844028027877
-16.426885569552041 1.7478007738002161 -14.827413328722628 2.6896839151550358 -13.146760848861705 2.0300850506774886 -14.622030795860473 1.1311266368545443 -15.681062970942159 -0.0016430707654821824 -17.610687532860769 0.42910851018052215 1.5667634866725617 2.2023981503226504 2.0277371153856496 0.37704305424458506 0.85666959110918117 -1.8740440489639962 -0.078944683643155059 -2.1052703611152435 -1.1053842463338579
-17.769419778552393 2.7598756355329659 -16.091577894772467 3.5390968801337177 -14.48248611673784 2.7167717682580164 -16.041932512876755 1.9669161449281294 -17.208743581304574 0.94552134564154977 -19.086182428104518 1.5692763243362804 1.7000444204288199 2.3933142293473617 2.2120737644830584 0.41870578837901667 0.93408560130613383 -2.0318585133344502 -0.083706418212191713 -2.2828921470470904 -1.2012284317028428
-19.015720016918614 3.9012401300810282 -17.258660612933301 4.5113788784356741 -15.745769556811545 3.533231715081476 -17.364636781129473 2.9388660882221846 -18.634932491557741 2.0390531528421163 -20.438113255445742 2.8528072984551689 1.8319175489536075 2.5826464212680551 2.39478505136612 0.46000606658984838 1.0108854779637781 -2.1881805150523008 -0.088316604407630825 -2.4588042342747407 -1.2960640399768342
-20.134759218565883 5.1608232586213143 -18.326460545809454 5.5947638156907136 -16.919176855153143 4.4714126864349586 -18.588230357419032 4.0387657290211569 -19.944668287650504 3.2706988535699244 -21.656306876340587 4.2643160823365838 1.9626789073801971 2.7707062315080306 2.5760113900180159 0.50064632911928086 1.0868951895887184 -2.3429389589387362 -0.092990674161685258 -2.6329496919931623 -1.3899529901946357
-21.121475371098317 6.5254028136112039 -19.281293980747495 6.7792575170878555 -17.99269667072954 5.5218849502371148 -19.696740865776754 5.2555523829303414 -21.12498176250358 4.6273738538038529 -22.727403490651671 5.7904125527236738 2.0922590706529109 2.9574130340302669 2.7557183387529731 0.54071240669729637 1.1621821984728749 -2.4961761444473578 -0.097657931336487591 -2.8053660881435158 -1.4828784148308924
-21.967700729386344 7.981674710037173 -20.112819912494746 8.0531118635117807 -18.955984632268457 6.6742237097287385 -20.678139618564234 6.5767840290467285 -22.164060599328558 6.0952671610413649 -23.640400431666404 7.4158998252775081 2.2206484233914296 3.142752473542068 2.9339026287457206 0.58022426214834277 1.2367646159864953 -2.6479156083222959 -0.10230333417128949 -2.9760759121476283 -1.5748449175511898
-22.66501959033425 9.5152806790671605 -20.812725765415443 9.4035235107294888 -19.799436349587388 7.9169556268576038 -21.522629820279199 7.9892030280105057 -23.051490031003706 7.6597250324431325 -24.38614323190026 9.1244225802989813 2.3478588705623507 3.3267340611347094 3.1105740468145266 0.6191864524244407 1.3106501507459467 -2.7981729641850808 -0.10692484127788132 -3.1450950394393229 -1.665859560712863
-23.206563180621067 11.111139327818574 -21.373968776509056 10.816887057003086 -20.51467487864462 9.2377284891354172 -22.221745477567822 9.4785993496300893 -23.77839545999457 9.3051379466106692 -24.957116880342799 10.898791295001516 2.4738953528794752 3.5093602873428096 3.285743319610047 0.65760833497018234 1.383848499028179 -2.9469625143042104 -0.11151818530028514 -3.3124410889240785 -1.7559292066555088
-23.582147867561119 12.74992759282194 -21.793719081697954 12.280217731431735 -21.093364154220637 10.622449572208941 -22.771006169605432 11.032659961346752 -24.336802863139603 11.015639610313995 -25.349006965548384 12.721271091268251 2.5989293877223978 3.6908150989796398 3.4594449507146905 0.69541906894753258 1.4563217566294522 -3.0942998063770162 -0.11613569825672836 -3.4780839457622479 -1.8450656266972978
-23.80372387841112 14.428442479835999 -22.058511875218286 13.774481292998784 -21.533339350417823 12.060181111935693 -23.156808676519443 12.62656176358535 -24.723365754833473 12.772142751403747 -25.553730711458464 14.573288275043495 2.7225660692473346 3.8706604327783949 3.6315658280291769 0.7328988288660867 1.528235887319529 -3.2402549269292411 -0.12057920117672584 -3.6421460151346823 -1.933244649786078
-23.843654115024837 16.106435287254861 -22.180996357047359 15.292897106216989 -21.824692532070959 13.53164825187009 -23.388307963587586 14.260746563673509 -24.930269726222701 14.56025891732887 -25.575824920188587 16.437046111116409 2.8454688768603535 4.0496194256234332 3.8023206773727343 0.76963520171525635 1.5993524476199112 -3.3847509301934426 -0.1251518991359433 -3.804466599473987 -2.0205232215848392
-23.718239090810489 17.78208908115073 -22.149929671234283 16.815389779089756 -21.968145564892296 15.02518254419433 -23.454568765427428 15.908545378332073 -24.957927760337824 16.359752913114487 -25.410897313587036 18.293553127666396 2.9671503780099502 4.2271438930645262 3.9715915178034353 0.80591071032969286 1.669837791086684 -3.5278335623370722 -0.1296511106927612 -3.9651856478624619 -2.1068886037880792
-23.429434332730878 19.440747434961057 -21.965072281685167 18.325431246045252 -21.962443613556282 16.52662419729641 -23.35469614990199 17.552174690468945 -24.806421811145384 18.153123565081877 -25.060139388680835 20.12356676126106 3.08768263748853 4.403349473703047 4.1394002509768884 0.84171560301493176 1.7396916738468928 -3.6695509588663966 -0.13409597837479031 -4.1243417982673067 -2.1923430539580675
-22.977831483479079 21.065290180530969 -21.629611885947561 19.808275701822563 -21.806982622484689 18.02027284949402 -23.090879397477853 19.175948194248921 -24.476756761765241 19.92242792902109 -24.527987693605745 21.908993828355399 3.2070270111569004 4.5781732580405947 4.3057878824212832 0.87699882423435538 1.8088872989577276 -3.8098590342740093 -0.13851846938474546 -4.281942239708509 -2.2769141045028998
-22.365056383882429 22.633307255914321 -21.148886551598871 21.251673395898568 -21.502728647958424 19.48969857794533 -22.666840623601828 20.767585946714508 -23.971368392487786 21.650272819791024 -23.821018716364303 23.632883253488071 3.3253238992175467 4.7517423284275289 4.4707599713398363 0.91176534893261674 1.8774306196310604 -3.9487783972562527 -0.14291438187343936 -4.4379360927285321 -2.3605897472241306
-21.597090804365799 24.122271746629579 -20.528582741718637 22.644508383450937 -21.052864124221429 20.919247725542274 -22.086365042327206 22.315283896635368 -23.29451525588486 23.319087993466397 -22.946654251710793 25.278743064550504 3.4428138249776086 4.924359685606829 4.6343768701038206 0.94589074519832894 1.945250369727854 -4.0863486876305446 -0.14738778997677968 -4.5923241093491249 -2.4434074713619278
-20.688909730695425 25.55501517904607 -19.767039153351742 23.9554066625346 -20.463605031319688 22.303549444697349 -21.354458003811445 23.780366868483782 -22.458198945282479 24.912596599577562 -21.908022893394069 26.823777817299796 3.5586913588489986 5.0951118231900834 4.7964327012400609 0.97993762350820013 2.0126670224405672 -4.2226528698410828 -0.15154435650647269 -4.7453202824661682 -2.5252944333550942
-19.64014648367456 26.869860449312704 -18.881295296153024 25.193376542933283 -19.738548417610254 23.616770577682573 -20.47943055638272 25.177402264787144 -21.464581992001015 26.414092440394228 -20.722999032434195 28.261385299951876 3.6737826146665902 5.2649081341685458 4.9572155121239003 1.0132354862775925 2.0793007400186831 -4.3575568712410053 -0.15586411786870372 -4.8967307102176845 -2.6063827171954461
-18.465939293217478 28.077858772340161 -17.876618276249324 26.33474845961274 -18.886000188850353 24.852591442061467 -19.469516348428407 26.477856372489473 -20.325565560479323 27.809210953357759 -19.399821339603207 29.571448399325604 3.7877348778336879 5.4333513454161508 5.1166256599963242 1.0461277036754777 2.1453543099152861 -4.4911145439374964 -0.16009659168324114 -5.0466272176930449 -2.6865844800889214
-17.175456667950591 29.168348364408931 -16.76251903371104 27.3673546948955 -17.914181957111573 25.99788678793881 -18.336331242772875 27.667889009107476 -19.054077614997549 29.083801698649857 -17.952026156631675 30.742067456582088 3.900609818469166 5.6005382308768921 5.2746326426940282 1.0785648760250599 2.2107975237400792 -4.6234169325987544 -0.16428776674302689 -5.1950702095749302 -2.7659594854538136
-15.790208305479679 30.098970443923253 -15.550918175704449 28.296510442016327 -16.834481508805219 27.0340444827896 -17.082889738567431 28.753534431670936 -17.657583521525552 30.223711978881621 -16.396543990058749 31.768037807949057 4.0130633727426881 5.7671909601344096 5.4314316949678414 1.1101797940354514 2.2754448521429631 -4.7543850095794857 -0.16868737373185344 -5.3418717505690232 -2.8445091185978058
-14.305273845486031 30.924638346862995 -14.254150253620297 29.080514588917236 -15.653747704035816 27.966260038326187 -15.739406202661808 29.687640079358744 -16.159445791214512 31.22134879253008 -14.744288923865556 32.624273876153431 4.1236325581890396 5.9316577310405263 5.5866773802861882 1.1418723227518954 2.3397684930088181 -4.8841538452160407 -0.17269799082406895 -5.4874554754423857 -2.9221964758906482
-12.758298160116171 31.570440360132999 -12.883059441881208 29.745920346500668 -14.388599429191151 28.769577377221911 -14.298989414411242 30.498423820987835 -14.565457082972692 32.061853456277618 -13.015744266238995 33.31747679461435 4.2337392065016513 6.0955338766335432 5.7407694465225925 1.1727163631996123 2.4032791124047002 -5.0125652199158877 -0.17694499719481238 -5.6314348041092037 -2.9991057267148133
-11.156242153928883 32.058487202332714 -11.451787920967185 30.271412422862795 -13.049623441104064 29.4428611249764 -12.783878393193335 31.161440353351054 -12.89539363931779 32.738927453452675 -11.2265920361447 33.834177312765867 4.3430142149536852 6.2584215347294627 5.893538958330093 1.2030728785579412 2.4661806088670621 -5.1397781427158202 -0.18118659961112943 -5.7739652279841778 -3.075205295962935
-9.4959973580065871 32.407502811293277 -9.9805990986162385 30.638239576022187 -11.646664419323697 29.983773407950942 -11.222755432473466 31.65804036879361 -11.168229703193607 33.248856038615195 -9.3955645182783787 34.16369804462439 4.4507584711542467 6.419586394709845 6.0449374838454641 1.2331591188526401 2.5285953207864247 -5.2657889441409642 -0.18527576483151292 -5.9152865400143062 -3.1505149380765087
-7.8220530736452023 32.57372394349283 -8.4757811025133947 30.86532698027284 -10.199538090196844 30.378460953144803 -9.610633484161502 32.004350257993956 -9.3973669554264774 33.581529249088504 -7.5401105743760555 34.312807460143432 4.5579134261132648 6.5800233181860479 6.1951700432986465 1.2624628867001861 2.5902509561741125 -5.3905140932269973 -0.18955855354765055 -6.0551165014372303 -3.2250974952430584
-6.1385726170050523 32.573600423237515 -6.9562744718612581 30.940205878593417 -8.7200587269285048 30.626922415419394 -7.9728388831125008 32.187372027771382 -7.6021145679594095 33.735928726066106 -5.6792376662363662 34.275394766112122 4.6640209083517385 6.7392347338008225 6.3441552880219154 1.2912659431733622 2.6513019630936299 -5.513998380896326 -0.19384790905935842 -6.193603323618194 -3.2989224276282458
-4.4602467585131187 32.408517226110796 -5.4384767347748442 30.861340315151512 -7.2222188105781262 30.727087076009941 -6.3271242689597331 32.204804352611355 -5.7999286068244 33.710737334983804 -3.8323370632190863 34.051073059506443 4.7690779860696457 6.8972167182980142 6.4918790776744855 1.319663590948968 2.711799559746515 -5.6362808448271515 -0.19808750751293944 -6.3307549613893714 -3.3719598020031043
-2.8123190670953311 32.07275845594107 -3.9334934126109729 30.633797405741014 -5.723609361578637 30.676185441070043 -4.68406729924041 32.059684408490284 -4.0095789740461862 33.504918528331331 -2.015925529984965 33.645015503649319 4.8732940976525754 7.0541697579656617 6.6383777069877166 1.3474898880979063 2.7716549698011312 -5.7573441462558668 -0.20238638745551785 -6.466519177133077 -3.4442713993814302
-1.2001262801229706 31.577991391356047 -2.4616171883947797 30.254941063642278 -4.2358153386016397 30.476741925751504 -3.067512601282238 31.74999982598731 -2.2480683808945217 33.12199095223562 -0.25087225119527073 33.057860788130895 4.976409839733468 7.2098412279254696 6.7836298684182177 1.3749666660924436 2.8309917581264683 -5.8772327022112814 -0.20660570734462966 -6.6009961278195224 -3.515800494631844

40
SOURCE/fncSpeedRef.m Normal file
View File

@ -0,0 +1,40 @@
function fncSr = fncSpeedRef(param,r,v)
sp = [1, 2]; % 1 = x; 2 = y
if (length(param) == 4)
% 'xsin' / 'xcos'
strSinOcos = strcat(sprintf("%s", param(2:length(param))));
if (param(1) == 'x')
strSfx = strcat(sprintf("%i*%s(%i*t)",r,strSinOcos,v));
strSfy = strcat(sprintf("%i",r));
% 'ysin' / 'ycos'
elseif (param(1) == 'y')
strSfx = strcat(sprintf("%i",r));
strSfy = strcat(sprintf("%i*%s(%i*t)",r,strSinOcos,v));
endif
elseif (param(1) == 'c')
if(param(2) == 'c')
%'circle'
strSfx = strcat(sprintf("%i*cos(%i*t)",r,v));
strSfy = strcat(sprintf("%i*sin(%i*t)",r,v));
else
strSfx = strcat(sprintf("%i*sin(%i*t)",r,v));
strSfy = strcat(sprintf("%i*cos(%i*t)",r,v));
endif
elseif (param == 's')
strSfx = strcat(sprintf("%i",r));
strSfy = strcat(sprintf("%i",v));
else
printf("Karepmu opo ? \n\n bukak fungsi speedRef sek diwoco komen e \n\n");
endif
strAll = strcat("fncSr = @(t) [",strSfx, ";\ ", strSfy, "];");
%% printf(strAll)
eval(strAll);
%% spx = sfx(t);
%% spy = sfy(t);
endfunction

BIN
SOURCE/octave-workspace
View File

Binary file not shown.

8
SOURCE/rigidityMatrixFnc.m
View File

@ -6,11 +6,13 @@ function [R,K, d] = rigidityMatrixFnc (edgeL)
% generasi koneksi bentuk incidence
tmp = edgeL2adj(edgeL);
d = ones(numEdges(tmp),1);
printf("Jumlah Edge : %i \n", length(d));
gInc = adj2inc(tmp);
%% drawCircGraph(tmp)
tmp = getNodes(gInc,'adjacency')
tmp = numNodes(adj2adjL(tmp));
printf("Jumlah Robot : %i \n", tmp);
% generasi vector error menggunakan matrix
% koneksi
tmp = getNodes(gInc,'adjacency');
K = kron(adj2inc(edgeL2adj(edgeL))',eye(2));
edgeVector = @(x,k) k*x;
% hasil turunan dari fungsi edge
@ -20,7 +22,7 @@ function [R,K, d] = rigidityMatrixFnc (edgeL)
endfor
str = strcat(str,sprintf("errVec(%i:%i)' ));",i+2,i+3));
% return sebagai fungsi R
eval(str)
eval(str);
R = @(x,k) errBlockDiagonal(edgeVector(x,k))*k;
endfunction

48
SOURCE/sinDataErrorEdge.data Normal file
View File

@ -0,0 +1,48 @@
# Created by Octave 4.4.1, Tue Jul 23 08:22:36 2019 WIB <adnr@morilin.fedo>
# name: yans
# type: matrix
# rows: 41
# columns: 18
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
-0.096911478315354516 -0.075730185298846384 -0.012818693778102748 0.02494319515544317 -0.071455460399853357 0.058061517393821691 -0.00046049687370259293 0.031894441863279527 -0.00020103212281644091 -0.023675953022652619 -0.020957934876851849 -0.0036309652191249529 0.0093563014520160383 -0.013149718234368087 0.01202648390515818 -0.0012963378978132847 0.0062554711548290943 -0.0022820491653563553
-0.10425265654265486 -0.074438472824579799 -0.013020183069880772 0.020937547114184728 -0.085196296071277988 0.061012542456736242 0.0061415215844350711 0.037731753417632005 0.0051921291614990791 -0.054166923794470918 -0.043490768870371808 -0.0075028693060688115 0.016084571794996859 -0.037198913299163625 0.030088371215826977 -0.00023775623244392841 0.016935638830019823 -0.0013201073157883807
-0.10496977223679899 -0.073423053303207697 -0.013052667938678717 0.019981714893807078 -0.087406749208225401 0.060803658484664999 0.0076919183959292259 0.038627371912961526 0.0062162636973524497 -0.085616578743486924 -0.065661095427974769 -0.011418819655041261 0.02219524096238892 -0.063204202538559573 0.048378181459469968 0.0018967953279961551 0.028437807258383522 0.00043298687365519631
-0.10430390951433496 -0.07262905261624597 -0.012956881974499325 0.019642588586123517 -0.087186661092282874 0.060268414355689706 0.0079645869608754253 0.038506981507778149 0.0063842529304324949 -0.1170217651945822 -0.087566442340655359 -0.015321451258742397 0.02813360911223722 -0.089417611490451535 0.06654164850612454 0.0042584180904954331 0.040017904201910121 0.0023318424946289448
-0.10334147914108427 -0.071890644056646885 -0.012837287263367293 0.01941026499079718 -0.086453982873853016 0.059681403532539722 0.0079584985700906045 0.038179070629303639 0.0063652323550624761 -0.14817120045783436 -0.10924360269998434 -0.01919095456577553 0.033989838629414508 -0.11546877580916949 0.084534540689276488 0.0066496709541252093 0.051522904026464054 0.0042459112311731603
-0.10232423391632046 -0.071170339812851133 -0.01271312996813039 0.019203510495762285 -0.085618466735949181 0.059088545258519268 0.0078933508743804573 0.037809920295536606 0.0063074949195453308 -0.17902201588564792 -0.1307027547139536 -0.023023148977446957 0.039783180832449104 -0.14128049582445348 0.10234998043734644 0.0090280797875413234 0.062921441869126793 0.006147782712668152
-0.10130754803357367 -0.07046171928648827 -0.012586017213071123 0.019015448790432432 -0.084767342205032042 0.058498964492245682 0.0078165770317406933 0.037433693418042077 0.0062463917487940003 -0.20956744422835819 -0.15194777134134443 -0.026817452589599135 0.045518097024653321 -0.16683831431615148 0.11998795385251554 0.011384743609812422 0.074207750518145349 0.0080317426809297689
-0.10030238107483322 -0.069762919234439086 -0.012456854398391658 0.018842363812125278 -0.083919272631581027 0.057914575253997569 0.0077382472243889304 0.037057893767409936 0.0061886305240407566 -0.23980845668789455 -0.17298115325004518 -0.03057403387500315 0.051196003398394509 -0.19214116983885116 0.13744985344656438 0.013717953067043036 0.08538149189880298 0.0098967339617101451
-0.09929137717680335 -0.069063159697639612 -0.012339706914387513 0.018628612991921142 -0.083080149085847474 0.057336503895833273 0.0076588431584823065 0.036690041831801606 0.0061152081648894775 -0.2697478192651081 -0.19380517636794001 -0.034293102965045494 0.056818120643506637 -0.21719082808867268 0.15473735011688239 0.016027548531943162 0.096443428612796769 0.0117428634735276
-0.098305451820153683 -0.068378347698565253 -0.012213016308279097 0.01845928506017859 -0.082247681517417348 0.056763593445683658 0.0075814058876084783 0.036321196331405847 0.0060581985655958235 -0.29938818339316536 -0.21442180238978531 -0.03797521164732149 0.062384297386103592 -0.24198968659740433 0.17185217888386631 0.018313659468684265 0.10739469341608321 0.013569991298129441
-0.097324407851270472 -0.067697762204744868 -0.012091075668594438 0.018277852652965072 -0.081423996409197869 0.05619659255680852 0.0075043113830484346 0.035957518262439744 0.0059966187669349197 -0.32873268891300572 -0.23483319193106297 -0.041620602233278035 0.067895584126475769 -0.26654020419750896 0.18879605392622331 0.020576520732586191 0.11823632678180396 0.01537848189046446
-0.096351808129880367 -0.067023246380468901 -0.011971300232215064 0.018094459653289499 -0.080608684345088333 0.05563530645003191 0.007427881347577275 0.03559788366996524 0.0059342654418439453 -0.35778425380223833 -0.2550413725871149 -0.045229662319721711 0.073352437412908145 -0.29084486349598621 0.20557068325930572 0.022816362101858733 0.128969432572417 0.017168490526842543
-0.095391690774902282 -0.066356906198406243 -0.011850755423780956 0.017920632518532403 -0.079801256546093033 0.055079518502223347 0.0073524628000205304 0.035241007328194178 0.005875437133398425 -0.38654576569947591 -0.27504834873050632 -0.048802779252144601 0.078755295006975173 -0.31490612861832135 0.22217775864089445 0.025033413795319332 0.13959510739945957 0.018940168115629503
-0.094438283987377455 -0.065695687242967954 -0.011733445460030818 0.017740489090924603 -0.079002202611461694 0.054529394018248509 0.0072775672215087232 0.034888574366505234 0.0058142170787666217 -0.41502016942907699 -0.2948561495372935 -0.052340276245531538 0.084104828428519296 -0.33872643171979794 0.23861895174123923 0.027227911603922014 0.150114411559292 0.020693753265862095
-0.09349677548800539 -0.065042304111926463 -0.011615620248619739 0.017568778568175687 -0.078210911781531264 0.0539846717680681 0.0072036352726408317 0.034538947774994909 0.0057561011719709215 -0.44321028535744422 -0.314466733838975 -0.055842540680070094 0.089401436365884726 -0.36230819042406681 0.25489592127070754 0.029400080988186361 0.16052842370460682 0.022429383097785732
-0.092563134733509883 -0.064394619863379701 -0.011500052775227127 0.017394483632749775 -0.077427698935402939 0.053445450191880094 0.0071303253366386344 0.0341932851951696 0.00569698455729624 -0.47111898577745048 -0.33388208309230971 -0.059309899439033149 0.094645737303602442 -0.38565379016866091 0.27101030625905792 0.031550151938285559 0.17083818745029433 0.024147276703630979
-0.091638201672378072 -0.063753061203606937 -0.011386070645042929 0.017220197378951925 -0.076652391029200517 0.052911638566220574 0.0070577090033772244 0.033851268561362953 0.0056378336000888467 -0.49874907213044606 -0.3531041368228735 -0.062742705996814738 0.09983822570212679 -0.40876559674772134 0.28696373100908862 0.033678348533939355 0.18104474875567214 0.025847607020257145
-0.090725561571805891 -0.063119499930138753 -0.011271059773990191 0.017056183496389269 -0.075884540401184433 0.052383039795339988 0.0069860907824611717 0.033511748291924388 0.0055824727289923133 -0.52610329384293997 -0.37213480258902998 -0.066141327321374094 0.10497932392394951 -0.43164595467929651 0.30275780453623818 0.035784890537305755 0.191149150400482 0.027530520172737102
-0.089819104780775635 -0.062490705299568336 -0.0111592665301291 0.016885606460261798 -0.075124673148911567 0.051859835043388806 0.0069149526092565031 0.033176514090574767 0.0055246291866872296 -0.5531844572470459 -0.39097601335578425 -0.069506067038775807 0.11006968593227111 -0.45429717653825891 0.31839411649802096 0.037870002632849779 0.20115239915787314 0.029196248631887193
-0.088919547307814192 -0.061867039242927427 -0.011050108519851028 0.016710705705754399 -0.074372631287090901 0.051341940133807523 0.0068443555441815929 0.032845287334410965 0.0054651375585167356 -0.57999528158177116 -0.40962965174946347 -0.072837267550234799 0.11510979409807094 -0.47672155766868823 0.33387424297496332 0.039933902208943314 0.21105550962961325 0.030844960984227867
-0.088036150550069289 -0.061253361502996394 -0.010936957778640993 0.016557602204343363 -0.073627400165010931 0.050828938547917257 0.0067750856843791776 0.032515199795196098 0.005413730706159239 -0.60653838424881179 -0.42809754234188652 -0.076135320692436961 0.12009991727337481 -0.49892137792334357 0.34919974721448549 0.041976798106313017 0.22085950890086933 0.032476746310111583
-0.087159258992185074 -0.060644575496098599 -0.010826571835070834 0.016399542786057698 -0.072889864292960571 0.050321151273217644 0.0067063241086391129 0.032189117822336487 0.0053604405517984829 -0.63281652091481011 -0.44638157806650708 -0.079400498077884352 0.12504078115415215 -0.52089887782683064 0.36437217044499548 0.04399891098912443 0.23056536394581825 0.034091863961459225
-0.08628121190539513 -0.060036634691951925 -0.010724297539014289 0.016215347272798632 -0.072160708850014937 0.049818822136935385 0.0066374211428539432 0.031869303722409738 0.0052973755183077119 -0.65883240315663616 -0.46448362428381784 -0.082633080344284057 0.12993305790502094 -0.54265627712966702 0.37939303914604894 0.046000457872464054 0.24017403708376545 0.035690553395671905
-0.085420283234529784 -0.0594391428090753 -0.010617132282300131 0.016056319238962717 -0.07143801731399968 0.049321185169954332 0.0065699275538551082 0.031550159254672999 0.005243651522509031 -0.6845884896585559 -0.48240540892598266 -0.085833505115181258 0.13477678186989084 -0.56419579601385306 0.39426387334718821 0.047981634405818499 0.24968654945021845 0.037272816469109793
-0.084571031650916992 -0.058849221730717195 -0.010508854029010688 0.015907477018287611 -0.070722253812695771 0.048828398823458616 0.0065033819703790045 0.031233275185037979 0.0051936865573156479 -0.7100874200730577 -0.50014875089195177 -0.089002060238900241 0.13957256150100306 -0.58551961180478973 0.4089861698887764 0.049942651683732145 0.2591038496662505 0.038838869002716651
-0.08372425098587262 -0.058262011945203161 -0.010405905291596311 0.015743314149223451 -0.070014261094614505 0.048340768750843553 0.0064370035893650162 0.030921384038168381 0.0051379770702163841 -0.73533184105297222 -0.51771546852580408 -0.092139007097162162 0.14432109244528532 -0.60662987667614932 0.4235614093604978 0.051883721484881448 0.2684268667470539 0.040388959357739965
-0.082890595217752966 -0.057683107001195645 -0.010300677160589636 0.015593830005896397 -0.069312889759339441 0.047857827439423829 0.0063716966519173192 0.030611199043533443 0.0050877010196461502 -0.76032420019079971 -0.5351072712033873 -0.095244726618936598 0.14902258093983387 -0.62752873857277791 0.43799106428849521 0.053805038788218303 0.27765657273648237 0.041923153592566383
-0.082062619207454335 -0.057108574539667459 -0.010198367541421416 0.01543843565325353 -0.068618807451949093 0.047379812991172436 0.0063068297671294285 0.030304920088027102 0.0050351871270291147 -0.7850670777759835 -0.5523259339863773 -0.098319484756699604 0.153677670387313 -0.64821830816124648 0.45227658636903628 0.055706809920595776 0.2867938823200607 0.043441680705373964
-0.08124375404816242 -0.05654020678683707 -0.010096479511706951 0.015286938462653178 -0.067931591185984797 0.046906540319474624 0.0062426943427241052 0.03000145089597582 0.0049840897976267959 -0.80956293400392942 -0.56937316461269616 -0.10136361156025454 0.15828673415609446 -0.6687006844372515 0.46641941647295915 0.057589231108053772 0.29583972962062688 0.044944669131044925
-0.080433222831066672 -0.055977581320657291 -0.0099954942303436944 0.015137382484611038 -0.067251241034564882 0.046437988783226754 0.0061792247543052924 0.029700971255683811 0.0049336804157604081 -0.83381424095861201 -0.58625067300306055 -0.10437740800021646 0.16285024380914409 -0.68897794171868421 0.48042097965886188 0.059452499673447949 0.30479502836226735 0.04643228394352867
-0.07962916490062627 -0.055419705447811739 -0.0098966603501107562 0.014984780601273754 -0.066577866604276753 0.045974180469520766 0.0061162634246432071 0.029403990010444202 0.0048821009505284427 -0.85782345557637485 -0.60296015707131567 -0.10736116530386329 0.1673686928901969 -0.70905213259381095 0.49428268637966555 0.06129681174203562 0.31366068023326465 0.04790469845855358
-0.078835477340095106 -0.054868617057499985 -0.0097971532877799035 0.014840236143643966 -0.065911000486035889 0.045514913663559566 0.006054141293552623 0.029109271307628422 0.0048334886829030132 -0.88159296014436483 -0.6195032714469928 -0.11031520725994207 0.17184242997740753 -0.72892529412499296 0.50800593495240509 0.063122357192520198 0.32243759308805631 0.049362032029334285
-0.078049145456413282 -0.054322716318647594 -0.0096990395882857287 0.014695532278008206 -0.065250869629981237 0.045060256412252024 0.0059926028654176261 0.028817674891870908 0.0047847821878763668 -0.90512517318775798 -0.63588168587760885 -0.11323981196210724 0.17627196797119538 -0.74859943695161335 0.5215921073886437 0.064929329064834243 0.3311266475004227 0.050804465287446678
-0.077267479219871898 -0.05378057440854548 -0.0096041567434393665 0.01454335749898656 -0.064597669162941712 0.044610264607928211 0.0059314201891869384 0.028529962638411028 0.0047332573664141094 -0.92842248651414172 -0.65209705213248148 -0.11613525655359859 0.18065780750677934 -0.76807655183576973 0.53504257199580574 0.066717918218125699 0.3397287160091042 0.052232174346238322
-0.076497014988677628 -0.053245618725066925 -0.009507814731082774 0.014402185506929874 -0.063950669095063473 0.044164635197295528 0.0058711481304487105 0.028244100316467544 0.0046857977040683065 -0.95148718771242624 -0.66815096325049395 -0.1190018722616402 0.18500021971914871 -0.78735861766532822 0.54835868654367947 0.068488306754626235 0.34824468677072101 0.053645249858923945
-0.075737719328861336 -0.052717818937292944 -0.0094099726649668307 0.01427212572793842 -0.063309799454970839 0.043723321551243428 0.0058117841352212426 0.027960045616367411 0.0046424441457712049 -0.97432159833827492 -0.68404502630900765 -0.12183994681990945 0.18929963184688431 -0.80644758781061654 0.56154179302690432 0.07024067974357151 0.35667542183361578 0.055043840694827019
-0.074977090981127703 -0.052190582693141467 -0.00931941196745684 0.01411843863884637 -0.062676242148472103 0.043286763230772178 0.0057522678078575588 0.027681535916618275 0.004590252895098494 -0.99692812565583278 -0.69978088969478613 -0.12464968805268256 0.19355677123645465 -0.82534538515349787 0.57459321579361267 0.07197522961075549 0.36502174161234857 0.056428207595474317
-0.074223977672831329 -0.051668554302265648 -0.0092298091296132596 0.013966052015665007 -0.062049037442718315 0.042854568574467088 0.0056933501184034908 0.027405837952757472 0.00453851223039401 -1.0193089675439722 -0.71536008782584259 -0.12743143286199735 0.19777183996554148 -0.84405393191907141 0.58751427318435778 0.073692131100625669 0.37328451449105504 0.057798415638699863
-0.073487967962877307 -0.051156768116773395 -0.0091343170952663515 0.013841962233461862 -0.061427151982057407 0.042426317529440434 0.0056358448915314928 0.027129994418325878 0.0044972811913220829 -1.0414662926981169 -0.73078413603357184 -0.13018551991542621 0.20194501789879879 -0.86257513180938739 0.60030627065343489 0.075391556565113593 0.38146460235682217 0.059154521677540234
-0.072753449817725011 -0.05064699487123054 -0.0090439202229316874 0.013702729357244314 -0.060812081996140306 0.0420025722514554 0.005578427791130232 0.026858687150697766 0.0044503742021749677 -1.063402467405685 -0.7460546499574463 -0.13291212937859129 0.20607709592040346 -0.88091084746134929 0.61297049207442245 0.077073695204032497 0.38956279201443739 0.060496810283082111

48
SOURCE/sinDataOutMotion.data Normal file
View File

@ -0,0 +1,48 @@
# Created by Octave 4.4.1, Tue Jul 23 08:22:36 2019 WIB <adnr@morilin.fedo>
# name: y
# type: matrix
# rows: 41
# columns: 21
1.5 1.7 2 2.5 2.5 2.7999999999999998 2.5 2 2.2000000000000002 1 1.5 0.20000000000000001 0 0 0 0 0 0 0 0 0
2.1939976086506423 2.3177007594396044 2.2436878998799084 3.8859764268273898 3.8212313705742988 4.4543534580896438 3.6896122276213106 2.7476414178103616 3.3008960948155814 1.0047755442052055 1.2594318161634215 0.78955239362779672 -0.20900710343385925 -0.1679995777941449 -0.040517577563748963 0.065179346724053649 -0.11262655445591017 0.084798495243911198 -0.076633761417685561 -0.026191315077429226 -0.055582069668167547
2.7031637038194174 3.2629692668792916 3.1036777243107796 4.7985253805746781 4.7635906988724477 5.091686469228625 4.311359472048661 3.4443543023505336 3.6943708099163124 1.8043182047694597 1.6550095658849517 1.7981463761974117 -0.36305121527201129 -0.31293600718385522 -0.032670278335428474 0.1323309748311603 -0.16530232008138654 0.18788643569788471 -0.099321977145477483 0.013610688464869814 -0.068534191812922432
3.1021585156417699 4.2831645317892182 3.9520606924447321 5.6653284617906481 5.6497636953211137 5.5580869736849534 4.7784343307736661 4.0906906302255797 3.866232081713544 2.6438169091658779 1.8698986046503732 2.9589124933437239 -0.49141290125259818 -0.39315813880070266 0.024517605229195015 0.20958101234019197 -0.20561249683879806 0.25713735050128445 -0.12484443120835033 0.013191472350293044 -0.10146909796961995
3.4245700277991458 5.3628184728524699 4.7197015656176502 6.4219053832795057 6.3406983423572578 5.8413737153985092 5.0529645609948712 4.7192958483689553 3.8430813567144262 3.5687846387940381 1.9708717495544716 4.2858219413064109 -0.57831025564791583 -0.4057368197437749 0.12635525807732972 0.29319012569207481 -0.22707261010566918 0.28302460173342114 -0.15063160888082475 -0.030524642909944232 -0.15466988496820977
3.6788356091146879 6.4351457370434337 5.2832815893187446 7.0659304423049418 6.7065466414009691 6.058228539938904 5.1336821833067985 5.3790131926600466 3.7034712496556761 4.5762037060057867 2.040324228535737 5.6854783820468953 -0.6129669123803394 -0.35972337298357115 0.25621645351852607 0.37292549992816448 -0.22593003017780597 0.26384367505840201 -0.17294071647544723 -0.11028085116693002 -0.22160596829919268
3.8098192130341126 7.4347178396402223 5.5613794461599122 7.6788180875530587 6.7489285956569347 6.378915991440488 5.053117893396105 6.0947159462183391 3.5015380745419233 5.6034885259638276 2.078915258401302 7.0093436091840777 -0.59734742915156924 -0.27252795414766595 0.39332809197768132 0.43736293011930399 -0.20411581086873182 0.20805502537475623 -0.18908560605429314 -0.21085227737763168 -0.2915819636452599
3.7602741314398664 8.3565238282705554 5.5584789192985173 8.3406694768669798 6.564729829246664 6.8819463129289673 4.8388300311333214 6.8610237917456036 3.2360341602720668 6.5861393376166975 2.0079368294175488 8.1736972525711842 -0.54266969884541716 -0.16085025157377728 0.52262511650669663 0.48005414827665316 -0.1673872677654121 0.12839916728459572 -0.19845274529747128 -0.31769716352109412 -0.35614920798891503
3.5082252851915721 9.2259857896792905 5.3214864806518305 9.0788146980174655 6.2317563532506419 7.5506943024668933 4.5045525508498612 7.6627110105849043 2.8824209247201718 7.5023587280733688 1.7668479000110482 9.1794354711780866 -0.46109936763758219 -0.035569780585048547 0.63833171316475068 0.50110109725180785 -0.12119811408526811 0.034909540317199804 -0.20176373116893465 -0.42284732206672948 -0.41188413252466705
3.0698593065646338 10.066160599584078 4.8930185487910958 9.8843185055582534 5.783497744723376 8.3389991693726326 4.054978180957777 8.4888892538909921 2.4273080093367132 8.3588962400006697 1.3418600938082261 10.062736231593364 -0.36141775708260121 0.098100721680064729 0.74153514944839005 0.5043844908328321 -0.069017016051154109 -0.066967694417478019 -0.20034151094581312 -0.52451972877608777 -0.45952029965493918
2.4737916723619007 10.890907166669162 4.3089676085325488 10.736205699914374 5.2302175595496578 9.2051552325885044 3.4974457540255375 9.3317595725024471 1.8715436984307328 9.1742025551519522 0.7554533150113546 10.861769773173556 -0.24932080400406703 0.23820629049025133 0.83671666817123969 0.49531801558719146 -0.012615828732972942 -0.17498465717128456 -0.19559977366830336 -0.62433805778561047 -0.50177814601655402
1.7567174853357796 11.707006469145256 3.6014883278601784 11.613991007659186 4.5806031810091543 10.115804008587121 2.8430004859624165 10.185825615436933 1.2264773656630183 9.9671117224705448 0.044562774008261127 11.610261176701002 -0.12878659930986153 0.38393839822966441 0.92909577189102022 0.47930372927059672 0.046988564647474002 -0.28817227410523383 -0.18893921981556844 -0.72488398146664756 -0.54187192949973151
0.9550060295778906 12.519376649545963 2.8057983883583608 12.500638153086445 3.849294196888863 11.044746343978424 2.1104318055376687 11.044827474951393 0.50861448199411985 10.753334843558317 -0.74949406657494189 12.337076534879499 -0.0031474214925964004 0.53431553532054499 1.0228743140840884 0.46112300971036874 0.10895855321552265 -0.40562762340926634 -0.18153659959737523 -0.82816934275431875 -0.58244646560814561
0.10826418068157394 13.331169677884676 1.9582107909982582 13.384311943762413 3.0603056544395124 11.971035912045934 1.3229923761859335 11.90344824011736 -0.26097928659790981 11.5433541154152 -1.5886514560096039 13.066680110774467 0.12446527694089743 0.6877239696752746 1.1204478793424968 0.44435979696014877 0.17226373876569681 -0.52591044979026602 -0.17435060758901261 -0.93492861843474928 -0.62515285420099664
-0.74726925138221867 14.146026050912292 1.0979292824373059 14.257170447691307 2.2434306596244573 12.879637194100125 0.51037809767423281 12.75632544579423 -1.0583711779706766 12.343384592284083 -2.4372431100477989 13.817456269217951 0.25092541516364197 0.84186187500067033 1.2223587781104557 0.43137481860694654 0.2356938171852179 -0.64699086490963131 -0.16802280170569922 -1.0445946763228726 -0.67052684189635448
-1.5749425645273762 14.966501994690544 0.26261429308736367 15.114980429600994 1.4323948244074274 13.760315562733377 -0.29629065983304753 13.600075243637376 -1.8562189974949876 13.156387802015175 -3.2627824458985208 14.601738967322593 0.37328898180009362 0.99411891840748234 1.3277769917718472 0.423393367472014 0.2979016238725074 -0.76645616444239895 -0.16297093563554085 -1.1556023114102865 -0.71826841681101739
-2.3406155025556061 15.79457085130233 -0.51202462998860243 15.955389705793291 0.66074966608586005 14.60683080334335 -1.0654354866373448 14.4327432656347 -2.6254383337303202 13.983955267222003 -4.0338234093619389 15.426510106704393 0.48886652063422747 1.1418597586431773 1.4351706443104675 0.42090262560037917 0.35756173705071903 -0.88182946287520414 -0.15943130311588222 -1.2659241824677216 -0.76760673945961133
-3.0141321114986894 16.631673061685746 -1.192994817550121 16.776591762747348 -0.041179580270675815 15.415530099027038 -1.7666347316820326 15.253421317980031 -3.3370531443834142 14.827757377306366 -4.7189984268594163 16.295026381253553 0.59531971802617334 1.2827001734303494 1.5429419769947821 0.42405730868010183 0.41351122084258335 -0.99088411893614803 -0.15752374121103618 -1.3735945637280365 -0.81767561319830362
-3.5665059348770876 17.478245224189273 -1.7528447656375299 17.576610907467096 -0.64724936548137246 16.183699843035591 -2.3743396731445499 16.062782899362471 -3.9631012247370969 15.690151441024103 -5.2887769125571005 17.208509684921555 0.69085201140741781 1.4148337789272047 1.6499272894208983 0.43291383488714608 0.46483335875765525 -1.091909410779945 -0.15735292640416584 -1.4771125110763317 -0.86784657831105561
-3.9744984865922013 18.334465057888142 -2.1686289396868066 18.35240561822728 -1.1384187955053959 16.90940258450799 -2.866831011751549 16.862069537296094 -4.478749022770633 16.574926255160673 -5.7145325809049012 18.166730946919927 0.77430805037895512 1.5372387968234886 1.7557311096407409 0.44767484728597218 0.51101036842159997 -1.183962797379331 -0.15898824342544313 -1.5757835707563435 -0.91788750798710539
-4.2174540402828322 19.199461405268366 -2.4252141599300221 19.100184401345441 -1.503270172852591 17.591448716785397 -3.2291412118984946 17.654103155836907 -4.8620051143171583 17.486436360301234 -5.9708433806565742 19.168365960462726 0.8454030256892926 1.6499706012916617 1.8609218588256051 0.46860127800428802 0.55198812313994672 -1.2670587544267429 -0.16251725097417705 -1.6699284371814656 -0.96811358229129385
-4.2816244612335854 20.071375278766315 -2.5148024898551533 19.815365717690099 -1.7391945364263077 18.230932698525802 -3.4511735237922014 18.442918631414731 -5.0947524178075509 18.429861875880452 -6.0356759101441861 20.20954579772275 0.90493024261150978 1.75436311884103 1.9670186337117088 0.49594052468487487 0.58829776548731694 -1.3423292080522868 -0.16798535152891747 -1.7609614506100921 -1.0193026910581833
-4.1587376052592964 20.946172824375338 -2.4396450071319435 20.494510496257877 -1.8520356979690706 18.833575129035182 -3.5284719253551255 19.234440577471741 -5.1619575013197787 19.408693629672847 -5.8924461455981261 21.282607343187035 0.9550803629787008 1.8531798770369372 2.0761843740027781 0.52967068628466274 0.6211033362767362 -1.4121112049729396 -0.17532677640021035 -1.8512321474220248 -1.0725674952764483
-3.8491142635233393 21.817274207673343 -2.2118293088228684 21.137861539775557 -1.8537307013381061 19.412904952440798 -3.4602441741484058 20.035720929887773 -5.052444332345118 20.422593411740731 -5.5321229324639143 22.373644958481812 0.99951254574954207 1.9503587477422717 2.1906573978056896 0.56921858713209084 0.65217790151153943 -1.4798043709181556 -0.1842614080493126 -1.9436427264295035 -1.1290297057589649
-3.3652680367773429 22.677205521157223 -1.850625184116425 21.752051963992248 -1.7562835409189184 19.991577448942341 -3.2476581024036677 20.852681462845002 -4.7608339523687118 21.465376267033655 -4.9579393558390663 23.461107336029578 1.0430570221841735 2.050305255740362 2.3119688602357868 0.61320587281766104 0.68370525070185595 -1.5493884299733769 -0.19422780772424039 -2.0409713092687718 -1.1893761155350659
-2.7273139431895843 23.516978167348135 -1.3801075681426889 22.353706978883828 -1.5639639197351189 20.596420322835016 -2.8953268842146778 21.68997952874259 -4.2915064507864544 22.521007793609968 -4.1930085061925757 24.521907208580473 1.0910143515420359 2.1569061620505097 2.4401512291353833 0.65920847663247195 0.71782279258134196 -1.6245656316422079 -0.20445926052726549 -2.1450111658536133 -1.2535747112484754
-1.9700755496981475 24.335319298961565 -0.81940623497675902 22.963477106601566 -1.2719587096441947 21.251240302348389 -2.4107993041281803 22.54500926141213 -3.662873310431658 23.570616316116851 -3.2773406681059467 25.534337714559594 1.1477806466254803 2.2723843552007446 2.5735085726880649 0.70429150984919509 0.7561840037001859 -1.7078267937321947 -0.21402196473803692 -2.2559498696400815 -1.3205375450407442
-1.1304063630413159 25.135364333619311 -0.18335645427774688 23.601623028090515 -0.87088021935985771 21.965050342428835 -1.8094859590454344 23.41199957875574 -2.9078172860696876 24.59557827797973 -2.265407821244191 26.490384439125972 1.2158359181661174 2.3969146280767251 2.7089647470438702 0.74532489976516481 0.79945179969795777 -1.7997717080178788 -0.22214024407164507 -2.3722561163803482 -1.3884646045481643
-0.24532924321414712 25.924490552770195 0.51565702263486057 24.280387811947424 -0.35869318578895626 22.730760392214894 -1.1149680209704889 24.283532500053777 -2.0686367847273783 25.584850376077682 -1.213196766168726 27.395978366936074 1.2953238913202201 2.5288559520740841 2.8428764583183663 0.77976666762893954 0.84724544214790209 -1.8991429964835378 -0.22832106349984529 -2.4911514333121505 -1.4552094514811511
0.65013272118222643 26.715247735877341 1.2622616596899088 25.000208276778878 0.25191274893095411 23.533555255018239 -0.35811031613991262 25.150662890172288 -1.1897698323179084 26.536846465600114 -0.16898340908998907 28.263479376552933 1.3843789069822545 2.6653422339782766 2.9717460622273517 0.80611951964569561 0.89843322778389767 -2.0033988560644955 -0.23232869914398166 -2.6093555155861603 -1.5186752430463013
1.5255540048585781 27.509999535022477 2.0337674120100933 25.759416630628881 0.93424270547130728 24.355651628833648 0.42640313708627597 26.011346470682774 -0.31379356153873089 27.453549505781183 0.82765623364941654 29.110036229051115 1.4801838514818602 2.8031526840040017 3.0925207351714779 0.82362348790050244 0.9513918691510409 -2.1093840063562168 -0.23423893669702001 -2.7235583976364088 -1.5771582706744296
2.3505154724652235 28.322514388292131 2.8023346254740282 26.546800407617663 1.6540189317264371 25.18819500184086 1.201627587993839 26.857335609477058 0.52205312382225366 28.342265647680655 1.7446508694354623 29.942888945091614 1.5793078838343562 2.93877597870787 3.2026095479938217 0.83238574628800255 1.0045447785656012 -2.213921228763974 -0.23407095280787568 -2.8308453365237849 -1.6291286178296558
3.0973157690775799 29.135849893568093 3.536989388117874 27.366751071350691 2.3734261359229594 26.018424230216215 1.9352268275808799 27.698236168202587 1.2818637302016818 29.20262299609978 2.5537235883377405 30.778115640562643 1.6791727946234303 3.069484384193502 3.2996343048517196 0.83225159409453098 1.0561813216957181 -2.3141187599988937 -0.23214335228438909 -2.9284240427683428 -1.6735681024171596
3.7377865865624762 29.969230554820612 4.2069104651242233 28.205962608410299 3.0592633465027741 26.850461743949822 2.5940530889888662 28.522498246765803 1.9381681166166564 30.042088460297236 3.2281486655269132 31.609758385756326 1.7770219556894751 3.192308822648112 3.3814985319429671 0.82379314030144168 1.1052220460688718 -2.4076601869755039 -0.22827477048755479 -3.0142139957893788 -1.7092372392897457
4.2485578718112142 30.801409673788044 4.7841712720039187 29.074002915237838 3.6809293457463963 27.679323824118033 3.1535966001502862 29.34226849237497 2.461625085759866 30.854369143428102 3.7447742623913958 32.448625951053103 1.871450773176019 3.3053892572425547 3.4463017619095835 0.80661859755007459 1.1505411303620816 -2.4928431730219081 -0.22267851422547003 -3.0864600271036289 -1.7356121985396322
4.6051926085200492 31.659328117034555 5.2438400463564481 29.957267977643657 4.2161107856345961 28.518998161996556 3.5919063367732837 30.141280500483351 2.8388109708375842 31.648170360873969 4.0787248853165687 33.274954881968185 1.9610542537607456 3.40689369304829 3.4927581611264169 0.78111256115477312 1.1919261494893116 -2.5688102619151034 -0.21492949512097462 -3.1444809763003563 -1.7516996705218022
4.7955758553099006 32.511683930263999 5.5655308634471856 30.872380527469627 4.6454668398492958 29.365351698493527 3.8962949822039388 30.938595900823849 3.0497699504219167 32.40909128139289 4.2147811301934448 34.102896661556137 2.046697522712865 3.497165153685116 3.5199152285926552 0.74646772518469751 1.2291395064464627 -2.6357561174508293 -0.20516436347982214 -3.1878709015887576 -1.7577360088458569
4.8094234885231781 33.365951557236414 5.733227446742311 31.816365718370232 4.9563493003274521 30.230980491307154 4.0582004472774855 31.731057446601316 3.0912891665206326 33.136371845942051 4.1401652878106665 34.919272940542889 2.1295005697921812 3.5772982426602149 3.528143431525137 0.70266008140180625 1.2628931051163432 -2.6949860597974959 -0.19304719459034658 -3.2176906196654658 -1.7539251483476903
4.6394688571932869 34.229497124234825 5.7372376341540949 32.785211271133207 5.1374999830813941 31.131134670661702 4.0777703600983619 32.51660534634766 2.9701984402070671 33.834279516554041 3.8513502064405567 35.703272071068575 2.2114291903924763 3.6499847490077686 3.5195368970530327 0.65005595131937877 1.294471009573279 -2.7489072489454309 -0.17827830497966918 -3.236384973405924 -1.741054536816631
4.2983664468177878 35.080624288362273 5.5687476390569213 33.786631444854223 5.1812956823094147 32.068783902618812 3.9548836171761166 33.314953235357528 2.6957790756081641 34.492129867512652 3.3579134279719622 36.456877261294423 2.2949954889768769 3.7193946624654726 3.4984560034694336 0.58858786001923613 1.3251777396339075 -2.8006113187421282 -0.16140144414516938 -3.2478787498043236 -1.7216811065589412
3.7952144228823292 35.926703056453775 5.2293409687365999 34.809798092775623 5.07877991195428 33.053222847616631 3.6966652464776062 34.126793607835971 2.2910317773644451 35.122790463357639 2.6820849858176361 37.160691931960272 2.3827003285127137 3.7907213078511601 3.4715174883144364 0.52036656804849313 1.3569452870841008 -2.853887522560794 -0.14267289455332846 -3.2572427671831976 -1.6986396480808297

11
SOURCE/systm_anlys_robot.m
View File

@ -1,21 +1,10 @@
function dxdt = systm_anlys_robot(t,x,xRobot, C, ppC,d,R,K,kp,ki,B,vref)
n = length(K(1,:))/2;
m = length(K(:,1))/2;
%% l = length(R(ones(n*2,1),K));
dxdt = zeros(m*2,1);
x1 = x(1:m);
x2 = x(length(x1)+1:length(x));
counter = C(0);
%% length(x1)
%% length(x2)
%% if (t > 5) && (t < 15)
%% vref = [5; 10];
%% else
%% if (t > 15) && (t > 20)
%% vref = [5; -10];
%% endif
%% endif
%% size(xRobot)
dxdt(1:length(x1)) = -(2*kp*(R(xRobot(counter,:)',K)*R(xRobot(counter,:)',K)')*x1) -(2*(R(xRobot(counter,:)',K)*R(xRobot(counter,:)',K)')*x2) +((2*R(xRobot(counter,:)',K)*B)*vref);
dxdt(length(x1)+1:length(x)) =(ki*x1);
ppC();

1
SOURCE/systm_robot.m
View File

@ -12,6 +12,7 @@ function dxdt = systm_robot(t,x,d,R,K,kp,ki,B,vref)
%% vref = [5; -10];
%% endif
%% endif
%% dxdt(1:length(x1)) = -((kp*R(x1,K)'*R(x1,K))*x1)-(R(x1,K)'*x2)+((kp*R(x1,K)')*d)+(B*vref);
dxdt(1:length(x1)) = -((kp*R(x1,K)'*R(x1,K))*x1)-(R(x1,K)'*x2)+((kp*R(x1,K)')*d)+(B*vref);
dxdt(length(x1)+1:length(x)) = ki*R(x1,K)*x1-(ki*d);
endfunction