sync ke artikel

SOURCE/DataOutMotion.data

@@ -1,48 +1 @@
-# Created by Octave 5.1.0, Tue Dec 17 11:11:50 2019 WIB <adnr@morilin.fedo>
+# Created by Octave 5.2.0, Thu Apr 01 10:57:28 2021 WIB <a2nr@localhost.localdomain>
-# 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
-
-

SOURCE/SM_2015_Rozenheck.m

@@ -15,16 +15,16 @@ bParam.edgeL = [1 2 1;
                        3 1 1;
                       ];
 bParam.dScale = 3;
-bParam.kp1 = 100;
+bParam.kp1 = 80;
-bParam.kp2 = 100;
+bParam.kp2 = 7;
 bParam.ki1 = 0;
 bParam.ki2 = 0;
 bParam.nNodeVref = 1;
 
 
 corRobot = [0; 0;0;
-            -1; -2;0;
+            1; 2;0;
-            -3; -3;0;
+            3; -3;0;
            ]; % xy xy xy
 robStateLen = size(bParam.model.origin.A,1);
 B = [
@@ -39,9 +39,9 @@ B = [
 % fvref = fncSpeedRef('s',50,50,robStateLen);
 fvref = fncSpeedRef('s',0,0,robStateLen);
 h = .01;
-tspan = 1:h:10;
+tspan = 1:h:20;
 
-% sys = formationControl2ndBuilder(bParam);
+## sys = formationControl2ndBuilder(bParam);
 % sys = formationControl1stBuilder(bParam);
 sys = formationControl2ndModelBuilder(bParam);
 % sys = formationControl1stModelBuilder(bParam);
@@ -54,7 +54,7 @@ s1stInit = [corRobot; zeros(size(corRobot)); zeros(size(sys.d))];
 s2ndInit = [corRobot;zeros(length(corRobot),1); zeros(length(corRobot),1); _zero;]; 
 s2ndModelInit = [corRobot; zeros(length(corRobot),1); zeros(length(corRobot),1); _zero; zeros(length(corRobot),1);]; 
 
-% state_init = s2ndInit;
+## state_init = s2ndInit;
 % state_init = sInit;
 state_init = s2ndModelInit;
 % state_init = s1stInit;
@@ -65,16 +65,17 @@ startExe = tic;
 
 % continue
 dydt = @(t, y) algo_init(sys,y,fvref(t));
+##dydt = @(t, y) sys.ss(y,fvref(t));
 [t,y] = ode45(dydt, tspan, state_init);
 
 % discrate taylor methode
-% dydt = @(t, y) sys.dss(y, h, fvref(t))
+## dydt = @(t, y) sys.dss(y, h, fvref(t));
-% t(1,:) = tspan(1);
+## t(1,:) = tspan(1);
-% y(1,:) = dydt(t(1), state_init); 
+## y(1,:) = dydt(t(1), state_init); 
-% for i = 2:length(tspan)
+## for i = 2:length(tspan)
-%   t(i,:) = tspan(i);
+##   t(i,:) = tspan(i);
-%   y(i,:) = dydt(t(i),y(i-1,:)');
+##   y(i,:) = dydt(t(i),y(i-1,:)');
-% endfor
+## endfor
 
 Dd(1,:) = [norm((sys.K*y(1,1:9)')(1:3)) norm((sys.K*y(1,1:9)')(4:6)) norm((sys.K*y(1,1:9)')(7:9))]';
 for i = 2:length(tspan)
@@ -149,7 +150,8 @@ plot(t,Dd(:,1),t,Dd(:,2),t,Dd(:,3))
 title("distance error")
 legend()
 
-csvwrite("DataOutMotion.csv", [t y])
+csvwrite("DataOutMotion.csv", [t y(:,1:9)])
-% save DataOutMotion.data y
+csvwrite("DataErrorEdge.csv", [t Dd])
-% save DataErrorEdge.data yans
+save DataOutMotion.data y(:,1:9)
+save DataErrorEdge.data Dd
 

SOURCE/algo_init.m

@@ -16,7 +16,7 @@ function state = algo_init(obj, st, vRef)
     al_state = [st(1:3)' st(obj.nodeStateLength+1:obj.nodeStateLength+3)']';
     
   case 1;
-    set_point = [2 0 0 0 0 0]';
+    set_point = [1 0 0 0 0 0]';
     state = obj.Av2(st,[1 0 0])*st + obj.B(st)*obj.d + obj.Kb*vRef;
     al_state = [st(1:3)' st(obj.nodeStateLength+1:obj.nodeStateLength+3)']';
     if norm(al_state - set_point) < .1

SOURCE/formationControl2ndBuilder.m

@@ -9,6 +9,7 @@ function obj = formationControl2ndBuilder(param)
     dScale = param.dScale;
     nNodeVref = param.nNodeVref;
     obj = struct();
+    robStateLen = size(model.syscl.A,1)
     [obj.R, obj.K, obj.d] = rigidityMatrixFnc(edgeL,robStateLen);
     obj.m = length(obj.d);
     obj.n = numNodes(edgeL);

SOURCE/formationControl2ndModelBuilder.m

@@ -60,19 +60,32 @@ function obj = formationControl2ndModelBuilder(param)
                     kron(eye(robStateLen),model.syscl.A)+obj.MOdist(st) ...
                     ];
     obj.A = @(st) [obj.A1(st); obj.A2(st); obj.A3(st); obj.A4(st); obj.MO(st)];
-    obj.Av2 = @(st,select) [obj.A1(st); obj.A2(st); obj.A3(st); obj.A4(st); obj.MOv2(st,select)];
+    obj.Av2 = @(st,select) [obj.A1(st); ...
+                            obj.A2(st); ...
+                            obj.A3(st); ...
+                            obj.A4(st); ...
+                            obj.MOv2(st,select)];
     obj.sizeA = size(obj.A(ones(obj.stateLength,1)));
 
     % build matrix input
-    obj.B = @(st) [zeros(obj.nodeStateLength,obj.m); kp2*obj.R(st(1:(obj.nodeStateLength)),obj.K)'; zeros(obj.nodeStateLength,obj.m); -ki2*eye(obj.m,obj.m); zeros(obj.nodeStateLength,obj.m)]
+    obj.B = @(st) [zeros(obj.nodeStateLength,obj.m); ...
+                   kp2*obj.R(st(1:(obj.nodeStateLength)),obj.K)'; ...
+                   zeros(obj.nodeStateLength,obj.m); ...
+                   -ki2*eye(obj.m,obj.m); ...
+                   zeros(obj.nodeStateLength,obj.m)]
     obj.sizeB = size(obj.B(ones(obj.stateLength,1)));
 
     %build state space
     obj.Kb = zeros(obj.n*3,1);
     obj.Kb(obj.n+nNodeVref) = 1;
-    obj.Kb = [kron(obj.Kb,eye(robStateLen)); zeros(obj.m,robStateLen); zeros(obj.nodeStateLength,robStateLen)];
+    obj.Kb = [kron(obj.Kb,eye(robStateLen)); ...
+              zeros(obj.m,robStateLen); ...
+              zeros(obj.nodeStateLength,robStateLen)];
     obj.d = obj.d*dScale;
-    obj.dss = @(st,h,vRef) (eye(obj.sizeA) + (obj.A(st)*h))*st + obj.B(st)*h*obj.d + obj.Kb*h*vRef;
+    obj.dss = @(st,h,vRef) (eye(obj.sizeA) ...
+                            + (obj.A(st)*h))*st ...
+                            + obj.B(st)*h*obj.d ...
+                            + obj.Kb*h*vRef;
     obj.ss= @(st,vRef) obj.A(st)*st + obj.B(st)*obj.d + obj.Kb*vRef;
 
     printf("============== Formation Control ============ \n");