From b4eb0c03bc771c693b0cb4c43cc850ffa5c98a83 Mon Sep 17 00:00:00 2001
From: William Chen <billyskc@ucla.edu>
Date: Thu, 30 May 2019 18:36:49 -0700
Subject: [PATCH] simulation for jounral 1

---
 CoLo-AT/Algorithm Comparison.pdf              | Bin 20647 -> 18393 bytes
 .../__pycache__/data_analyzer.cpython-36.pyc  | Bin 6861 -> 6875 bytes
 CoLo-AT/data_analysis/data_analyzer.py        |  18 ++---
 ...simulated_dataset_manager_w.cpython-36.pyc | Bin 6686 -> 7071 bytes
 .../simulated_dataset_manager_w.py            |  68 ++++++++++++------
 CoLo-AT/gs_ci_journal_sim.py                  |  60 +++++++++++++---
 .../__pycache__/ekf_gs_ci2.cpython-36.pyc     | Bin 4287 -> 4341 bytes
 .../__pycache__/gs_ci_bound.cpython-36.pyc    | Bin 3409 -> 3415 bytes
 CoLo-AT/localization_algos/ekf_gs_ci2.py      |  15 ++--
 CoLo-AT/localization_algos/gs_ci_bound.py     |   6 +-
 .../__pycache__/robot_system.cpython-36.pyc   | Bin 3851 -> 4113 bytes
 CoLo-AT/robots/robot_system.py                |   8 ++-
 .../__pycache__/sim_manager.cpython-36.pyc    | Bin 5293 -> 5346 bytes
 CoLo-AT/simulation_process/sim_manager.py     |  25 +++++--
 14 files changed, 144 insertions(+), 56 deletions(-)

diff --git a/CoLo-AT/Algorithm Comparison.pdf b/CoLo-AT/Algorithm Comparison.pdf
index c2a354708623c45db863e612509ca6068795b2f1..a1d4ee81737be9c0f6d25f3cacc10dabf459062f 100644
GIT binary patch
delta 8833
zcmZX0cRZEh`+q`4vUkR@H)l9F$6nbZD`am6kv-yxvbSUJY}q3-*|KGp9Z4c2Gr~`O
z`hC8i>iOe&UHAL-+}HJfU*o=?;%6AMu^82z7(j+fEZu9@;NyKBl)b>h;IXBBXT!B_
z6Mg3Q!7hAn$mO+(<&Csnot|B&%XwK(yM#E}n_~opbY>3sduN9AYe&qF5k2&)3*NT9
zIQKsbIJ<dy{xEW0_{Z5g)7N4zqQc;z_r<1_k{`7Mq9udRi!Xh=Pd=}d0c-apYIib1
z=SYBJVu7~f#QdYMj}w!3kA~EeWt)2hiQiRKNSb)5z1q4hD5ElJ>g$?(vHt9nN|<<$
zyM%lA2f7q@(c_h^`O1f9=PO%O^Y<=E8>t(pB*p7}XqrFi+~K#5%r|A0jD9n(Iv>f%
zc)|bU{Al&lc8=*6U!|N->V4~{Q(Iy@E^*6cfHc9mmhxzR0d1c)cfn~r;R~ZpYPeYK
z59xqL!e9^kK03>8X^jodoSN?Pl!b9I$qDm;-5GP$%zJkj&DV|+3D50C3l{kI;%`;f
z`i-fV8XMxnj!&mLA!7b#Q}fozb1H!QX~J@aGz-3rLw@4BY=WK>RqC~O0^+ZsLk8hb
zfFCAouGA?a+5?bJ2UeVYwyU7G4@+x`Ng26n*1UM6c}FEI7)ysaEFltPxIUvQg!VCe
zis5YS_wj;i8pX7uD<o*5?0b7=<5R_xHlJZJ=xT9Koxe4C>3@8B77O@j0XXAw>-EI0
z(r!=$16vN!v#>?(p$~-(bXz4QE7YENz!+96qrtj$1IheCtWiR146oJwSo`%^*>?Kx
zswF4!^if?M=8LXMJIGobI<d&n@<Y!xSm25RDR%sUA;(ToD<OGaw8?!MuC=Yey)+Q6
z<33TmGR2m)nc{iCXbQQkl7G}h1J!Q%3@XF<^1O9-d7EVK?Y5DM_$pLJ_eC5((3~D-
zv?%d%>B8;IqZO6i04%zQpXTdFyokQT)P+@OeEIpuGaQucAbezd-npUb;`8D1(ktKd
zZDCXADhY{_fS63*&Gi@te38mp!`b|v2=Dcq&ytFwX3p5megvjM(-l72hU3U*Ge=RA
z@4WXJ4pWfw80LG}q~q$K6Asx;1@cwO)%t13Y_B0)&-gxX>bJdPKTJSvHXEd-fLAp|
zp>x<}eIX*nL#zis6azIX>eATl1rw#kYBWAG*^}v!KbFo=HF!imAwFK*v*4<C;`V{m
zQAydXKBC@9OCXh_?Cbm44Noamrj;Z4jQ0w`#kn`$G1cZ5+BUOZfBtT%zyPRx|K#%`
zUU8|Kg(m)S+zT<|xi2H9`PA;DEBS{XWb{WU)lB#?A7v=__LmL@poQtE+$MY*2=pz}
zAH7EeVfr{iEsve9i3?FzT5pdh#X?qF=9nqi(b$O4WVM(IoV!{JHx{~Mdmlv)Z}wJ~
zy*G)ZiX*nmM3ZX-D`J74?=Axm8R0mHG`#*BlTh^q1wK9-Y>Qm1`!SzbtX*4>^`cB^
z)Hf4yN>lR<?P}7ST%_|P?h~&u%vCSGC{pufP}vSDqJ(h<mszkckjk6&mBsXYVTHFu
z&Sv^LetTLL0Sc^^tGY&=o;l2M6P%bxL|(sfMl4%^f#8e#IQ3@uBNhNqyA@f@g$5ov
zG15fHl_c28WDvh_i;TMlR-&xffae)Js}9rC%7Pr-JZPAHJK3aQkMa?c!XXOIbHzq+
zz%=Yp*VQUd^1`PijMy4?PVc!252v;9KSWawJ5mtE6ZPazh=z(SNWk-rHn9>&X&Bjs
zf!_68^N6BY=H&vTtfxT9c)5^r|75w;ZH!0pJLNmMa%t1$ecmz)+LlgKLRG!xZ=@)`
z8gDChUN+~|3~=?c$~7iT+cI=FmTvml-L@*EL`xI91l?2_OwR1N>w2?35T~p<q@vWD
zjunC(F1AX`ttQtTs)nPZ$XSjXwl1S=z)s9<;6OaId(g(uFCqY}oBLg}V2uyQpkQZh
zCjiZe^;|Q=GFp%+DvnPdD$Wgj-ym3Y3&)$4G3c6LCM7M|*Nq~C{!rR;TyoCMhH?kj
z)d+&|D0GunAFoXhQ~!J6xO<ffWp=NF8Ul0j8wS#)zIkMXtKA%bVD@CCDh1nl;N=Zg
z9t&CzCt6=sO}4;bpdXKvneW7dBTh~mUG;!=WBu&Sqd3H^?}}8a3u12UB|+@kXPSQZ
z`4&H7DO;E344uoMv!6Ff%f73AC!2Xks5vg}5si(_@aGAOoqBE@F{p4_w|2><N!g&0
z_UFQsY<4Qvky2#nvdzuzZ`sc<(QzNS-Kiys9U{zpEZ%DcKL?U-Ja8}!YEF8&k}@)A
zm1!`dqF1Q%?sS}{AYy<=9ShZpGhl{<i+38SYU^(6#N3|oLhLE7>o@as&03*Iud&{_
z+uZ%BAV8R7P%C_xYpH28j^Gx-bH&u1q0Z8|ZM~o<!`3@`{1dpH?_CeyvW7O?of&tZ
zuP7Ny^j2x$6n+B8Hb`<)eN({0GSNoXE+>~^lFDPpQgi8MH4fAbj%aq0ZBot=?MDoz
zft0Wl7KZ9oZLo9WXrt%32(uX7>blCG&&%MbhQmLe9dPHm_ATpf?Vsq9!xyyRH^_qU
z#X<@_cxQL8mC;M`6hzHmY)g0w^S_nq17x&;sACGlcv*nkC;sSpg7B;CSJydJO#-)g
z+tS4;l-D%*LQ;BMYb^KVS%O0Z?s?gAS$&7dhM80ku*Dig?6FqsEaT<VDuAykzF6rM
z)VT@IusUUF<>S0%XEohbq8EMwEz`Y*XE!!$5sg)@*G<Z4oLDt`H@)itZibXSK07QC
zZa0-^Hs1(X@FieCMPoiY;g*Dw?I|_vgmz>;sAnHzv*?Rb4OV7w6fENdular+md#~D
z=&MP&Cn?f$_$*KFu90ySkSYx_MTeZA;y=?<bk0O+J)yt;*kKO4`b`w$RP<c)!B7?*
zs=t(pWzA^h^!<w#a|gVLYg~pLL3$!JxJ2c0d5#T$e+abLlP3YIThN;|B#Z^6CRm>>
zHTZQ}cki7J^T!^G?c6nbj*whZnu^8&hErK>5~skm0{-(-OafyVUmrpG*U)m$MaCw6
z-}Z=gcTzjbdB!FihOLn@N`DV8k;2)$Ma`sZ!W=Bh2iu)2g*l6OR<VR$+OZhPl4{W|
zc1SYd!>Y7lqM8IdcHMSoR4gXiFuLeEB+r~`FU&C6q1@)Rl{s1(^@3#aR5FND-L!(f
zf_@NcG{Ubt6TPm+TC!X*StzuEoXUvdsZhN~?1E_<sm$7Ro!~Rw0F47@eTz7kxb)EG
zR+$v2><<37>+}PH%^gpdvrNkRDW4ecL~{56_$I5LH|yBE`}XUxl}$@?Xy(#~I+?T-
z_@u{AEy6>__KbPC<B_E%x9(_75*x~5wOoHY`$iOV#Qm(<m#x4vYOR%wh&6_rH3A!2
zb8_S>pX*02VK*-GT_Rmxk?wn}P{RtBo99eelymF7_ab<>`{eAbj}MV3rtmg@0AB76
znEFMta-<K0&b=t79k_S`l#iQdcZa{YAB|;aU&u%CMlavLmR6}uKF17ih`<G}AZFd<
zfU70az|@%9(C(|6Z#~^?Z#u;CMRG2#MaT$GJ)TJXqYa6yl`OkWVo1{P-T0{Pq9B{I
z#}ld%-4U#)8)-djN_BT@nSHW6hQ)O0fgL;U1n$ZDg$F`r>7E+zc*QVd$|xAy*udFu
z*WekHES{27bc3jYmnUX24(TxeVMk==7#tUZsKC{%WKP@WV$;9NylF=a);)X`U*<=g
zkT{h+W2{cAF5I3i(MiFRAnTFNZR3jBmR|M2?&z7s*2hq~TBcO;BC0Wd4O)V4z`^!y
z`oK=)<wt(qO1*=)GDPdk!D%J9)~4nG!3+$n7C93_)qTfwV<+#u+u?Im$zxmM#@e3T
zC#@Y-7W76eMg-R$Uf;2>;hVvmxWOK4c+CJ|HOcSK{uuTMKw*;<%~e5V(+-RnySm49
zZYj|rLAz-k^b5f$f*s1(%o9gBK#=Zpodzm?$5I^Dfoae+V3r*cLa1R9$e5dp(2p@Q
zt1rg8A@EZ7@Jn9hQwB?9&*T1sWTjryEVF#N-m!khSAHk4+f_#*6cW$eM#nUA5)@-k
zn2t^p^?Z-HDtcPZa7*XA=Bp&y?tvGxVj?)8n<E!rQm+2#deT!4j>;x-0s4ckW9MDd
zwjbNuXQqvB86{p`R%dh8Ns1up8+RjuGo$>>Rt1yblgCBtH?h+gP(w$XEs=n<s)Af$
zqF2gGPMfMo>4Ll#&&28Zu6sl{5D}?NIX@oJunZsFx}!aCPW_9M+*#6Ba{@J1UzEFO
zEDk4cJY3RSB5YAkzVpQ^3E=w(7}TC8gkln|pY&u`^&obf+mC{{IkqSc-^YoXYBP?m
z3|s9CZry!BNmTVs--e{YExS?b%-<HNu{&6Jiv3)xeaCs=>AQ?r$(O#Z%sdP%JYvk6
z6!#g9$Q{^QC=M?^WtB{SAaj|FzEQm_qI5xE+{)-f9JuYdxhPmMM+qq3ZHvuaj1?Q)
z5!ok!_#qF562Bl1L9L&rU=)7H!y(rHzxwWrJlwC}3RvA9^UCxrxUZqGC|zH<Pgv_W
zDn?fR$+BIwV&^r3wHMu4n3g!>nKjgd-Y;&^?Ms^6&(*2>@0E~v@o?Q!`)O5j`GitQ
zA|N8-wr-1mGEYU1cdpu?u(C7WD>Hf|H+_C1*PX*YM{?&(lYk2`B=VN?x#z;gr!)0@
z)OB-IcjwzJ`;Xrz<yzl#Z({8GE=jvChX;X+S|W)00@iFrv}%Y|5#-C~{cO$qik;Um
zC1lyM+Zbjg#uDa!=npfYa?hr^x&6NfvI6Hj+ZkR0z)6$;<xcwq0||;ZHuwusO+j(X
z#n(^zS~(^j{qd~2mtDIvFa_=(r9I8upH^isYLHD;<4K|vgU%E;NN;CQk|_feQ!vgY
zN|HHvWPx4V<*IZm!^<dXv2R(M^Wgeu`^X)++m3N#<?Mrk>@OXMH`Gzg+iEQRp1_(Q
z9Aoe8DAUeVVP*H%G)72Dn1R@j3V<Yq+M;J2y%X5@#kYrO!~iXSKhM>1He&$OX<#Wz
zQr(+xLTlF!Vp2NYYp@yOtK%<cKnU!3Rhr$6lYQx$e&o{?7)a-P8>`h3-7AGUOjQTb
zQ7i6|VK{nPB<y9bTcoD86W-e!1yojj@D*xO@-g|irtpsR0`k}>I2OiNw>p%WvzfhA
zoof^Oa;>Q*AHk`1jo4OswT2H&bP5H{HQjAoAW7*iXM)Hd9oo!nmu2%F<B7gGRw$xT
z)DCZ9Pp5sQygcdRvl%;)05j&$Ilt*2+j4S*x<x+LPbi7Py*IMDKM!Pu0CaueGwq>3
z=QYJ?yH8P66(#24o6+{7H4&_agu<&n92D`JdqctZl<!)l;Wsv0+mauOla#zKn#Yqb
z2tPQITKq1@_JRS+*!Kfj{LEv>m+CBGI-z%_vNpxHLJgBm7BeH^Qx7q$j`=(%H2qY+
zy=(5wf*ok&iaAkPmfW)%ECwFx;dH%Og32^{6=Ir}l$pV8qo_<pTEv-5XZy4G<y7s2
zi|wfHi?r~4)gY!c;8c4vHT$ubLYb@$g?Q1V9c;$zdqPij*G|zudB0awLqtv{tZ6~r
zJ$LGcS0?EGquk-=Bm*WNsp$I`KkI%-(q5RA3RmbKb>W3-VkA#RJ97a1nkKtH9LuOE
zN>n@pMrgXS@ao^)9;ezB%WdHAW!jR$9BhB8@>SfE(&b$-xj9m;vCiL<+0)C_#rDi9
zA^yDci`0>M2d6+Pd*qh35$WTAwY*bS2Be!P#6Kb>;pGqdFo}u>_0bIjtKSQbQxT$k
zO_Ptp0=oq>gXaxIjf?969_M;R(VMEY7@xj*j*h}uB`m&#FdV3ONVt63in3{z@T1#6
z8e|B)$v3a@NCrVnm0o65ps6>G%LHoGIgZ>v9C_BNV<;lLB6%y-4JTiSgw~fJvBQ@(
zvO4F}kn)D~OULwKNcyztpa&e;>JYYeQUAvMhpXA^2cdnK^uj&B=<sUdrp1SlLCvhu
zTl`S<GA^B7L}ON@>@e$cmb|G>ri<8+#(a2qox<CRC`J+Dhe)VH$E^CJjP0zvNg(pU
zB)c$by`#|{ex3(gj(mV#dioM)%woPg54<3XXhb`)-|)#ioNE?xn)sAddtc|a#o)<7
zxshr{i%j0@sU;skh+>ka&*RhW5h1z9<d-GxB(_GVh#Zg4(Zbq+6td0q26fLgR6>CB
zI<(E4$4R3Q&I8pHT;G?o>QkvbPZQ8LB55WUHEYf`v#42mEV3@>I|HdEH{WM{3#6Ew
zsrVksf}=GY@#w%x%*Ddy=D6B?_>Y88zun|<oEQ~jMO$`P0EH^94Kazb-yU|4OiNG+
z84JU?P@0&DL<dI(&{}j>Jqs|)S(Co*kIbk?YiMPJd2&0)Yf#?q*<*<rNJmp=GTkRf
zvl+V-S&^th$?21JYfjiyQdZo%EK-JHg~1AI2_Kzu)O~p$DB(@mxQp_klpyP$-7S)>
zNa#K|ZZIbU^@fv?&b9l>C~+>)<ZC#F1M3HLmF3f;+-a#z)zgu>6Y+gM4>+`xX0q8@
z&nb)7;`NFmy_5%(1z6(go2l8-xKF8k4015WZ4fbJAs!-G^^>aIo3;>f%CrK$jrUax
zZ${bho}RjY71LCj=q?H&i?@1Tx%Gg9LFvB6W&c(Oz--Xov0#P6PpXvkcxm|gaG)IR
z>5W_jW&<aa1Ai?rcNTS`OnyG@b8)06aNp^De!G~YT-%fe*oQW1t4;{}$*b1WFLD}%
zWd>wB_Ia;$fA|l68p}8o>Z^Zn@`Bx0t@KoN;Q2i5=fgc7zWzsolMXgkH5_}in{Yby
z7ph16)qut!?J=oSk^iIWR}UH<5<P9G@_t)i^-v=x%|N)O&p+CaN%x+M{~|9W6z6io
z2f&GIn$)A#sR+Fme%s6(-FF&gV39sQ%i!O~s~aK_RK151zoAHYr{LQL%w`eS>0I~2
zSGvc_Ml6Y2ejO4gDkjK#-*yv@%}rcKn?{=*(o+DYNQ$*>LB2`Ri0jc3u{$hI^#bim
zRSM|h1s(FVd3$8k<|B-rwVcOGAuw?)_syKxafjUS&7A#1o`dcUW`vUIwrEE8mpuQt
z)?&iR$QX**9n<W}J^8IeNt=_<V26V@7f-W+Qr}c|Rt`e{8PP}Ii}efn*)KI+JO(Jc
z%^HDN#xjaYn9}aijz9NoLfM-4iXraMSLyAhu7m4%;XIS8B{}hzchh}<m5am~L*9MM
zfG>NsV`#fZ?9gRj&*C#K8;^}si6E1`mM4LE;;Df5_szqji{kAM&H{j6Tbu7?V+!Na
zs~?BcTav>fAINTmSsgvYyxF(8(`8C5<o_Muz3!bd;k!VIuWbjY*SZ=XZJOg@W4Vh`
z0t%&FFSdrBbR(wrw&n`Q6Sf8SNwf6V*<NY&hRS*>nPfEE-Sf3geLGE<vCHo5*mjBI
z=`&|EKEImmHIQST3|Ia-bb6_b@`=#DP#syk&>U&ol$`o}n7q}}L7L_Kb^Wf57;sa{
zU%QHa@H?gCkK?n;^TXF!V%{f`dMGUueCr2RZtkwu=1xB?XA2ZQ1Oftsej0Eg2>ibw
zI#7fYEy-z!He=^PcX4{6vpBfw%(*Zz(3G59XiY)lx-4!>40JjtCz_Mj42|I8tgGg|
zi-ET0=0tPxqY`*I0VqruBq4$SJDPW<{9PqiEGB~1W1fS+(k`B_+3F*u`<D*9FeP<L
zGY5!$U?87&IMYnWaG*f*Bee~|N5atgugF;j7mX(Vg_NNn+pgq}dpapzZ34!#ivC`q
z&jW0%{OS}(;*ye(hcbff@0du$S)c=YylzhlxGJ=OkFPlIKjz;|YET!APRL~;UvW-k
zS@}$%?5XBRTQ-HveTF;Kz#oQ8W6kq4e~8*kW@66qu!O%piBxCyeEs2~9K3axHDSUQ
zXYnC>zFM*|CZgHrd9@xlB22<$yzl9z`}07{mv0*m$n4)YADOP;NcdmIQlB<2N%rj+
zF<<2o?aeEKZsoT{WAJmLl?5!(XnxMRX#pGzG_$}}a-e@E2d*Jni^)W=g$U-4qlt1d
z8>>sx5jb|vv8ugAdZGC3Ku&?(WW67^e0Hyak|vDigi?F2N_@Nf>85y7DDKBqnN)Mu
zO!!t)M)8N<cH@t*na<W*uPl<Ouo}SV!=&D{A<s^IT8@Y6wYM$b$~jsZ16!ZTXGl~z
zF`2Tq!EF6>6kPtb`B4a@`dauWi|4)k?YrjwokftmF1mZz8gHyLlf`%0bLpuLmZzha
zl>{SAu0!L^%(Gb6M-9|ZrX3Vs-;HF3a1_w2K}jYuD=qM!;q@X#@wq9mQ#RcO7bPz3
zjPI-c(Rt`%0f9Pf2ss9N9?XT7ffk`@p`3NZFlr3+29y&(fdA+mwP<P54iGNp?n`D3
zO}RIh?!FV3re#xvZ5`^@!zty14&W#i@J<ty!a<WOH8G__7i@HMZYOW>0;;5%6*B49
zi20;;@8OdPrL&`lzSqjmeQzuhP0%sx|3T^3Fu<TchoO#7unrG^LH;U{mWrz^1b4n&
z+a+)`wcS74y%x38Uh$NJr<zYnhF`#7LM32|ZN9L3?H=PXTaAoF>~{LFR_`%o9r+kv
zmBJWFBOgB2T{7_#C<g`e)Y#Mp(%U>m0<(%(or}^N2Yu~E)rzTiqWY5#(c91^5;PD`
zOU&Att`e!Hm?*CFgXiO6L6V?*06UM{cR$xsa#a2RE(J)x`5jIH4;KR~nRvKjt!taG
zUf5FHy@c1cZ0t0|i|jRO^&KA;lIowPrOIHh>pz}$uWLD_Q`P@a{dAApoO1e!S~k4&
zsZD*1rB%20ahGa%a7Z_)W`=(f-t!c-*vSPvmLdf;&+V?-`$D@+a{1q3yosH!Iax76
z)h$`^C#oNl1R~n1Bihz)A}Bk;th3U237qWMT76@JJKkj06`J?G&3?Oofg^w4FCTp&
zE{0wa2cs7yIME1+0yKjZ=g(sVfNLQt7o`Zc{PB%Jk3B~+KVg>!>@0^Qn&QjW>qVr;
zP$t=bw`S7iL-V(9D>;SV{&qZoA5NBVk8Q4yAYSvkSg=mJ?a<3tGnphHl_Y#LI^i|j
z%?pB*C4Rb3mJX7!EMDTiX1Y;NNil}V$?_cxkgeV6(LcnBFeFz=Tr2e(0Xj<x#ak^8
z=O}h%_kH?lK{lz$5%|V>;gkW=d$|e~{>S<{`k?V`IwhB=XwCS?o`G|U1c*Ay))IkJ
zgTz!k_w7jLoxBcGrQu8qW8^83ggb9ZyU>TBsu<-{MIHy$Ij?*r`(b8uWs1DBu(Q8-
zlhi){AYd{AlGdK#w14zQ>v@(XXF82_uoqoAf#7Sp&|xyMP#9_Afj?(}sS0}wXS^ig
zt!IEKLw;1|4QC9u@tlSTOsc(E^C~*C;ECS!HGe5?GGDXP%eN;!7e6rR8XoJQL#0IP
zuqA0Q(A?6T07Mw_=Pbpk5y~(TKRM3KqXIwM$Bf;#Gx~hq==t^gYS4iSlWcJ}CcVPy
z9fgq54e#m!2;s|{P<E5({hX*R))>4^3a8GNdeNN`UI+&-8H<@i;zEW3`N>M^P3fR7
z-@2)&<q8~9o#8ni=zxIMN#m(P54qz>7W~Zq7)^rPF4*rT#R(TkM|acN+jY2TGDH%I
zbg1HMXYqnd3N2E!b?u^7^~850O}4*dbRLPG{oiz=S5zp_Vr+EiJ5oC68YCyWOUm{-
z9E^sran@-|+hZ^axZAl}TZ05_oIPBx-u9kXwwt~8)yvw``T;(AMMf7_zyc&-2@<eE
zOUm-x5byvAc;eSRlzoj2sDdCuS5vPA5>NwOjr=c53j`7QkEIQQApT?NfS}<2Smq$;
z&r4GaU%=eW(%#<E-qq5>2?YIlc3gR}06}5DEmw0(2WxjnYa4eERPYbX)!x?b3WS5e
z;y?XZf}p=SSb?r?hF^ZHLD1hEY(UW89DYfI{ig9#EqYJM$Kgt;;P2pWF6NfjAi>`R
z+(ClB!s7o<kl<CkpA_A_oI!%W1A2f2e+Tq<U}f#<X6fvD<=}T@PmtiR$oK-j)~?PV
zIJmAxSp?%c9D?3bVZw!hAtEp+7>p)Y^}G&;qN7v=0AVovYRAt9^Z<na%YX=p{LIbY
z82pObUl;;$Me0vX82qy)e`ByKet%$B3!y7<U^G$<4hVsT|KR|K{Y>^BdmwPgznGxV
z&wBs)oe)g;pLjx$D}{f42Nr_DuKEHDT*3bp4;F%5-DZD(DD*ET3<3Y#{KsD5pZ`#Q
zV?vO>69*O+7W$tUKjU9LB4FYF0e@pq$W?3neEyH12n_yrb-*G*@V~PK7D4=rA)vzl
z_(K4K|D+#`5Jvo+R`6A||5gkE!-W4P0|8&v=`Yd{u!!K_;vf*P@UP5V>Hn`f2n6<T
zkgLM~%^Cs`g8iLV2t-8a9}Ef=`dbJDDhU2x>{tFF(0>&}pduoFHxdK}M*O1=1_%E`
z5GL~T2>GLQ5J9leKmG(EzW+KDgu(uuEXe<My1T2ny`!}&zL*$DK<R<a)vW(I;y?mg
p&d%<j-?p~BuQlkZTYml4o4dMuyIR}eLxiuI0H2LbUgHk_{{s!}Wk&!2

delta 11198
zcmZWtbyQW`*QHBZIxi{BEn2!uM7q1Xl#)YtUpkcTE~Pu9Bn44g5Tr{&kq_T{WBk7N
zJpY`v=iY1WIp<#I+&j+41mx0u<hovDz)(4%_>J50W$jhkJE;YK@iaF&sbEVphd?g0
zVZtE`+3+DL=Kfzlgi52+xf=X@YnGniV!bNA=6O9>;i0Q_zCi!#=f%NRjrdx3x1oti
zckkWZ;psy9)zPO%5#2BSl?Iooex2W5X{cOl9}5WFov*5?bptnp;%kq@*V}nty8~f@
zf<R?+_|C_0wKr5%`#;*b+C0zlB|+i_0Btpf(}h7SR-w$DL(Wdj-Kl1OHKxwq^e*v_
z73cMpqgmdg>YF`YwQm0xw;Luux0WvsFV%`y?N4i(x1~D#D+N}~?Q7ELYF>2v-H2RX
zF0_jq-u-lrkJ<D&?OKL*;rh@ACIRBYC`bJhUq5KL^hTtI6P}J)u;~i?Y#6e@RsMBM
zQAfl_j?2LM)r3kqz=t-t|K>VFYH`@~OF}%uTkja5R!#jwUcCIS4qS$>2In;k7HAqj
zkCE~|HQyZCS6&Pfd0(D#$b5BPZr=YPhQS1Q6E3fs@3N;aD*L$+zJ3~!G0F|JZ;aLx
z%Kh2_nRC4{uJ9<Ah8fcgJUVB1&ej++NRK%?LQg02K%mTW5@LqKi*If>&=uabQkN;J
zozq9<272k{^J{<k1--cc?W~J2&7Frxnp?(m&hJYZv(>h5I#+x>AhpiR2U{$&Jko<L
zrTEmINA8Mrdr-9TCU_|)yv-lb(`p*SwaAn{@p<0LOz=EYSVgDi-C#I(vZ#+&8tDfj
z`gZ<y!(`0%Dp!W5krX)Ta_#<BX)nG)Zw$J&lP0|bEJA}O?IRXGw;a)CUG??zhCT1!
zAQcXyaOrIiy?!_9Px5|LF5s5l<mIg!$LgJ7`%t;NJk~CbVW2Dt5&6zAK(GK9106$U
z{TD9e*75Zs{xQY|*c1)z#n4|mC+XkZ9T>K!YE1y2+l|*^Kd;*_=>u-Jcxx}kMMc+b
zdo<=HLWrDM-l}Mo9Ur%*OueX%kg}=ri;s9Kt<1U4KXAQm_#T|QD(>UIDMA+S5du*x
z9<Aud_?+|p_(`Gw?&H2AAnLIh*Ye4kufyUBnT(+`jS}{0tFqY0!pd~Z!ZR7e`Vl>`
zQqHh?HdSAz6{~a^{IL}&;>XXN>;0tAM(eL?2C|BBUx9zJodzgRqX&_r5c(pX2X}|%
z_{;vF2<8<M*(H}JFz)BOrjg?u<k~<xOIXFY{>%bGbt*x^FtbP=0UUa<69=O*wBw4+
zq#yfMEGxo@8o!YeBAeUGc(tWXX^=m5Y#Q;n%t#o4X@iI9WbvhU5aS%?+M>R`N}~`o
zp<S`wt*dWsEd*x_Au1=~fZG#;2$nu9nRvx@mnQ}U%S4c;@rp!pR;dyAR%Aj1vSl@+
zmgVurUGJaH>qhUj0b9-9PV{nYJ%)a~e#yxRvT_M$dsKH7e#B?Cth0l1udOD)X$1yS
zG7nJ=gFIts=_?TRlC4OkiZE>BA>}jln5moxP^z{}#tD)f3(TWp_kAlD=qQR6!5sTM
z{4KS5S+KZ`Nl3*@=epGfEc6(pK+-HYB8VV{%YBgXoF@VYFwY5Ou0|aml3;ptilkE|
zg1-4xBM!2gWmZ}-T91`sO0zsfQ`izByYo?Al;ClR@Xb>FqeUU1COiiXD{g8saM-Iw
zZbwsYEz$}zBG)qF?t~4-0nY1iMBGa?)i0z&T5_|l%obyMMcAbi?-l})>QmVeaVYL>
zReEahR5SksU?sO@Wo%+l2B~lQ+!$<`WO0ml_sxn3%uXXcVwVOTH;S^?H|4S?5fXkt
zTdL9(p~KUFmDBdV9?Q6Z9Y;tz$wHenyEr;(dec9;!)&nrr?GbR_Kqb%xp7!`93yW}
zywQN5`R89;<~iv*7al5aHafIYQ469=&=Yo_(;#a6fDC<h#+o8{r;ipKUt$2Ryf{&2
zK3*_ZWSYN*W3Pc1BsoY)CPbExDTvLd&cJ6xlZQ*QHYbaFoLImg(n{u-yJRGo$5V<H
zfaS27$9O6#m(&?c%*xR(Y%aakBzfe;%|uS~Tr^TWFy0!%1O73$B8NBN>q%q%=%9y$
zbcwMAK>ZZ!QN0-iK_TX9sx#Han-UypLi&ixM`KQ-z>$s%(<)H)egMHqEj9d()>7ti
zg22U0EB@AG8pAPcG8bv;EW4FK_N%v2GJMccaSOXrah7K`uAFtb-_Z0*iom!l!dgT~
z0$Hxq%pruTJYI|JD|T5}^Po#al6~u_4}Mr4(7ZJ@>@3NtiT?yqM^SNe94pMNOCF2i
zY@F*96q2kJ;$^pZrz?B74wk`jz}Fye|K>IUe$6<%X0VMbn}98<HQ7JsF!KRYSo0P6
zLxH(hG6&cA%ZEC2rrglB`Av-Tx$}%B+&$dcA~6c>s!9%|1&?<a!FlgBv}|v~-``3Q
z0yXno#sv774vL0M2O<q5c9`4fz~PNh8?7?Io|F^sY+z+Ja!Foltt<hKx5-Qh%5$k7
zqIt5t_Ip(Q-+Iut3VP6$O$IAub#pO(XclLFS?5PYLO6FMwIom-79(gPDS0aJ<<|y1
zBqt9dl$z^9z`7hmMRj_r<aiN}1yU`v0o^dSpUCSK`&ywyy%arp^KLyQ*x+bE_|pUx
zOUbX;uxCZNNV>`Eo|@CoMisjwD1)4$jB>&7{CD<!?8{}tMowI0`TVzH{d-9KR(27l
z=1mE$sLWmm<V*AIT+(auS25pX?jp5832OxS2K!nkZZ-NuLax|zh7u9&*mY^&0qpsp
zM08J64$>9rPx;}+?wGW?7|Yd>UOf<x&9x64a{R33wiNVy2XzrVBVe=VdG<kT!8};s
zC(3gS;$$5;hAGKB$a^EEqIFesH0`U-vmlkp22(~dYU=U?XEXY>l3s+$N6nck^gK5_
zOH&rpX4@EVb`b?(Xda)eP9Dgu0;bm(`cI|hd_W+A7fnc6EIVZ0IJ(qnS}%h<;(R5A
zsGOV^pL>7#qKO+xpv_*E_i$EIvcBH;S$WpXBIj2tIJ0eNQ`@<qSXUX%%~)fFasmu7
zm@A(_p4C*v59?=+i3kbnOE3T#5@tt{rs(mFSC}hn&g&;sS3aKMeOq<-7$}|5vNC(Y
zq0R9kwZ9XqB3>0sm9t{Qlu)&FK6HAnKoAzRt4W0`0fVM!hJTYlFC4E#6b$IjOZHp6
zWjzaK2zrT)@_<-Xdz$G4`+8PKt-I|PhUf9A+CWOvQP5%(=HqFtX`g_2)Zlmhn!mJ|
z6DxxdkGyA+C3J}-ya)Hw(101L-dXGom}5c(>OeU>?r|RJ4UwMj3KsSh9G<uuBE?+n
z7lT2Z(+lE=zd)<yuC)GuM$Xg^*-sn_#@EX>*(dDjZPiUFp?sd9=nSTz>@th^Jj7^Y
zZ4sfXeI@Pb>a?F|O*hr2MOkbXF-J<lZqzJMeMs6Ox>0(3OLH<r1=wky$UcR4+cHg*
zqDDP+Z?|P4Bqk%H;@Z?}B3g>I>Je<Foh+VZ2ufY`9uFCPlxz6|tgi?v&PUAfYSwd3
z<b)<$Q-GoP*UTx7^p&Yzv}E~T1Wz!(re-{mQf$ZTR5)Nxk?D0kH;US1O!>^<+T=^D
z^p+P1y|m~ZSSc1$69~L=w~2Sn*0m}{TL8;*2+bMVj!qIX9Yo>+rIDV`gvWCkQPTM8
zH$-HsB)TdzMP$EZV%@anM{4R<e7cBb3>HVfjAhh@I9yYbGph$@Cw@13VuX3ZW!}{j
zH1sv*ENJyh$s`iZLo#x<WrA(22iy+Ut)|4U%N<-=O<7M=-vg~6RunYRrCKyno=XvR
zZ47&MMt|0&hYGDQ8E+`hFpL#zVUcv(fE8<kd7p&KHWTzys3Vurb`0%-XxWOoqh{Ul
zlEU<~9QrP;8B@w<Crz;1fmWAWFN$&T)Qd+UbrSE`TMaD1Pq}>wbuHeR=J9XWoCgmh
z6*{PXIX;*y_Xm;(MjF4F`qktz4~@7cq-TSaiBDb*4U8!gqs#ZvVUrBYBRR&e33#`j
zlMzHuv?e8~V+c<_k&2o|$=0fM;*z3N=yRh>0rA|sjdS~<oJuOHnBnKs(*`USCobG-
zcUEaO27(TC1PeH{fjEq`vy4az+qGB3G6RzNvg9B_6zGZ(bM~x9-7rP=GK19$OS;v+
z77Uj-3z4Fvin1_}!1Mxe<ElKz4us+c0`)gZYI5cJb{_h(&!Qro4?uU)>5!;DH02KR
zGBLe%s`KmTt=C!H(Nzn_7IHz!4M<4kdF71$7I`nWkTJ$N!-f%5-L-QW<AV~~HyP2Z
z**lmA0YWomc2+o#4E<Ah>dm+<`N-+RK1{Q7>InG52_r~5TOgh!H#pLT-=}7#6&^%r
zX$7?Iuyukuv*6u3Y$a&lAv|KZw0Ip+PXg)|WaUK@_$eZ1CS~QZHS#pIvQ0eabR^9S
zj>6-ma0`zH`(%eR$6U5NVkLSZAMr`5?cYo-05`E+I5>SxXdP5Hqt8cl32wfH&40Tn
zDXw*&Kn-BmU7FH)o^2ucvGOZksEVacEsbiI23o(do%#Ae$}7t6qd{75ph4#up{dC3
zheVy$_#<P-XqMWJ3B~3;bE;FNj*(SkOnDZwY3i1^f&x;^aHfzZlKF>Gmbr6J)TdR?
zMgbv#_)!lIu;XUg`GnU~KV4{5T)G0+q=;ahbLv)2Ciz-LW?DP3ckd`pXZ>eVgLc7+
z^_sY>*_2omt=@+z;6pVe8jJ<L95Q|yhny!*9@OncHq?g9^82!wq_k5#`Q&X-q`T~{
zZ;XbNu=^D;GukXkO*8XUpBJkb$Fe0Q%^lc4O?5DZr}i3|=~)tSErdxvP{rFF^2(24
z;XG;4Oh<mRh~Da~DKOQmy<Cy|?SsNtfM{VGxZq=_jF(0+c7U9hPmPtJ(GKNqX%!{m
zQ=Jmdb1w2N#kxgf%I}kbZZsF$z7{>_2wi`tVX)DVg3`2Kl-kIhHf{#8`jrApCpo}C
z`54Pip{)h*$;I7F@Z1KpWSWQq2@)|jJPKpNYultzPB-I2H1|T$(y00yP4B=LV?*+V
zE4?K|{DV<yxj8m`IB~XlJu7hGYnT%ie-m3sVRGQe^X>q=Xk2>Kk*)wyJ&z*gU9W*M
zGKUgc44yb82Zo5(LqU9T&yB++jR2KGgtYv%r0VG4v?k6gInQ6hV6UPn(f1~5a@^7=
z=+m}T1Fy)tOB{m5p03V}Cp_vdVTmx<9}OkzW>u^0<Xoy#*UNDLr_!aosIBAL51d8q
zh_K>(Q@Vqmfj8Fo3MpyO_t*gioWrie*U{eBjO2*^$hbTWdx4CRq^abTv<?Wn2z6Ok
zTt;ny_m!vRJ@!R+%)COz_!J+=z1;Tb6mdk}O{SBcTLAB`EjQ}={yo+N7B962$T^<K
z$0~L<#x3@}@G=DzZ^k^gkd?5J-w&8}R*S=2)LQC9X(`a{QuK5!+{|?g6l81+j}O@n
zMK;10=6O*%n$Ar-JVM&%><Q3R5yAEKZt3c+DwF{R^9P2hUT)tboiIW*wOvqKz%8<a
zem!I|Z*9xG2p0LX7WaHQ$U;7(uA;U`a?P9ta)S%4HE56x$mJS_i0Z1183rip20v=_
z{Ngg*OUAHH2;}BHzW--ta5YVoaj(%#w+v?bz_w%oNxp}yBG2+L5?Dleo1kz=SI<JJ
zrQa>@=mw8I_X~9l%4qxEKJ0iR&cfEAY944#>E@1FWis5Y)*z@TIKfRK&Z*eKOamW1
zqdHtt6daDS(%5*gflmCyhF^rq2eS=-?6H9o{se~QhX)U4fLLKFZDnt)1UkG*BS{V0
zr#7*=a`wR`GH%94i~ug?P!vXwdbB+|_?Z52G=;Kq5KOI!I>=5s2*SKGj<sZfv;WRW
z2?#=Y^es00896$}+K{BqrVTrIpX77Eo=s#kxD-Q)-LH)etU`4VKzS+~q`1B|kn_y?
zzJTTNvj4L+i9VcAP7yOU*R@Ram8r5_#$Hp&jDyBJt|Tuj;Mg`&<ejv>h!T_J4=c_0
zuI8o!84+dT#wirb=KGsVcIxKF%ptEh(REXFkx)PIv!~{!P)x>`=c~F*;IiNwR+PO6
zN+UiCen2P_auH4}jjuqM63U~5J5j`~K8M4VNfA})@hwyvRB3qqV~(3>yWRq2iE=os
z5d}m&=RveC1Be}THZb<xW?;juZ349dqwMmj`lEY|TsojnwKp<jHY>-rhdmYRh9`b5
zC0dT7wlN<9Oz&dNd?${U6rgwy<+wZ5jWS!Qv|SjIK#yEL47yv>#63O?5^&zPD-0>J
zpbauIt@!r7hX=Lyvnpk?*+i_m`5V3|M_t$#*B_TXCxH1x?9xS^^|9pyyH?yJ3-p}0
z(^vu)SUG2(a20Il6=$!{KXJ6Y2K8fpYnO2li`LIl(fnx<ZlvgT9i{-T3(+ZPlMl9=
ztZZ<k#vV#yr)vMq)VsJ_QQ0aV%n42f$goT3cQ3I{qR|Ax-;9A)4VW`_V}IRe9t6JX
z5GBC{G(?@#zdr?QJoxb-f30loT#t<c;nX%FB0iJ@hn(V~_p_d-UqQZ04g;aV$cdZE
z`D>rEfgr31Ujjb*{I$gXm?YC$_K4TX1avB5d#^})_G2%?JTE#6Fq}kr2d=L=2r*Yh
zwfj8oT(`8k=h6`M4#)|sqc6I0%V#BV)O?Eo#Jr<J171@P*Di3kek%an97c#6(+B)K
ziJ;f<eK8m6#BG{YZ64O-zl$B98jcJ5veRH0c&-80RKB`>ctz-09aU)Jp4f8Dj-@k1
zUm8iK((QW(51Vq`)`?j)kQy`-)M`oBKYTrj%su@>=#)h|Cg^f8dDIDb;z8Tue3%Sa
zE>Sn|?H`qnk58VPrmcVE=(2J8FrJZKN#<P5OjG6L4JNp?)7cns&^yP79MlXPCyq>c
zlDRWqh1XuWU}7!1n3hB^TO^Lz{pG!M`@$pt>v@;E6O#mYWQ2&@ZS3c$A8%oCY;`KI
zP2&cEIFZIf(ED%5bdfs4w;!l$e~_31?VOWYeaW46I1=;clg<V=wfi(3qGN|_r<-=#
z(x{AUG4-&UOFfmH_4Rky==&qQ{O=5uaBR}$(9nv~QxejMaX1JH-M=)-hBhj5`sKYZ
z^mg4lKeGy{)M4@~TIe)%-Mcso3vSf8pIhjZxSwk*4yt_5;#)M<$?Up!76h30e5_0r
zojEW@d;8+Y^=bEfb&}}Rf%^A1_oLvyM)z|&_oH{j;w$e$e0%04qP)|yY}{5a?7dcL
zO2|T&C*C=H>di@HCJYlWxiu!hxI9*>r>^trWeh}#tw~$!UGZL6S3<S7wqD#hN!5Qd
z<flrI;&<eRO}9^k1W>*ut0_FTSM@uu9iN`XSeRUo<B9vyE7ob*6=D>^jcgwO#3Zw%
z{^;^a^&n8CvCT<R@>Y>qQ3UTb=Mk#SmlnYi!Qdz#y&|op%lQ|p0YA6jh#Q#zKWo9H
zuHjALCAS@xPh!ent!RH59?zgr=yzgv_;wp@&OIG#3MuN@w*c@=Cmf&1flw4aC5^u`
zGL0Xnt5Zq_sx!4&`!!l{L*<?ObpkmcF(TjM^4}fBc6^F@Qd_U%YlFk=YkYQRox@R_
zjp{$sRzq5nEQqBoRj31d^U%puG}l<GwoY?n>oW3l)xhL{1cnO{Pxb;fyOG&Kf{iz}
zdC7;^#kp7>mK9(`ppyXJ%YvIA35|nREfPN_j5VE$og>v(A{{FI3=_7o#5QM3U3s6`
zLp+FWs3F&|p)_uPpyyz!qRX!Mp?u|4X4$iEDsjp-q@b=E(=%BQAsZF!QZz38{$ti6
z<|y?BWdj-ZX@02Q$r6gvXGLQlTiMmvM<>t{bZ!>#Iy12HcJOJuDY1B3eYM0h#w9zR
zquuN|2u*cToKU|)vfc+Np=4p~giL!sCrHuAUih=KqylWW(azVD=_D{5S0@H)1<pqI
ztxeJfqQ|e=!?U%^(EBYCy=APeg)Iw&JVa@cDU2g7Dmd&mcomzTK3rB64BPGPysnr|
zr&LuOFRuZd&_t}kEJ=3HOD{J#)1=&Jg`lh{eV;CphMiE@6B}HC0SjxIFwqSp`<72Q
zg>`m0wa+!XAe@4!V#y>nHB8;{y42MX+2!%$gqF<}avFz{WUZkQeu>Igq!zuy$l)M*
zhBRhb)2sz2^GCi#{tGig@2pjbaU<JiI5Q#W5Gnw=kvf{8%es4>n5`mL=3*tN`;g3r
zk%>gKE$M;4EX7AxjzUj+D~Nj(h0;9DyUc-+-9dhPBpr2{i@Y8;T0Xa&R}T8b76l?S
zwwp0X3uG(33Mh&p{Lp?ucrr!z;d=Cj#C|2&d=>@%yalsFHWohb*!s7461i98`l}8J
zLtsEQd){DyEH?jbe~4|Y^^o?Id#}&bfpI>mezd;7)`@6CUt?HfsZRxGd*ZKJPgOox
z5=m8ZME=~CZW0O6u)Aj4G^g-3#L$4E_{_JtgsDD}EWbhKbSan({sVWJ20~&_J^o6r
zRU1o|6_q?s4}ke4Itf%Ewy7p5us#iaJO%u)WPC$a7a|A84+`y~E0S1klfK~l%n?>E
zqG=HfP1(7cZ68VEvI-t<>u?2op_qSd+3#eYC+(tn=ZA~@Hfh{!;~~i~*f~STJJ@&B
z-!x8td<V@8B|xxZ+L|yp5~(taj|^-<s7doy00;%WP1-T!?bSgGHphMJU}}y*wFuC>
z)|k5L7oE(jT+(Xm!~IDSU8q{iCe7%NdqWeX__0vk`ZR8c!g@c512;YQGE}rBY42@;
z2@!^~suxqE6FLQ&P<_GAC7yFgHHG1G`7UZvPgG%%nln4m`jD(wt<DGamUT;77UX*~
zulW7b5Y?7%26E6Reu2xKcZht;_?Cd};ManfEqU~pKV)R_ZDp(*4^LVTR(_<2bGPU@
zyA+_gR3B~FaS_Y<qY?z3TFhS%q4cBX%&p#RS`n1&uBC7+8Ah5!2W)i09gB;iE~l?D
zB!7Bdo1tZJZP=g_BxaJ?QVzVn#p<LUsJ((F@RMz*aZOgpbEk*8W$JqzMA8F`!aCI#
zu*mMZoIZz@81^sbw9NQdifCB1?$0RfBasT%EY$JV(upJ0b1jg>_ZA&S<jcvta*pMa
zZc%>5)_F~|@CS+`^b&5IP_JOF7`RGeGb2rr+zI;)*jMZAwDdHINnAZ9$C6WvZM{Fj
znio}LIJs7AZX86;hTo!-R;~aq3=_nWi@r6=U@S_<R%^{#_lOc<24>fcpj}4gjXIFE
zBVT?w{U}R2f^vz%RjJPsb2yF7nfhG@eDa3jD^sm<Y+DzTDX2TcE{)fOzcOZ$;JfYz
z=Pl3a<nSbB%PVAVyW$Aa6RzO`BCh6Z(Lzo|*20Bqqj}vZftUXC>r1CV2eC4-IhCCO
zY$8UMGQjc&_jb~kUDo1|7;o24R|?SqkE7(RcbdPo@(rN2N8M2m!ehJ@7BQ8|j@(o>
zdYj}-QMv2&212ri8RbaTHq@~mY$*8Y4-{g>%|-a(o`D$gZUUni=v*C=^c3WOk!doX
zY3q74v!1X$^MB~M7~?Gg;5KGp_Gb(f*1?f0+R}Bk7#UrSxaaf-q$~Iv;@=Yz7%$G3
z)GAipa2yKf=zoB6jLD<CUaRvDjcgA;X^Yrm>bX!=HpFG<RrK6HUcypL8_)39)`mzv
zxG9@aRLD?3=H%jb2}Q&5o@=F)+Q`6vJ;X^BlSr5rre17=rAYi9(2e(m#bb>X7g?VW
zmB>R9_LgQQuo&`@4o3JbsL`aea9eC~`WH|iyTH-q^X5WIvGCDyG*1{gxPtIE=QC1m
zhH76{X4SN0;JgSAy<18iB7SdAt2R`X1F66?G+c_pdHAIO#jsza&N!+bMHMCcaTq(x
z&Jr!9Nk?pN<}9-aAgy_YPs)a1bzP3aiRiW0E2cw~O%-2M*~y2O5jq~9AWu+c%gA!;
z?F~E7Op!`d7G#pa=3`XbY2ueknZIgeCcWF=*wT30KP+pH#v03fENh>UMUz40`_g4E
z8yqX)AJ8$`YXC+9(0|6Dm|?opyR1{-57@g8drmuyed2fqq!js#hf__zxt=+bj4KPT
z<9)=(;l5t&;!f$TiG2`5?9?B3?3I`^?XW2q))>1!^0}gla%40akFVlYF}r%U@m!gD
zemF9CE>Z$zM4#xcK+aP$vlVASE)EO4F@h%0s;8@ylk7hyWF=I@eqi-P@0Chrg<uR=
z5?5`Z+2583V5N#&oFP=}lUbCUs*F?VOUYC`<TLUmV)a*9^d}P}itC{t86b}fe%Zs%
zsl*4bn2}kir;Cvz($Zcf`AA7O*pG2sfU5U6dN(UzZX&yi&A&$;KV_50cb*t8r43KI
zHsjNVTzPrEPM&7qb=IiU73rP{W1y-q-E3aYa2s7QAfu(`B+e0T`)MR;rgRYC>u(Tx
zsOC~h&zRTW*LH)^Fi31LRpR<MUy@ieQjIUMVUcVvXh_2zw$sO!!>ixn$VM@Vg&nQ2
z|5lKK!8z(lFUJ|jwePcbN3EyZ=_kI%q-e#8pUtf-5q=PdY&FN7erkjtQ6>s%>dEPl
zrAd_~u<n(Cq0Rb(gR40;tYN?Mc)9m<KS{QPI{0w+P31`GyN3c7WhZ8n=G};B(lCZ@
zw&2{FxYT;;s(>m?ZVL`3ZO)t~6*}G-_aD_gm=0U#+7llx(J&edXWetPR{B5TdrMW1
z>dlJ^E>}m`42r5(z~0Z$8N2tsTjunu6Ejte1#C`_zzU1ccP7bCUC%dcPKiUQFN6`V
zNjq~E>K@PwR)2ox^kDCU&>L?T2V&^^rKGZ2XpL#imsESJoT%oVPtH_gAI-S~yI5X%
z#)^d{v>S??6o_}8)pG|h4qwH-K2}7)l@Mfc>$DiQ!NiG*I&25!%}h_E(9-viYGY~=
zDS$r7;Y}y31_c`AYIVJi&~JfewwYHOEQHc~hhMCrg#0KSDe*7p1Qkp6X>mhWDv8DM
zAyK*x2CdkRZ7}Q=DFxGBQ+U@@AU3ZDY`t*5b{*89X`}`hirkFbBERfL9^U|c2AayS
zhE}6kY87R_wB?tp3227O;c?BiG6^zL0HooM=m;>sGpi_tN;)IV^<Y~*K_}ytUDJ=R
zdv<6mG2k4~R_xfP;q}~?rr4_m9@0mON)jJ^I%_rHYS>D@uDm^6l1RwZFNv=7X;@~-
za9=5UPvfBCEukYCY3MO2jWR|s@jW>%hmjw7na;~n*A8dlkh)i`$e#@1MCJLI0!-X?
zON@*ZQbLs)i-rBIqR~$aNOHez^)pgEo&?iYP#VZELz#=|)k=cYQ3Ls`-pAOvLj4of
z(}q&gBy8NVZLF{#V9t6s^>;M)5E4CDPJLXJ5vkd>=cm|G6WR<=L`FZFFLQ6K4$_n6
zb0y4?LvzXt&D4o~6Y1U6AfcAQ0SLZzk5iPi#b2g)T?|sWL08be0dI1s8ZXoF{!+z}
z+9UO8%p-k-2+1QNLt)!LPhRhD>*j503C;ZV%#XFr3KbfEG-rRjkVq7bE@ebbNo25q
zDF;8|Vip@)Yu@e~u=6$pzxvn^L^b?~MM@NvuY+U~wL4oPuA`?)?Q~HB4|sX)tu}&T
zADKG)HcKFgN~QFJar`5)GF~x<=h1j`_aEjd-424Nd_Frqc6#fLiDyCMB0?R`uu&G4
z8*VoB%kQ1aXNvI8LK_NnxFLIZ51tb6^afhd2%LtiOxgE^+=QsuE(%|7c8X%lX&gQ0
zVaQ0U%|9|^RE%!d_~~p8RrvwfX=T<%l^#>h!#(E)G|IG|M%DHEMqLhlHYU3JY#){S
z>yk^yn^ZYRarmoEtqtp{_#*MV*!0Y1Dse~5hR1uf7fXFAMt+x@0!QM8;y-`g?5eH)
z-0i>k6(J6sZN6Cjd2$=^+DNcB^m>ExP~YpJa!aZz*GcDb{;_=#q}5Uh_;#DE1zXo6
z#}H)3X$)0CRp>Z}7$sW9svU(u3iM!RmIf1{x2@V@i_V9{E=w98sV=xO!5Rh!*`$Ix
zM|b(gQY!&iv0+t3FPZ$5XT&~Qr@j8+_v`CwmvQiW{bSkNw(w3F7fLUt(yiLBd3S0t
zELY)$9OE>1#GO_@Q?V6(;x`+4YOtSFmfoR9qj4Z3s>!L*gupO3Vw@ZTKw(gjm>7<S
zr@NJz6OMNdpSp-UIs|L)C5{mYw(auT23D(dxGGnNur@=+#{Me07&(wmF+;l*?V^`!
zO&RbgrsGPFJ*R_QJxEhlm*HSdZ_+-z*Mmu8&7D<jZ2Zxz`7laFl;C81jx`ty61Qh7
zL~EUeDax>b$>h8bAJYPG?K%xA7B2OFX~<rWv}JF7V9(YsH#m<KjT)QWE9X9VP#C{a
z0-9O3h`0oq4CD;TKAM>bfNj+!!_i97kC`AT-#)JM^$om_MN|7YyZbUFd_l~%t?LEv
z&d5Zl$`;wE!XP4bC<CI|O*FxLaVtL6C<bJKHRW-voGpKAMyOK204N;(FAr4J6`cpb
zxa~hkfA%1yRSo*6MZa3QGf@_%P$3AX(Ayw>@rbqIxFM{KOqj-m|5Rxa_a|M`_Q3l{
z(z|JX#%lkkPC+UIGD%ai%eJ4HmXA4Pek9=-m#<h%$bYVeyX-Cw{#wDD&62OE!Z}H_
z@UR4!?2I1D4qs0BzQ>zp4?(;697;o2EaXd?{A-C|p?P}$!_xSxGJLvB;?YMF_O?$-
zGZ-54>E4v<zGM4b2IV-^ptKVm`6M`o^d$b_sIBP7`mD%Pjn{@!#wr8Y?fd(>k*Zvd
zG&2F3Hv^|<)dHI?CCzD4L+DXcB<U;fy%Uba(Ux@$nh;+oM8x1i|MCIW0M`OPt!qLe
z4o}<0*_KPgVGlQy5Q|J5Lw`7i+=v?p*R{h}k;T`cLMoQQ{emE*5<%RO>mxd;*%5&h
z&k?dT?2Yu4sK|&eT6TmgjJVN=$_yEyPREAWrZz=H(6co<)2JgO$Utm}ueA1v{QG+?
zy$xcDfej%BvOxS~WN#b=VIU)fnb{hdnMjclm@MoFbLJ9+0xMf1n1uql@f+(~Yy=w=
zk_=@>1ValEdQi5;9XKU2f(6EgpcT+W^uXB=nF2P+{OpbFg7(NPd@vBNwcT?o5Fa0i
z*Td?$l{1K60K_W|5`chs<#7b=rxihn1|cnMAt4a21qcF$AP9sxF(D8^9Ny=kMhW3z
zbU+#e;Rk_1&+aoo5P`oVMG!>r???p%5&An)1wp}oN6$b|$ls9$2nzij;qYpLps>F&
zGY}O1Pvl`?XJ=vOZsFwwg7W>1nY)`=I9Pc)T3LI7puhJOzEA!Kbhopyy+{AyU;%>u
z!@&v!{fC1!2=)($-?9<?5<c>-W)@Z;_+KnQyq+NVKP)_7xPa~-gul3XIa^w}dsw)*
z-!J@!#&Zz-AJM*6?)NJN8cQVwkx}4+h;=CjET|v^4if@H5jfJ%QQ$&|U};`J2zGBL
z`1b{J2I2e_K!hN$`zrs%`0r`|2ZKTGY5y04-JAGdjPG8^zp(oO|2=0g0xZJ^2*HK^
zs4c)R@FxcU6XO@+zqjLGwIPB6e{cW`@%=vVf2{)x3I0A(FmR9lPdpd`|3d&6%Ku*h
z5HL*W597dme+Y$u1^>h#Q2zgy`+I*d7y^Oack^H2|HwcD1pYV)h|urm{C)jz9rSNE
z|BXSxe>el>1K+pde{uoA-(CD~5C;CkLI@1T_lGkufj^!3-D3Z(2Z#Rd@PF?ChyQ(a
z_xk^*1TOIBCh+?o!v7}of&V(Y`-=a|<%9D5p_q@K?+;h`1pejpU;ab*1O@+SIDW9u
zA4>S)kUz-q^TGbGm7ibm54VKiu>Y6q>27A{XyuNB$d<?EgWqrd`(^da#l;hJpZGi0
hu=BltwEubCo4I>>yIWb~K!o@N1#nncWYy$w{tuZXuN43Q

diff --git a/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc b/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc
index ae53ffcb4e2b5018885bded3a62c28e30f410d13..4e73685ba4848d50830b3fc9c200733bc22d692b 100644
GIT binary patch
delta 1301
zcmY*YOK%)S5bnpkXZAfi4->(`2fPp^g5aP;iej9EL?E0sSws*<k&|8-?b=J2-AG2%
z$cN1d$&#h!zzzOEgnY;^;DW@7a0!HS%qa(skwR7PhA1<tufG1OtGcJB`nTmjm;F<f
zO5oppvT^@s|66y@X0s<p7ouH28^0|W13q9=R@&$}dI!Bg@1mFJJ;h5OJn(a-c+ScI
zf-+R1jJ8-=L93$G(CTOnwB`Zd1<Yxo#b|A`MDgQgiX_|p8$t*yCfSgGbr-ov*^rGa
z!8kkM#RuO+w3I&S<$F<=ea1%OI(unx*KEWZisAtz%6^xX3CuJHwa1VeEuCvFYEL0E
zS_YP4Lvg^z*}S5iGq8pZuDb%Qx#r~zYqlWXn+y2eD8$e<McUvS?|(h|ltF~4y)MHT
zA4=o^m4S_=+OM!F=L{M+!pzxd6jN03=aD(9;+({2HDsw_b!3Z~p-rUblpsu*3g(54
zQil2?KmD*W<r5zeF(x>hII1%7pkY+RnF{hi6idJc0aTy?O{hW*>RS>VRo%C7t42M%
z=bz;~*Z%~kYqfEIyMJraS2wmdZ}ztjUkxXm7g|SGq;-rR{uQ}$ZykH6_?X}tK@Z`f
z9NpRJJq&mHo8vo0f2Tj16q15w?kQrFaAY*fI-<rjsYed6PYAFrS;v~?yj=h289p7i
zcn4?boAuMLy+`v`38<rsiv*NjTq1a1->?6`f70E?k9<`>Y5ZEVRJc$7#u>OOQi=Yf
z*}3#wbB&hbk<dZ1fv1|PMYECn*gkP;{_7-AZTdN=hEr@@#jdeVrmwc{X*YhOsc_+r
zn0>Y3k-iWw>f7=6&La$sPw1e1EEEy1%7Ei5^m2Rk|NBXIlMce%&+m{-@ZQmCc(-gc
zXZJeri|mUo-cUB+;3|PTllyW*mR{-O{S?&Z<5h460>scQdSDME1lZ50Y)R8rj#2y<
zivG9VsM+9v06Dere30unIqQkyJZ_>`(d)_U?W;s1f-?j|gifgUlf{J<8gCJNNpOzf
zGQk!7OY&BX71=n!G2<S$@C|r!7^Ek;GwUIJK6}%dHIcrZ-Qs=yAX{=aslex2<d?$@
xq8`Bw-OHDFOPBddCe11L);hfCVyq+Aif#03`uF^dV>Y{~n+xaoy6!HN{{b^=6oUW&

delta 1307
zcmY*ZOOG2x5bo-EKm48<+X+&TB0;Q3L=ZNRNKuv$2ohqmo2*C(AsD5XC3e=ZV@KME
z8u?%ja9C+k=KvhwCm^(c0C9te0}`jr1vnt~z&!_~P}O4|nNfZ9_19HB?dodZU-)q$
zI9{oQv)}%_Ccg<j^rp5bpANZ*>_xJ1CWSy4h?yvC@<84pFUh;)E%Kg%!bcDNu29ew
zMSwvODp^G9qKL^VWL2^nS)Hu016zH{X_B?b60%f*e(1SRY<-6z2DY%o5Z?cYxNM7|
z7}-*Q*n#}1J28JqAM?A@Xj!}=M)Hby<kPO%hCP(|&zLIbIZ>ooYA$JiU}m&zu6d;W
zg>9p?u{mFfehB3?u{9qDX57NW#3ksnpBDb+=(50ww)On7ph*~7#QPWz99k+lC}scx
z>olU&ax_YKR>40rb5^04)M!;=nPD|zZNnPGOd&9A3l+}Sy=kN(++4jZCVUj0ie?gs
zt9asLJcCKVNa3YgA+>H)!cs+RN9Hk#L5MNdv4It=Vr|_*M^(2S+PqPB?**p-I@*m+
z?7kM|03YaY<5$4ijVhisU8XL{R~RlbTq4-BMz=Rs_o9upcgDB#wT-pWB)53d#2#nH
z3HwIltbK<yPThBztuj#KiJo2NJi7k=Lwb5%{wzi4PwOWREpq$|3}+d*aZP%Dj@e84
z_xfk>rJgpvf~)#a^M`|$a=0ky4t!;+Xq(T8d92#{L9@4Fsiv9t{x{D&Rvjt~ms1P^
z)lvy(5)aW<Y1^FlBvR)5NR4OIv`SnOy{>+@^|k-S=lpzsMkmS9<_+3ePnKmdyrNf<
z!{MGkUYopq=hn>)JxT7m_bF=pjIO4S)gY#~YK6dW40<6w`|phnDU3_VKYaW#!Gvxi
z_YU1Qo6OmI9pRjKYl-fx7y!CTDr;L_vkI^9>Ap%OhH|_}HIOKWUVahnfrSCJKPl^$
z=|aaS{(s75rXQrWnvD)hv}QKdkhspWQx@cB^p)(%bcwc-#|$SKh6KG(-^~uE>pXss
z;WWd`4CnPv+0zMCm;QBVEWihw-Vl=AtbGjJveWddouh8qY8pE?K<QsPN8COya8*~k
z=c6~7`V7}~zk384`jhTrR+f6T2l)k>>dB@27J2<^_oQof+1HOBI1Lx{^#jG<3?LTI

diff --git a/CoLo-AT/data_analysis/data_analyzer.py b/CoLo-AT/data_analysis/data_analyzer.py
index 30302e3..0e2fd62 100644
--- a/CoLo-AT/data_analysis/data_analyzer.py
+++ b/CoLo-AT/data_analysis/data_analyzer.py
@@ -76,7 +76,7 @@ class Analyzer():
 
 
 
-	def calculate_loc_err_and_trace_state_variance_per_run(self, data_recorder, unit_time_interval = 1, plot_graphs = True):
+	def calculate_loc_err_and_trace_state_variance_per_run(self, data_recorder, unit_time_interval = 1, plot_graphs = True, selected_labels = None):
 		#recorded_dataline = [time, robot_label, est_x_pos, est_y_pos, trace_state_var, gt_x_pos, gt_y_pos, loc_err, update type] 
 		
 		data = data_recorder.get_recorded_data()
@@ -121,9 +121,11 @@ class Analyzer():
 					comm_count +=1
 
 
-				loc_err_per_time_iterval+= data_in_time_order[time_index][7]
-				trace_per_time_iterval += data_in_time_order[time_index][4]
-				num_dataline_per_time_iterval+=1
+				
+				if selected_labels == None or data_in_time_order[time_index][1] in selected_labels:
+					loc_err_per_time_iterval+= data_in_time_order[time_index][7]
+					trace_per_time_iterval += data_in_time_order[time_index][4]
+					num_dataline_per_time_iterval+=1
 				
 				time_index += 1
 
@@ -233,7 +235,7 @@ class Analyzer():
 		fig1.set_title('Location error')
 		fig1.set_xlabel('Time[s]')
 		fig1.set_ylabel('RMS[m]') 
-		fig1.set_ylim(0, 2)
+		fig1.set_ylim(0, 0.5)
 		fig1.legend(loc=1)
 		fig1.legend(loc='center left', bbox_to_anchor=(1, 0.5))
 		#fig1.tick_params(labelsize=18)
@@ -241,7 +243,7 @@ class Analyzer():
 		fig2.set_title('Trace of state covariance')
 		fig2.set_xlabel('Time [s]')
 		fig2.set_ylabel('Sigma_s [m^2]')
-		fig2.set_ylim(0, 0.2)
+		#fig2.set_ylim(0, 0.2)
 		fig2.legend(loc=1)
 		fig2.legend(loc='center left', bbox_to_anchor=(1, 0.5))
 		#fig2.tick_params(labelsize=18)
@@ -255,12 +257,12 @@ class Analyzer():
 
 
 
-	def algos_comparison(self, arr_data_recorder, only_trace=None, graph_name = 'Algorithm Comparison'):
+	def algos_comparison(self, arr_data_recorder, only_trace=None, graph_name = 'Algorithm Comparison', selected_labels = None):
 		print("************Algorithm Comparison***************")
 		arr_loc_err = []
 		arr_trace = []
 		for data_recorder in arr_data_recorder:
-			loc_err_per_run, trace_per_run, time_stamps = self.calculate_loc_err_and_trace_state_variance_per_run(data_recorder, plot_graphs = False)
+			loc_err_per_run, trace_per_run, time_stamps = self.calculate_loc_err_and_trace_state_variance_per_run(data_recorder, plot_graphs = False, selected_labels = selected_labels)
 			if only_trace == None or data_recorder.name not in only_trace:
 				arr_loc_err.append([time_stamps, loc_err_per_run, data_recorder.name])
 			arr_trace.append([time_stamps, trace_per_run, data_recorder.name] )
diff --git a/CoLo-AT/dataset_manager/__pycache__/simulated_dataset_manager_w.cpython-36.pyc b/CoLo-AT/dataset_manager/__pycache__/simulated_dataset_manager_w.cpython-36.pyc
index 995425e8175bc1ff852dc294c58f967c09f771fb..43e34a0be729cd1dc1ba44c010498b30d68197cb 100644
GIT binary patch
delta 1327
zcmbW1-)kII6vywmGdnvwv$NTqnf)=FM549P-NqkaZ7l=}7Am!*t%){JlWr4lx?~e~
z)7iCZa;Hm}yo!Mn@kOfC_kiRNXdgu^6pAkqszt#!5np@}^xWMwsC^M<xSw;+cg~zS
zbLZT-dGyoc)(ehf)#NX8JBPlo?rX4v547+pPxxExk|jn(K@_*(k}XOiASErzVwkKW
zMrK&$F>L8==NMzG%?S+FNAQ~N?eWAshckF3c@yVN8&l@%-r2u`3|8OpcVBt9zO}bz
zj&nGHA6u=irf#Q0r=e1vqS3xc5G>Lp2w34^S@NEp7Ch3LFOjN|`aG%1P75IQ2~u_C
z4PsCCV$%mP3CgBX<;J<tr|e->p~AYvBWgOKkRw#|@GzPERJhowtu8FCF6|qwwc4_H
zZ{O%Fg!QFPFRTw&MY^rU_ELC#p|jj-*7_j}q_@ii20EB@8vJi0U+3%NGs|z+@Ev`~
zd-l-;=%mN-h+Ty?UbJJlj{Ei|ypMC~8zAx9^eyP(4QFcPM_Muo4HmOVr)5Y;Ei5U!
z&6{`d2WL5F2{Up!&@dY*`k<+p<C~dxyFw;KW`%D<xGRPtN3zBcjifXJCX+FbT<Hq7
zqHXiANqSaf12)GaFY=`)trCm!(w8>bLMJB+BIhxgS1hs^de5>%lJ0Y3USv_R<28I)
zkolO)bcw0!-9|rvBnl`=Q)FdPmW2C>E_IQfWWtFO%{x?Nj*6rr{zH+5bOR>zfJFg4
zy6+{(E6ZR|a`~X-?*FLy-z7`;(ULobgH@z=2PREvNn56c*LyL4lJ$KY&gMspQjdXR
zpiz=eZ^jy0BZ!$~wK7g+52wl!uBw(T;zstxp?fC>+h^xwB@xcz?d(~YMLYKl@kDOI
zuB&ci?B4ClFxGQ*IFDcD4#6q>IoC*pQ*;HdxNqUFZXUmMtNOeW46mArY0_RLFx8bf
z=>;!mz^ATN^dZfCFu6s(3l?dri=Tus@(gXD^2|SU9kPHQdfWCa<qR(?)UfJT^J-Iv
zErm6OYYHJg3_O&60lr4`XW+u#PyTVxuksjhtndLmz}><}@E!h<ybRyt=f&UQ8~m&^
zq5H7@xccGX-9pAVSAo-&4E|Pnu5w-_y`b=#!nDGS!W#;-*;%-$u!XM%Bcn%Y+Aa`G
aZfeJ#HeFL2oVs-eH-j-)!dt-xSbqW%1TL%q

delta 972
zcmb7@yH6BB9LMLkyLY#@+}>j!w+|$WfN%;?g0CnE6^T!bo;C<v)DiWN0~d&bvk<db
zNHD?tVj&@z*l3J}ij9SqYGp%5L9jOb2Nuo{P&(sI@|oXb<~QH${O0+Q#e-U#u4|L)
z>piWz-f7D+%;KsX)K*iA+SAaj(L8mgNp}@>smJFO^^2siW5vWLx{0$&9~@fxsW1sn
z;LXG$oW##=pLvQ9G9q!r$&LAE;(N4oPu(NIQGBiq;d{LSFQ)cF38zw#esyaxfr&<5
z)x}1=7p~(&eNw-<EmyyhYf4YT0KQD0bzk!X7?epwLWRkJMInsMNtGGQW^QE^s)qU~
zl+|*QCL_R^%-yL2EJ4!)(i{Y{G!yEKlruEzksc{DLX#Oxb4h41lU4EA9?deF=1jsY
zL5D=JQB9)>zAqO^4hi$4M%iRGvm%M5T(T|CBRuagm1bF<In-ECm@-9E?S$%Ktn!Kr
z$#Ef4aBx#dg2idtA-uGmCmso1ZpE4F5vp*{`{Tv9=QD3ps{db7+y0@Ixqr28NyYwL
zOHH0B5rFvD4%0U*QkKhZ#O0Pee4A@XdJOty-WGXW%^k|Db!=Yx$BsfQDB^D8G!(I7
zv~VsM%~HX~W#a~Rm^<M(mdvuc5+52a-J<vKyLnBy#IGDXt$o;P**InuQbVE#M3WFu
z@r~sjOMwLnq&Y1}NfxM*w&AOwLOtvP1LCrXSbR;I$F`!1j0t<Ls>mw^-2#{Kg}v9l
zAk5nWg8~(SA^c$%-~(3Wi_o=H${ztKl5pO63}3LxeF|Sio5MG(@qWQ4^!;YVfQee(
zpceM|2IzRl-(P=5R5~lrC2&sQyZ{rJ6nKEW)ef%tvGz9Zj#wP3q{@dIRe5tLTBn)}
h>Ug4%U5Qo328YLkq}VE+*NQwcJ`!BPy6XFo{0%yo;2{72

diff --git a/CoLo-AT/dataset_manager/simulated_dataset_manager_w.py b/CoLo-AT/dataset_manager/simulated_dataset_manager_w.py
index e7a30a5..eac9c42 100755
--- a/CoLo-AT/dataset_manager/simulated_dataset_manager_w.py
+++ b/CoLo-AT/dataset_manager/simulated_dataset_manager_w.py
@@ -48,6 +48,7 @@ def linear_interpolation(end0, end1, x):
         y = y0+(x-x0)*(y1-y0)/(x1-x0)
     return y
 
+
 class Sim_Dataset_Manager:
     
     def __init__(self, dataset_name):
@@ -73,7 +74,7 @@ class Sim_Dataset_Manager:
         self.odometry_data = [[] for i in range(self.num_robots)]
         self.groundtruth_data = [[] for i in range(self.num_robots)]
         #noise 
-        self.noise_v = 0.0125
+        self.noise_v = 0.01
         self.noise_range = 0.1
         self.noise_bearing = 0.035
          
@@ -92,6 +93,25 @@ class Sim_Dataset_Manager:
         self.dataset_data = {'odometry': self.odometry_data, 'measurement': self.measurement_data, 'groundtruth': self.groundtruth_data}
         return self.start_time, self.starting_states, self.dataset_data, self.time_arr
 
+    
+    def observation_target(self, rbt_lable, op_idx):
+        target = None
+        if rbt_lable == 1:
+            if op_idx == 1:
+                target = 6
+
+        elif rbt_lable == 2:
+            if op_idx == 2: 
+                target = 1
+
+        elif rbt_lable == 3:
+            if op_idx == 1:
+                target = 6
+            elif op_idx == 2:
+                target = 4
+
+        return target
+
     def get_start_time(self):
         return self.start_time
 
@@ -110,6 +130,7 @@ class Sim_Dataset_Manager:
 
     def respond(self, req, current_time, need_specific_time = False):
         message = req.get_message()
+        valid_respond = False
         if need_specific_time:
             valid_respond = False
         else:
@@ -127,9 +148,9 @@ class Sim_Dataset_Manager:
 
             if op_idx == 0: #propagation 
                 req_type = 'odometry'
-                actual_v = np.random.uniform(0.5) # between 0 to 0.5 m/s
+                actual_v = np.random.uniform(-0.25, 0.25) # between 0 to 0.5 m/s
                 input_v = actual_v + np.random.normal(0, self.noise_v)
-                actual_av = 0.1
+                actual_av = 0.02
                 delta_t = self.op_time_interval[rbt_idx, op_idx]
                 gt_x = gt_x + actual_v*delta_t*cos(gt_orientation)
                 gt_y = gt_y + actual_v*delta_t*sin(gt_orientation)
@@ -140,28 +161,31 @@ class Sim_Dataset_Manager:
 
             elif op_idx == 1: # landmark observation 
                 req_type = 'measurement'
-                [lm_x, lm_y]= self.landmark_map[6]
-                delta_x = lm_x - gt_x
-                delta_y = lm_y - gt_y
-                meas_range = sqrt(delta_y**2+delta_x**2) + np.random.normal(0, self.noise_range)
-                bearing =  math.atan2(delta_y, delta_x) - gt_orientation + np.random.normal(0, self.noise_bearing)
-                message['data'] = {'time':req_time, 'subject_ID':6, 'measurment_range': meas_range, 'bearing':bearing}
-                valid_respond = True
+                observed_label = self.observation_target(self.robot_lables[rbt_idx], op_idx)
+                if observed_label != None:
+                    [lm_x, lm_y]= self.landmark_map[6]
+                    delta_x = lm_x - gt_x
+                    delta_y = lm_y - gt_y
+                    meas_range = sqrt(delta_y**2+delta_x**2) + np.random.normal(0, self.noise_range)
+                    bearing =  math.atan2(delta_y, delta_x) - gt_orientation + np.random.normal(0, self.noise_bearing)
+                    message['data'] = {'time':req_time, 'subject_ID':6, 'measurment_range': meas_range, 'bearing':bearing}
+                    valid_respond = True
 
 
             elif op_idx == 2: # relative observation
                 req_type = 'measurement'
-                observed_label = self.robot_lables[(rbt_idx+1)%self.num_robots]
-                obser_x = self.current_states[observed_label]['x']
-                obser_y = self.current_states[observed_label]['y']
-                delta_x = obser_x - gt_x
-                delta_y = obser_y - gt_y
-                meas_range = sqrt(delta_y**2+delta_x**2) + np.random.normal(0, self.noise_range)
-                bearing =  math.atan2(delta_y, delta_x) - gt_orientation + np.random.normal(0, self.noise_bearing)
-                message['data'] = {'time':req_time, 'subject_ID': observed_label, 'measurment_range': meas_range, 'bearing':bearing}
-                valid_respond = True
-
-
+                observed_label = self.observation_target(self.robot_lables[rbt_idx], op_idx)
+                if observed_label != None:
+                    #observed_label = self.robot_lables[(rbt_idx+1)%self.num_robots]
+                    obser_x = self.current_states[observed_label]['x']
+                    obser_y = self.current_states[observed_label]['y']
+                    delta_x = obser_x - gt_x
+                    delta_y = obser_y - gt_y
+                    meas_range = sqrt(delta_y**2+delta_x**2) + np.random.normal(0, self.noise_range)
+                    bearing =  math.atan2(delta_y, delta_x) - gt_orientation + np.random.normal(0, self.noise_bearing)
+                    message['data'] = {'time':req_time, 'subject_ID': observed_label, 'measurment_range': meas_range, 'bearing':bearing}
+                    valid_respond = True
+                    print(self.robot_lables[rbt_idx], observed_label)
             message['groundtruth'] =  {'time':req_time, 'x_pos':gt_x, 'y_pos':gt_y, 'orientation':gt_orientation}
             self.current_states[self.robot_lables[rbt_idx]] = {'x': gt_x, 'y': gt_y, 'orientation': gt_orientation} 
             
@@ -209,5 +233,5 @@ class Sim_Dataset_Manager:
         for label in self.robot_lables:
             self.current_states[label] = {'x': self.starting_states[label][1], 'y': self.starting_states[label][2], 'orientation': self.starting_states[label][3]} 
 
-        self.time_arr = np.zeros(self.num_robots, self.num_op)
+        self.time_arr = np.zeros((self.num_robots, self.num_op))        
         return self.start_time, self.starting_states, self.dataset_data, self.time_arr
\ No newline at end of file
diff --git a/CoLo-AT/gs_ci_journal_sim.py b/CoLo-AT/gs_ci_journal_sim.py
index 32d081d..7adbe72 100644
--- a/CoLo-AT/gs_ci_journal_sim.py
+++ b/CoLo-AT/gs_ci_journal_sim.py
@@ -23,7 +23,7 @@ from gs_ci_bound import GS_CI_Bound
 
 
 ##############################################################################
-duration = 60
+duration = 200
 robot_labels = [1,2,3,4]
 testing_dataset = Sim_Dataset_Manager('testing')
 start_time, starting_states, dataset_data, time_arr = testing_dataset.circular_path_4_robots(duration)
@@ -34,15 +34,14 @@ robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
 sim = SimulationManager('sim gs_ci')
 state_recorder = StatesRecorder('gs_ci', robot_labels)
 
-sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = False)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = True)
 
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = True)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = True, selected_labels = [1])
 
 analyzer.trajectory_plot(state_recorder)
 
 
-#animate_plot(robot_labels, state_recorder, analyzer, testing_dataset.get_landmark_map())
-'''
+
 ##############################################################################
 
 testing_dataset.dataset_reset()
@@ -51,8 +50,51 @@ robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
 
 sim = SimulationManager('sim gs_ci_bound')
 state_recorder_bound = StatesRecorder('gs_ci_bound', robot_labels, state_var_only = True)
-sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_bound, simple_plot = False)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_bound, plot_graphs = True)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_bound, simple_plot = True)
+loc_err_per_run, trace_per_run_b, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_bound, plot_graphs = True, selected_labels = [1])
+
+
+testing_dataset.dataset_reset()
+loc_algo = Centralized_EKF('algo')
+robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
+
+sim = SimulationManager('sim cen_ekf')
+state_recorder_LS_cen = StatesRecorder('LS_cen', robot_labels)
+	sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_cen, simple_plot = False, robots_cant_observe_lm = robots_cant_observe_lm)
+	loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_cen, plot_graphs = False)
+
+	##############################################################################
+
+	testing_dataset.dataset_reset()
+	loc_algo = EKF_LS_BDA('algo')
+	robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
+
+	sim = SimulationManager('sim ls_bda')
+	state_recorder_LS_BDA = StatesRecorder('LS_BDA', robot_labels)
+	sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_BDA, simple_plot = False, robots_cant_observe_lm = robots_cant_observe_lm)
+	loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_BDA, plot_graphs = False)
+
+
+	##############################################################################
+
+	testing_dataset.dataset_reset()
+	loc_algo = EKF_LS_CI('algo')
+	robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
+
+	sim = SimulationManager('sim ls_ci')
+	state_recorder_LS_CI = StatesRecorder('LS_CI', robot_labels)
+	sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_CI, simple_plot = False, robots_cant_observe_lm = robots_cant_observe_lm)
+	loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_CI, plot_graphs = False)
+
+	##############################################################################
+
+	testing_dataset.dataset_reset()
+	loc_algo = EKF_GS_SCI2('algo')
+	robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
+
+	sim = SimulationManager('sim gs_sci')
+	state_recorder_GS_SCI = StatesRecorder('GS_SCI', robot_labels)
+	sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_GS_SCI, simple_plot = False, robots_cant_observe_lm = robots_cant_observe_lm)
+	loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_GS_SCI, plot_graphs = False)
 
-analyzer.algos_comparison([state_recorder, state_recorder_bound], only_trace = ['gs_ci_bound'])
-'''
\ No newline at end of file
+analyzer.algos_comparison([state_recorder, state_recorder_bound], only_trace = ['gs_ci_bound'], selected_labels = [1])
\ No newline at end of file
diff --git a/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-36.pyc
index e0fb6da36a8122d0230cf0396f055b6f7bee79b8..c9eaca6174e11a6bd773ac94e0954da503cad1bf 100644
GIT binary patch
delta 819
zcmZvZ%WD%+6vpqp^Uh;38E2ZNR!fyMMnpwh(`v9SJ}8O>b)ldTTBZ$CF{wARaB+f<
zSiyy;+=aSv(?!>%U1X(yf(qgy8=<a>8xdT%D0olXR45Ld-#vWuoyXky(Eq#NI+;wy
zmLLDTy|HY)W=3oO!jG%1cVdYx(lK#a88IU=M?q~;@l^R@cM(Di8RulwzlzVsUOk|V
zSir<HwK#4BT9z~%P(hbx8mxv3ZXk0=d0XKIMu~tOF-SM5M;d!TfkeP&S(Zc!q{SaK
zPt&4b%Rle}1w85S!2|scdrA3e(7*>jP%>nJ{mW*VM&tw=Y>#yMg4@aG^d|90ktn1@
z?ybpb{?0T9yew%_h|Aq%ZH9E-$_<sX(x_(0wpoS$o3|aZVApCw)ANVdZeU9lw2E?_
z6wJwryfpcI8s$_T+>uzvY+VsI^-0<#mh^{CsuGoJJe;XUQ&Cu}*2RD^-g`p22u)Cv
zs!Ww(Qb*<{Ds1JE@r%+S*`n7>@B`Aw4~jQdAI*uDH9!Z%H><lRkMbdjwpkA=cc;p=
zY3`$TOr&G4Xh|%_=IN^ZpMxtC@mti3;-Etp_}!$*D1KcQTrp<(;(fx^hp=#S;FZom
zR9wU<L>!Sr6cG%eAXLP#L|dEZVKplBa2us=#O9`>JDc(m6t|YdY@b9sUYoAZ@N!rU
zX72MdD3=g;KW%lQ9L|Zi$v*u&Y8S+6vY=1Nv_34pCyUe;S^J}l(+l>lu^Ew0ar#_5
bNrlcJ7GFeMLR=FYsiRuX%$Y~Tap%QfkL9sZ

delta 832
zcmaKp&uddb5XX1-{mieIm*kaaT12BwL(pPqYePzGt1VP4)Pn~P3c>P{n2J_6J&0~l
zs2&ud${f53f`SJ@auB=}{R6~<AmpSsdk{nrJUWvm7K`{`KKp&MGdo{)e`Yo_q48KO
zJh}Dl{EJti$HKbly<GTm;|@F*Px%xqY9*m8c%XfB1{h-&#vDfTJ$$!@%z(GTfdLzO
zv7{?A&04MswGt{3pjL5jI}(9aX3AL;%3bFkYbBMYqJgN1G+Wb@3zm`NE*vp(w^D(o
zQdr@slrk@ghg^;+LwU*zG@soSJE|rOh7%m+c&vY}O7!)Lxz0RRWEv~8YaN{C{h6k#
zRGO{xt~c!US$`KDYSUP#`>a>2tA7{wDzIUfHAtDc!=1Bqq^^v*c82Bcr8-^~exGL~
z$3?n`EmC<dIRxesAAq)b8|JOq!2+fkk8+#qLdUSVi`vFa`-$~~XUC~c$o;r<@t2Ou
z4CN2N>(D+v44*<HJPU=mKO{#epF?!)W}|-jLS=PX(o@P&D1;y36Tb^z<<IarC))MM
zISwD9qZetE{O`O-ZypsU2@`|}VVH1~AP5>kCrl#pgVG_rhtPFc-Ek<3l-)K*YA>P_
zSzT^cWu<W?s9u*PN>35!wmSOBN@GQq>HipEnlM9{B^)Q5Ak4wnSl+ClY)*pj6yaAm
r4i#rJ+4bb?@^p{ihd1$tJ4UVZgayKBc<cIb%Jq$dcGf-wHFx7Taw4Uh

diff --git a/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-36.pyc
index 0011e92cb63c4fc5e20457ee0cf17255617a39e5..ae10aa7792e6ae1c7580c28e4258839aa5d64c59 100644
GIT binary patch
delta 56
zcmca8bzO?nn3tF9QqzZ+4;wi(nAjP>;KF1Jru~ddCjVes!ssx0F0&;^2}2fR4NEiQ
J<{QkFoB;lF5t0A^

delta 50
zcmcaEby14bn3tF9e$4Hdw;MS%nAm5{1c9BCEtvK*woLxPw1m-W@?2(1rW*Fm_n0d=
E0r!v-TL1t6

diff --git a/CoLo-AT/localization_algos/ekf_gs_ci2.py b/CoLo-AT/localization_algos/ekf_gs_ci2.py
index 9fe960f..13aa509 100644
--- a/CoLo-AT/localization_algos/ekf_gs_ci2.py
+++ b/CoLo-AT/localization_algos/ekf_gs_ci2.py
@@ -12,27 +12,29 @@ class EKF_GS_CI2(ekf_algo_framework):
 		ekf_algo_framework.__init__(self, algo_name)
 
 	def state_variance_init(self, num_robots):
-		return 0.01*np.matrix(np.identity(2*num_robots), dtype = float)
+		return 0.03*np.matrix(np.identity(2*num_robots), dtype = float)
 
 	def calculate_trace_state_variance(self, robot_data):
 		[s, orinetations, sigma_s, index] = robot_data
 		i = 2*index
 		trace_state_var = np.trace(sigma_s[i:i+2, i:i+2])
-		return trace_state_var
+		return np.trace(sigma_s)
 
 	def propagation_update(self, robot_data, sensor_data):
 		[s, orinetations, sigma_s, index] = robot_data
 		[measurement_data, sensor_covariance] = sensor_data
 		sigma_odo = sensor_covariance
 
-		var_v = 0.5# variance of the velocity
-		exp_v = 0, # expected vecolcity for other robots 
+		var_v = 0.25# variance of the velocity
+		exp_v = 0 # expected vecolcity for other robots 
 		
 		i = 2*index
 		num_robots = int(len(s)/2)
 		delta_t = measurement_data[0]
 		v = measurement_data[1]
 		orinetations[index] = measurement_data[2]
+		gt_orientations = measurement_data[3]
+
 		self_theta = orinetations[index]
 
 		Q = sigma_odo
@@ -49,8 +51,8 @@ class EKF_GS_CI2(ekf_algo_framework):
 				#print(np.matmul(sigma_odo,rot_mtx(self_theta).getT()))
 				sigma_s[jj:jj+2, jj:jj+2] += delta_t*delta_t*rot_mtx(self_theta)*Q*rot_mtx(self_theta).getT()
 			else:
-				s[jj,0] = s[jj,0] + cos(orinetations[j])*v*delta_t   #x
-				s[jj+1,0] = s[jj+1,0] + sin(orinetations[j])*v*delta_t #y
+				s[jj,0] = s[jj,0] + cos(gt_orientations[j])*exp_v*delta_t   #x
+				s[jj+1,0] = s[jj+1,0] + sin(gt_orientations[j])*exp_v*delta_t #y
 				sigma_s[jj:jj+2, jj:jj+2] += delta_t*delta_t*var_v*np.identity(2)
 
 		return [s, orinetations, sigma_s]
@@ -131,6 +133,7 @@ class EKF_GS_CI2(ekf_algo_framework):
 		kalman_gain = sigma_s*H.getT()*sigma_invention.getI()
 
 		s = s + kalman_gain*(z - z_hat)
+		print(index, z - z_hat)
 		sigma_s = sigma_s - kalman_gain*H*sigma_s
 
 		return [s, orinetations, sigma_s]
diff --git a/CoLo-AT/localization_algos/gs_ci_bound.py b/CoLo-AT/localization_algos/gs_ci_bound.py
index da3a820..44f0804 100644
--- a/CoLo-AT/localization_algos/gs_ci_bound.py
+++ b/CoLo-AT/localization_algos/gs_ci_bound.py
@@ -12,16 +12,16 @@ class GS_CI_Bound(ekf_algo_framework):
 		self.d_max = 2 # max measurement distance 
 		self.d_var = 0.2
 		self.bearing_var = 0.2
-		self.var_v = 0.1
+		self.var_v = 0.25
 
 	def state_variance_init(self, num_robots):
-		return 0.01*np.matrix(np.identity(2*num_robots), dtype = float)
+		return 0.04*np.matrix(np.identity(2*num_robots), dtype = float)
 
 	def calculate_trace_state_variance(self, robot_data):
 		[s, orinetations, sigma_s, index] = robot_data
 		i = 2*index
 		trace_state_var = np.trace(sigma_s[i:i+2, i:i+2])
-		return trace_state_var
+		return np.trace(sigma_s)
 
 	#def propagation_update(self, s, th_sigma_s, sigma_odo, index, odo_freq):
 	def propagation_update(self, robot_data, sensor_data):
diff --git a/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc
index a3749f65089c47a4d5adccc0f77e5a142a6440b3..e6f893b9bb34363e909e83cc8442b5731bb39e34 100644
GIT binary patch
delta 1711
zcmZuy&2QX96rZtoy<V@qA5PNjCYx>2M9`%WR29)mv_&dJszB<Y<$ywn<?NWn-mJZh
zomCRqio`-MJ&-UQK}fy92?+^K+&Lib%wNE##ECO+#@mpVt~Adx@BQ9y-n^Oj_Ce=l
z*|=g^#xK3Uw_b0a7(dx>AcQ=$i3!?N2;NkQee&yNt4<mpp-qD*_fU6+b?`2J$R1!;
zO{}1rqM$IrDROG811@zoQGzyC!VxbXdNhi8t)Dm%^}R506F&-L_LtJeoEhpujhBfZ
zJ2cvflCKb5U{}-)+-3*r68lVj_|h!I!fSEj(!>Foc#cacod+##3bAl17R77{#2O0<
zch`lXt5D%jO(7mfa+7)=?0IqG5I1ozvEK{Zs}j5X6s*1*xx^W|qZ@2ce0NR8Walvr
zqp=_Mox~q{@!2idpT$M|D$}(Wk5ph3Kk*ggH5hRfM5;jEP>Qc3f*`5*P8ZV|(05fC
zHZReg#PP}groA_UD7~{8Ji8aT@Ht5E|53NVbSqXsB7C@VSLFEKL7aHQ^{2vn5((fA
z3=M`1)l}IX?JEA6-Pf)jp`e(mX@OWRG*K{0O=1iX{W{S|6|^|l!m0*GE2Zjmt&{o$
ze~42v)pK94mivXw%6)R|b##owowP_A1LYXg?-DgA5Sti%g&3sVSNh5Xvx}um-EHui
zX(4iMPypOEna@hVU<8%4Ho=*l+5`<$u)NLE(rUvTXfXejv?R(5EcQ)lwWN_n@G%)L
zt=zasmcS-CGZ8mnB-1R+tKj<&d~4}!3kCHF+ETN*>|8b<)`d2xvupYbM;hr&Fl0kJ
zR_M*tN-KROYs%H<l0%_m$pQHY8fhtQPL!=e*2)&LMHy91WGb*HC~JpnlD7d_7C7~B
zTwtdvoUt9=p7Q=E&@W5(JUB^ux>-)ymXuG}1Dh?B--05H%C{|EIa8hN`|{0i72^Va
zbB0&?UgE&pj@`Z|>xk<@lN^jZ+JG0rwV|6(f1f@B3tAWAf>4(?_Iut))Vh4qbeWOL
zb2V<}HJrax&c3L$k7PBeG#>HNqkWzg4CZtt3zX{skh^ko?C$zx`q=3;0mWmky3UxF
zW4}Liorpxd5qn`AQKuJ;UFy4G&!f*v)~VhPM|%mcfP?hdi9O1-XeWSr<Hpc)p(D8=
z9Qm?z+I2%Rbm_p^je3wOfp+|mc>CM{J|b3RpI3X)aOjAlm8U$-sc3kdeQ7q|gAMbr
zMAPAhXg?AP4N}afcOdchsnL0K0&XSL?Ridq%9vMV)65yp_5-$LHMI>0JigCfvCdcO
zijJ$ej&;RmJ?mp!VZT}HM{NMeeo4=Z*@_TrLdctZN6h|fnQ|sOY!M6=j9A>Yt`@K{
zqnC7BS9DcZsjzmdT!EIa#s@JkhI_-&LH<$-BR6@U)&Ri^LBu|(&b4?WKSB@w7${up
Vp+~4_k(~Wd?bM&#p8Zi>{ugkx*w6p~

delta 1459
zcmZux&5zqe6!+Nqa2&@@vLE@_bazoI#IB$fP*Eh7mamGcM?gXvEm)I`73?H-7(3A}
zvRR3Mgv4c6!-c8>)EnXm;=rGP`WKiRr(Quq;)2ARak{jHNb~YOe(yK)#_!G9?ibs}
z^R=4s=gzN(FSX8$d(GDrMR663sDf3Butw`=_n$4QZ!3EZ{v?{9FF8k?Ux8SSf2I0-
zN9!TX545h0E7Xe|77oJrJB4j<SG$3_{Cn*J|3&-uN&|SY9mRyj4#?Pb2xDvwylh>F
zjdQUnv^I!7|GMzjB@x(DnaF3R5RW1=ler(x-6(b_iOD|SF23KF*zW&e^+`ymGbOVF
z{!Q^c)Zl*?+tr8o_-<(nUE>F(%Pkes#cvx{3D)bdqQ6xBbona!nbY#scTH-H6?Qu=
z(pplWsG}@Zq=025ieE~!m0hOx5`BUaGtmK6?<gZJUEy=wfpUt1K~kiRv3iQwU0%|!
z@7)6IT1TOq;{y0@(N<apwC-0DU`v}xlPY5kB5&|O?>CHb3G|O?N1_bB%J1s^a*=L9
zj=}HgPrSTNFF;Ikrnd0<Rmrptx(T_z!GA4T?<l^tR1URtBi&3}L0x!#i|<#SIVsZZ
zB|=J4I#t<zQcJ2MHEm1lbIGByf#iU?eLE>9?WKBHNIPja-IAxOi5v0jOC{|E7bUNq
zXcDJAiu3D<1~vAAn=9TQ1^NY<o*zz<o^O^@_9W$Falg(@;~i*EV%+3E8P}>I^V|TM
z&kKVe{xB}0R~lJ$<i^g_jUqB~Wq)P5@WjV6H&cUIR+^HSc}J`c-=FnBWR)97Lw6=P
zSeFxX-^OO%(DNCzpo{r!vv(p|Fw<Bx%gPVPcff_sl_k3s>j02jVRJzy9(8gGc3EVj
zMWzvFfh&<Wni40ZA#Ox&5QWSch6}<x5)56|m#izl7tH1{u0n#$7(_11O5wnFJc^B}
zOJF=n0CheklO`mfQ^Lm1Bpl*uDA0}<Q1>V^fR74`9${-3PNxnVti~3A`6WXA5$1Pl
z?GxBAKbDw4Y=~Ku7ZPDx=nvzY=RxN|1-KS7GIX81$|$SK7bItReAd_Cq;O9k$uGFN
ztXGRrO!O&#(b}o#$U=3bs}>(vpCN<aw=SKuAS|B~))Cs45Mrp=bs_#8eyPc~M-bLv
zMHUEMD<Gq(m-VKu>KYTVJu6egDAS_jC@Ti@>Fk(22kxveBk>2!mZ~3mK~|<7<9BT9
qP29+f8bbFmQGiRC1~}Zz{qS%?lMUvvw{Tej0&<`K9FG5P@BRZF^o=e6

diff --git a/CoLo-AT/robots/robot_system.py b/CoLo-AT/robots/robot_system.py
index e072873..075be13 100644
--- a/CoLo-AT/robots/robot_system.py
+++ b/CoLo-AT/robots/robot_system.py
@@ -31,6 +31,7 @@ class RobotSystem:
 		else:
 			self.robot_sys = CentralizedRobotSystem(name, robot_labels, loc_algo)
 			print('created a centralized system for ', self.num_robots, ' robots ')
+		self.gt_orientations = np.zeros(self.num_robots)
 
 	def set_starting_state(self, start_state_arr):
 		if self.distr_sys:
@@ -62,6 +63,9 @@ class RobotSystem:
 	def set_start_moving_times(self, start_moving_times): 
 		self.prev_prop_times = start_moving_times
 
+	def gt_update_orientation(self, rbt_idx, update_orientation):
+		self.gt_orientations[rbt_idx] = update_orientation
+
 	def localization_update(self, rsp):
 		#message format: {'time': float, 'robot_index': int, 'data': {'time': float, 'velocity': float, 'orientation': float}, 'groundtruth': None}
 		message = rsp.get_message()
@@ -82,12 +86,14 @@ class RobotSystem:
 			sensor_covariance = sigma_odo
 			delta_t = message_data['delta_t']
 			
+			self.gt_update_orientation(robot_index, message_data['orientation'])		
+
 			if delta_t <0:
 				print('current time: ', message_data['time'] )
 				print('prev_prop_time: ', self.prev_prop_times[robot_index])
 				raise Exception("Error incorrect delta_t!")
 			
-			sensor_input = [delta_t, v, message_data['orientation']]
+			sensor_input = [delta_t, v, message_data['orientation'], self.gt_orientations]
 			self.prev_prop_times[robot_index] = message_data['time']
 
 		elif rsp_type == 'measurement':
diff --git a/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc b/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc
index 9269b36a4870d9616b209d71121adb18806b365c..73c6d492da67ac9b0409341a80ca3aa9d5e6c4c5 100644
GIT binary patch
delta 1646
zcmZux&2Jk;6yF)I?e(r5yYaeFNIy1Ijk7?Uv`G<7n-pkBS|yE26F_YmmUVXGI`OXU
z*>y?6Mm-pDKtifI@JHlOKE#bnkK};3fy78igj+blt%3{l)(M1)!~FKWH@|s1^S&ON
zznZDhOh*0r&hK~mSE+q;D<Me|7VD~5{-A&mZOe|d9S@O%)-lx`9LZS7h7%LA9L87i
zf@I3`6bb%*1mJ=u1(*r;8NSGV!e_g?geoB!%SlBbEY=mfh_=|ZSov})%;-`ZZFdFI
z8g|qJE$b-4vo^mU7e^}}=%3H*ZGC!$$2Y7--RJR5_)zVUTo_oafOkoH`s6_P<%t>N
zvTFzTxa>D+z++^i!S2eF`K+LEjgl2oy-%oB322TzmhU9a2)nPdOuUB;#>XDO6W}dT
zBxE5tNZbY6vM{QtRCU#$exp<&ffZ~v$e=kfQFMvzw<vLm7Zk?@Cq>JFRdqIuiGR4X
zUakw9=a7v-Gottn%dvg?2*<Gld+<TD+xbgGSStFh@)}tQtm{+7Wn<E~VA!5x_@uU4
zioE*PitPnfz3dZupI9~5sQQLi4~+S`N1owYHM>k|{?fq(myN3TjHj{xxQbqrq9kZh
z&zQDrJ`s0c>|_{IC>jdsWk$l#UJ-&1(UTPRR$^3z+(khgXO9ve9{V_y))HYVRC}a#
z=^>^nD<&tjY@G{xF3hzNt0&JDcEjvLM0dm7n%<V2#5x9zeUU6`yP@t#>#<1ROZNBd
zigUeTA?&H(P!Eq+aJ$3pRi^bo5~YN~La!*}Q16x6F(ffd87di}ymm?)Jnd+oAS4N1
zy50F^;CgM0B>0Micg=4NvH8>q_M<X;o@+-^%+-Gr!evMr*DAE$@|=LSpzK!auFED<
z3u8RpNqZ;ghnm0X2gK#a|37(bC-p5FU`@63JXa2@jAubQQbY8Z2rkXAFV#MDmVKwb
zqn3n$W5=b#Y)CtkrY{0v24I9OYg3wX->y|1tKOhpY)9+sPs0|p1kC}Q05~atCtc$E
zc7?F-aUc6r%O|27uCjc30_p6{bRX97*R}Ky7!}#;*^hcvI2pA(=D0M%<yA_WCZe-!
zFWZZD*k9S(r*4B|v<lWkHdR1c60pN)ZU(9Bo7`~L1Y*)5wZOIl`T~2JGv$|rjm@P-
z9+;=;5FCQ1(qVve0OtWl0mc9>0K6=K$7;l*P?hu|sMu?8#io__?-5FP%&mHK95ye(
zrs5woG`$SADS%f2rU51Z=B@xjG-w9k4FNnRvdQIUs~U8I4MlH0*XfMtp4WgHUG_Tg
z;}9L6&anROyfz1WHvs0@mF~san-L>Q6j#KQv4W~gq6VXjz<L+p7Qg~9m8j*#py`DF
zAh-}$uCs?Z9WnMt_Y#V+8~J7-Dwwlyb^zdQfF^6|J?yu9ezIFBD4Kec!n`w&@wnG=
z8=Le!q49Jim&8P^Rm-#hCM>3}1A^QTbCyoBOZv$|6iaA-j^|$E2`YwE-DPX~)xQBu
CU$lw<

delta 1694
zcmZWp-ES0C6rX$NV`pb)Kf2v*3luCtAcIoCqC$ltw63Yr8f^{op_?f)Ec-FL+cUEW
zWSWw!(U?fkTpu<43w$vC0YqN#AMoWq8Q-dr7>$YX$#Z7gN@6DSo8LM2+&lN2-?{f^
z=Nl(ImCahsC%;`=_%^-C)>De2aC~R+4d)PIwx)T?njWhSrLTCb$>9ughMezGJhjQ5
zw%3eUY4C;`8-0dzv{=8vmlQ`^l$pSH!yfp7-;Sge_EsAH^{6^Ox2fY#98H=vKdMI0
z-s$=S>ba3CZRo6YqH4=s@mrzP>ut}!?WDvUKP__h9&w+S3pP$yn8r0W&TM?dgi#Vd
z@Pq72@r18TXX9*1!M)@n+?SWzvhtDI&>FgDdg(s*EX?6EFSDe0Hogk(H@^tB=&HLv
z*rZz2=ogCe_qi?d(>Y*hZ<Yv%EmyWf9&U#`bPTBlZgf*>;VMK@^>242Z+E2uZrkew
zpyE97Se+TBRb&SI>wf)~57k-(Wih5**}oh8;TQsDhxwVz<V99w`*@D!%p!Wk1MTFx
zhT}VWy@5u|c66#BN1fI<emQ>CSl!Z0g9_@TdP*~$FfB$c$<s8T;%Qg~9jjn0f`uzW
zhJfl^S$1^DB9VDg3&qK!I*BTfBln1C>mw78NA74~;0J~YhG@`)uv_)q&=r5_6Xh4s
z6%NF+879XLVVWr3ltn2e?KdIbH$EEb;=YVH9b2PHQ~8|1@8S>RY({Am;zFNwWSke%
zsT27}aiLMdo-~>qV;+fnsl&66;u2=4k{$ELSh7y3&(}tFtTI}ov7SUIja_j$9<A}X
z6pz$+f3VIksfw60k3MI_T4Nus@Qi2OXH1C{FVi1<ZCba_8P(6gs%6DjW@GPvm6KZ1
z%9$E;dTlR)UUXBON-vEiRod?G@J<-{0i3|p#l!StHYGl>CXXhyhU4gjQGz0ZEN_(_
zEC1Aza7_GZU9zb1l6wK163dwbS*cy`1OY72<P)NwnVZ*dxvjdFsDe$GoUSl(BRq5z
zSw)Ewv4FY~$^?f94kO4^;D@1G^I-%nvCBSSyn-b3#0C2-%ZYpTp6|Z4f8@*(VgBQ(
zahjJxaJb}?$E9`y{8fj+JKKIRk^N(xY1P9Bf8L$h5YHDbAE}~OvK#-41Q!Q3@C~Wo
z4dw2%_(Cg-g~CA&<KoxCvHVGznezNr<W?hiQ8+`6@e=y(PKph4Ttq|V^_S613J<4<
zohFzjpiJNl!C8VC1gW<CHoQvad4dT9X}VDR@TL#GRD*gOF3<oqt29Hpzwr73x;250
z;y}KO#9kwKT|qC*6D1|#O$4c;^io^x)uVw-$#uAUaCS&dSR_bx`xeFPRK^e%#M{MT
zys~g?nc!W~DK5`nBKjVMC5J7l2masyUZnw#;4;CLB&4$v;)R3Vkf^k!42dF3;`>60
zeJTzNUu9gZ53iP!{yRo<rwFbQ^n_kIGLtsP%(S%#bZng=skeJU_s*bLcETs|8f?{9
kU@rzp-U*>jMC;<^2o({O_7;;WlB$wlJP-Skh>g<hztB{bmjD0&

diff --git a/CoLo-AT/simulation_process/sim_manager.py b/CoLo-AT/simulation_process/sim_manager.py
index 4945153..3024da4 100644
--- a/CoLo-AT/simulation_process/sim_manager.py
+++ b/CoLo-AT/simulation_process/sim_manager.py
@@ -18,9 +18,18 @@ class SimulationManager():
 		receiver_idx = rbt_idx
 		#sender_idx = (rbt_idx+1)%(num_robots)
 		sender_idx = None
+		'''
 		if rsp.get_type()=='measurement' and rsp.get_data()['subject_ID'] <= 5:
 			sender_id = rsp.get_data()['subject_ID'] 
 			sender_idx = self.robot_labels.index(sender_id)
+		'''
+		
+		if rbt_idx == 0:
+			sender_idx = 1
+		
+		if rbt_idx == 1:
+			sender_idx = 2
+		
 		return receiver_idx, sender_idx
 
 	def simulated_communication(self, receiver_status, sender_status):
@@ -43,16 +52,15 @@ class SimulationManager():
 			return False
 		'''
 
-		if np.random.randint(0,20) <=1:
-			return True
-		else:
-			return False
+		return True
 	def allow_operation(self, rsp):
+		'''
 		if rsp.get_type()=='measurement':
 			rbt_idx = rsp.get_robot_index()
 			rsp_dasa = rsp.get_data()
 			if rsp_dasa['subject_ID'] > 5 and self.robots_cant_observe_lm is not None and self.robot_labels[rbt_idx] in self.robots_cant_observe_lm:
 				return False
+		'''
 		return True
 
 
@@ -83,7 +91,7 @@ class SimulationManager():
 			#print("Current time:", self.time)
 			g_req= request_response.Request_response(None, None)
 			prev_time = self.time 
-			valid, self.time , rsp = dm.respond(g_req, self.time)
+			valid_op, self.time , rsp = dm.respond(g_req, self.time)
 			#print(rsp.get_message())
 			if self.time  < prev_time:
 				print('Time inconsistant!')
@@ -92,7 +100,7 @@ class SimulationManager():
 			if valid == False :
 				break 
 			'''
-			if self.allow_operation(rsp) and valid:
+			if valid_op and self.allow_operation(rsp):
 				robot_state = robot_system.localization_update(rsp)
 				state_var.append(robot_state['state variance'])
 				state_recorder.record_state(rsp, robot_state)
@@ -100,7 +108,7 @@ class SimulationManager():
 			# communication protocall
 			comm_rsp = None
 			if comm:
-				if rsp.get_type()=='measurement':
+				if valid_op and rsp.get_type()=='measurement':
 
 					rbt_idx = rsp.get_robot_index()
 					receiver_idx, sender_idx = self.comm_controller(rbt_idx, self.robot_system.num_robots, rsp)
@@ -120,6 +128,8 @@ class SimulationManager():
 							comm_rsp.set_message(message)
 
 			if comm_rsp != None:		
+				print('comm', robot_labels[sender_idx], robot_labels[receiver_idx])
+
 				robot_state = robot_system.localization_update(comm_rsp)
 				state_var.append(robot_state['state variance'])
 				state_recorder.record_state(rsp, robot_state)
@@ -189,6 +199,7 @@ class SimulationManager():
 				message['groundtruth'] = rsp.get_groundtruth
 				comm_rsp.set_message(message)
 
+
 				robot_state = robot_system.localization_update(rsp)
 				#state_recorder.record_state(rsp, robot_state)
 
-- 
GitLab