From 0baac7627e15e873e1c2d39672db4f2ca63f4e98 Mon Sep 17 00:00:00 2001
From: William Chen <billyskc@ucla.edu>
Date: Thu, 6 Sep 2018 21:37:27 -0700
Subject: [PATCH] small changes on CoLo-AT: foramting and naming
---
.../existing_dataset.cpython-35.pyc | Bin 14400 -> 14019 bytes
CoLo-AT/full_test9.gif | Bin 0 -> 1820 bytes
.../__pycache__/robot_system.cpython-35.pyc | Bin 4050 -> 4221 bytes
.../__pycache__/robot_system.cpython-36.pyc | Bin 3788 -> 3788 bytes
CoLo-AT/robots/robot_system.py | 3 ++-
CoLo-AT/ros_compatibility.py | 23 ++++++------------
CoLo-AT/ros_demo.py | 17 ++++++-------
.../__pycache__/sim_manager.cpython-35.pyc | Bin 4267 -> 4324 bytes
.../__pycache__/sim_manager.cpython-36.pyc | Bin 3922 -> 3924 bytes
.../__pycache__/state_recorder.cpython-35.pyc | Bin 5019 -> 5037 bytes
.../__pycache__/state_recorder.cpython-36.pyc | Bin 4575 -> 4628 bytes
CoLo-AT/simulation_process/sim_manager.py | 2 +-
CoLo-AT/simulation_process/state_recorder.py | 1 +
CoLo-PE/get_sensor_variances.py | 2 +-
14 files changed, 19 insertions(+), 29 deletions(-)
create mode 100644 CoLo-AT/full_test9.gif
diff --git a/CoLo-AT/dataset_manager/__pycache__/existing_dataset.cpython-35.pyc b/CoLo-AT/dataset_manager/__pycache__/existing_dataset.cpython-35.pyc
index b8f4113524e113b5bc74d0dcc2360c8aac644e3c..355f71f66a3183e2b8a402317cc436c3ad78597f 100644
GIT binary patch
delta 1894
zcmZ`(O>7%Q6n?Y*i~pU(KiQ;-6E}ZL;wB{}X~AhgMWp<w6{HHt1T5om>`nZ~?j~)L
zH7+DY^n}zo6+sUO4uDh%`GmND;DUNWoXCwM;=%z54i)0PwbO)9Y|XcC-p|aNdGDKV
zYTuPTQNQ2g9)JJUo6BE%zGi*rr;62be(I`PDyiA>)P?E`RSS|&U1@eoE0XjFh^l>m
zfRDf)gLF6WQQ%|1$E|4?xB^ScBTe1$UN_rs-tU=rvSRb`_%LJZ&CK97^OgZXix3oV
z^@YV>L!Ys7^W(&{jg`btBRQvK*F<V`mKB9QdM7xFzFjyj;LxLv=HOUbVjE&d;VBQu
zbs0>6wGJ>HAe&AcZjd<{dckmk=>b!Qf&``)%w9`y!LAMJed3;Sxp4wGnq0u$VD<op
znSH=93iV##K2RNC+JXBaRr!b=+egBE0=Q2<m^Sj2>jRc#Ln#hsKky)|+d=!@MuE6N
zKgcT*)CZwH1eJA~H^}0Fbr)!V+Aw(@f|!AcTH0L((R!jiF%zwwEX$gYg3;bK`vJ{(
zid|g&B$y|G<6YoBB;k$Vc18f!g0L0>-br!H=mZn(4L|hI7e8Th2+ROZTc2<Y-Hfh-
zKrFKlC}@Dfxs9$6g;;$Uj4&L8wpPL{%pmj_5r{ryf%wq{mT`p<Frr{0MxclGv?eI5
zV~6~1ZyV67U8sO{vt3{g1Mj9d)qYSStkjYczN8uA$BBuH+o5cwuu;ltgPZwMHK!U|
z+LPEp8Y9>sND-VS7)JQjNzy#Qc?2b<ZIXOdj8D$Tt0cM!B!skG&*!;jr@2CN@z!K)
za#R~4#TG#kVLwpLXZ4I)scjgU&8((vlSLyai*F})0%?0TvpH?yX)P<xCF7B_RJw*^
zEA6hnv4){Me@&~1ax%rX#plTvT{4r|fHWcQCuMtxJ;y}s)ECbiwB%U=>sYfWDfV{u
zq<xv-71FwnrFb+Ya=I@3Q%QDHJTY}{sPMsU`u(twv!RK8AsikY77<TKB7Le?<ffmM
zmYKLaeWMX!*cWC8v4|sQkUxmXOCR+P%ETok3UMhxEkv#X;6OYfS8Psn!fLjhuv6=P
zQq{5K*x?v}HsodiRvoZp;2g<d6<<cREbct&YM|@Cl;|wigO(Dz(vGhKdXloe33)sJ
zV{kx<o(_!buzJ#zEF%BbegU+WfQ)984L&^dpwNNl+iWG;+a%saC#WE;t)o2T5K8x@
z!cO?^;xF5K3)r^VnTf{#we4ZYK4{S^&nVvGW$wiKXcq|<38?BC)l7Su;2na?2ukR%
z6VgFLE$1^_U0uzT7S*z99BGRNI;Z_rUOm#s)(d@|mRccrjo@{Hw+VI#isIcf3(i}}
zHT|r(cP1X4kP^&^|89FPi%8hUwm6Hhrth%;Ygjyclfw20uqN%E)lo`&^}LZO=XE_>
z$Sdx{w6z*pyDG@Uj9$y<)KxW?p_5qIr~RARlFBn$UawUvyiGe=nW(7+A3zAi?O8AT
zN_;vy!y@A6*(u?llV=L#pc0e_EdE<5QckTDt;^h{sg?E(@%LOz)aE>FSnSLt954CY
L!7=g4+=u@F6ReLN
delta 2249
zcmZuyOK%%h6h3$SN*p_mUx~*}o5pQY+{AH7S_|5=mHH48R6!^bRHk4VU&o!qo=LAK
zX`9YqN)pipbitJ>@lbaN2_z)s6+*nk52#onMJzz9SRo;{sGM_c(>7$1`R3en&*MJM
zcl=}H`_<rZI2=6p)Ap<B%U=e+W|yq|RjX0WUpMM?quj_Z8jZDj*)}XQchOuogk?6W
zrkz`|MDDz0tyTrjtygoGtV+3VY$(J0(wbQzk7oN<c16>G$ur)aqgw#QIPeMJlfaL_
z7K35}_*1~Mz>lh768ID>W~bW)-<>!sw!cfA_On|1p96;(TW)6#Zm@wm0Ei?(O@2E5
z4Xd}SnPVQdBJWL9{A%)=d^k~HHCdRvA321EL-?M@xA~d3{rhB5W4b((<?^rWsg@30
z11A8yADk5M0601r9&o(CgD`Kt#~gs=K5%@XJAQmM(D?x9ZW<g9@DNJ-l;lBa6r42h
zFsyh%T=kG622F#0MFV#~xCg*op)pMaVI=_KBToy1@<AA}2fFG_2E*=Hci@b5GAW1U
z5IN&~cRCC>jVs^8=m)_$2prdlc}T*QVmf;e92|!s)%_%#1K<qeu=7cbY_=HK5wM3~
zISR`$+7LPIqcq$DoA}RQ#2p7a4n5yaM9k_$AY~_Dc#i<1y8>juPJ)A&fD}gT3{Y4(
ziMP1r?asKF-eurv;7LZn83#T@j)u2)C3rK7+3133zCS%REo>@-c>+f8G{Iql(*$P_
zvcqDH<aODcKCPQ1dI_=w{RA4qj(jjZGJQylli~(JjkJS}YPnf5%+)o!v|bkCCe2(U
zsLNRHmb{-EP3~y*8~ENV_FHc(qb;xA5DobtH_wC&&Ac=^=G8qx7GxndtVP%e3u`eJ
z!*Ni1UX!<HK7XM}@tz@2yD6ZQ^>!Cc+E)lJlQytxeUGn)j9T(~KF7A@C;4Yv71|{|
zK72`51kh2S`a@)Z#f(Lam+Y$3UCgqDEEAh(lJ)J*^d@RM=^`X4AX@FDE23JqtEDpM
z1`)noFY&Tnez|U#)w_5pO|=e|WE+_asBc#7nY}&8q4wbi^n7);;=$PT5YI#Ss@3;-
z<imVLJEuv0Y`c|bSfWlJ5tSodk)&9?ey{^XSfm~*?H~}rkg!zH1HFPKf-CmUn|~8w
zUE~rT3RYdX!`4A?krn8SWC$W(I-dkqyfc79^=YM$KUmBE&*-DnPiAz7z()Rd{X5}^
zLLsD37%8-8klo96ql%q~NNGh~3=}1*t>Bd~4P}}^v+lv*bXd`7NYN;&Xw=y_kAjPQ
z_kxSej?r--v9Oysaai)fqK#ub5mgsDs&zbPYBXrvQ@1+$IJTOE(&Bb-+sYV~-2*0b
z-1V*rAIwvqLBBrL5U>)dY5uH{`A|=Ysv^adK2@aZvdJK(jA#GUc#Jk-;ti|a?6Hq*
ze4%zL@x^n$ZET$tO*u6?6%sKlCq=bC{L-3D_QiV*;@#O)>&dntT(VXDVuB>9N<oAY
zK>{MBxInN?@FGGs`G`5ih;1~gC2lM&mFh;rupi?q@u?WLxb;|f!QZZvNMZ_gh2S-U
z*9qPtm?yYR@HWAJ9Zqb}uqH?6&iW-W?t(1Ojb$!seimm7>>?Y*UXx*d{EK_jEUD>C
z*T(SMzBf0>(yBYIQiyj5ZV{-KBjmToQ-0M9E&0pw)G6wI#qc`*$n%n@Hdif^cbg&k
zP{Zu>g-6XH&^7x(=1<JAVOcqmXJvW!#QwRfWQ}DDLbYU-U(itHryeA>2-Lx<h24?2
d3nK$!47JDcZBD*l$OINb{UKjuMjk1A{4dN@%%}hW
diff --git a/CoLo-AT/full_test9.gif b/CoLo-AT/full_test9.gif
new file mode 100644
index 0000000000000000000000000000000000000000..31759e06f486ca6ddc1867ff549f8b0d90842bcd
GIT binary patch
literal 1820
zcmV+%2jlohNk%w1VXy>*1MmO<00000001HR1ONg6001li000261cU<s0{(=LsmtvT
zqnxzbi?iOm`wxcVNS5Y_rs~SJ?hD8AOxN~}=lag~{tpZahs2`sh)gP%%%<}RjY_A~
zs`ZM^YPa03_X`e-$K<m4j83c9?6&(2kIU!uy8VvN>-YS={|^`_I7nD%c!-#&xX9S(
z_y`#(IZ0V*d5M{+xyjk-`3V{-I!an<dWxE=y2{$>`U)E>J4;(@dyAW^yUW|_`wJW_
zJWO0{e2ko|yv*F}{0to}JxyJ0eT|*1z0KY2{S6*2K2Bb4evY25zRuq6{th26KTlt8
ze~+K9zt7+A{|_*rz<~q{8a#+Fp~8g>8#;UlF`~qY6f0W1h%uwa{*4?vdi)47q{xvZ
zOPV~1GNsCuEL*yK2{We5nKWzKyoocX&Ye7a`uqtrsL-KAiyA$OG^x_1Oq)7=3N@<K
zsZ^_4y^1xf)~#H-di@GEtk|(+%bGolHm%yVY}>kh3pcLZxpeE=y^A-m-o1SL`uz(y
zu;9Uj3mZO+II-fzj2k<C3^}so$&@QwzKl7u=FOZtd;SbMwCK^KOPfB8I<@N6tXsQ&
z4Li2%*|clhzKuJ#?%lk5`~D3)xbWe`iyJ?VJh}4a%$qxZ4n4Z`>C~%Rzm7e-_U+ue
zd;bnTy!i3t%bP!sKE3+&?AyD44?n*A`Sk1CzmGq^{{8&^`}_Y7V1NP+NML~m9*AIq
z3NFZCgAP6jVT2M+NMVH*UWj3a8g9s8haP?iVu&J+NMea5o`_<KDz3<4i!QzhV~jG+
zNMnsQ-iTw4I_}70k3RkgWROA*No0{m9*Ja<N-oJ{lTJPfWt37*NoAE*UWsLvT5ic@
zmtKAeW|(4*NoJX5o{46fYOcv<n{K`dXPk1*NoSpQ-ic?PdhW?*pML%cXrO`)N@$^m
z9*St9iZ04%qmDibX{3@)N@=B*UW#d^nr_Nzr=ETaYN(=)N@}U5o{DO!s;<gvtFFEZ
zYpk-)N^7mQ-im9ky6(zrufF~YY_P%(OKh>m9*h2LvdS*YY_rZj3vIO0PD^dI)?SNk
zw%TsXZMWWj3vRgLj!SO2=AMggy6UdWZoBTj3vayg&P#8-_TGzczWVOVZ@>Qj3vj>!
z4@_{u1|N)Y!U`|UaKjEi3~|H~PfT&e7GI2U#u{(TamOBi406aKk4$pOCZCLQ$||qS
za?38i40Fsf&rEa8Hs6eM&N}bRbI(5i40O;!4^4E@MjwrI(n>GQbkj~h4RzE~Pfc~z
zR$q;E)>?1Pb=O{h4R+XKk4<*jW}l6A+G?-OcH3^h4R_pf&rNsTcHfP6-g@uNci(>h
z4S3*!4^DXDh98c2;)*ZMc;k*g4teB~PySAM<(6NLdFGmL&Uxpae-3)+qK{5`>878K
zdg`jL&U)*vzYcrsvd>O??Y7^Jd+xgL&U^2^{|<cc!Vgb;@x~vIeDcaK&wTUFKM#HM
z(oau)_10gHefHXK&wcmae-D26;*U>$`R1RGe){UK&wl&vzYl->^3P9y{r2CFfByRK
z&wu~^{|~?b3UGh~ET91oh`<CYaDfbLpaUNW!3aulf)uQv1uuxf3~F$L9PFS6KM2AQ
zig1J^ETIWch{6=AaD^;vp$lIK!x+kNhBU094R46U9O`g~JnW$le+a}N3UP=;ETR#Q
zh{Pl+afwW9q7$D8#VAT~id3wk{uQr?#Vl%Zi(KrY7rzL`Fp6=EWGtf@&xpn}s&S2M
zY@-|B2*)_eagKDXqaE*v$2{tBk9_Q-AO8r*Knik@ge;^X4~fV`DsqvGY@{O}3CT!G
za*~v+q$Mwj$xLc;lbr0NCqD_wP>OPtq%5T=Pl?J@s&bXAY^5t-3Cmc@a+b8Lr7drX
z%UtSmm%QwyFMkQlU<z}X#4M&UkBQ7=Ds!34Y^F1x3C(CqbDGqwrZumL&1`COo80WC
zH@^waaEf!B<SeH-&xy`-s&k#}Y^OWl3D0=SbDs39r#<h9&wT20pZx5nKmQ5PfC_Y=
z1TCmR4~o!)Ds-U?ZKy-}9}3ZkN_3(Wt*AvWiqVW}bfX;Ys7F5v(vXUDq$Dk=Nl%K>
zl&W;4EN!VvUkcNh%5<hQt*K3Kiqo9xbf-M+sZW0j)SwD=s6;KQQICq$q$+i(Ol_)D
zp9<BeN_DDKt*TY8iq))Yb*o(Ms#m`X*073otYj^#S<i~rw5oNjY;CJs-wM~b%5|=E
zt*c${ir2jAb+3Hwt6%>L*uV;Qu!JqFVGoPg#42{NjBTuA9}C&YN_Mi8t*m7)i`mR-
zcC(!AtY<$9+R%!2w4^PqX-|vV)T(y1tZl7pUklsV%67K2t*vcui`(4lcDKCkt#5w|
K+~9_!0028R*st;c
literal 0
HcmV?d00001
diff --git a/CoLo-AT/robots/__pycache__/robot_system.cpython-35.pyc b/CoLo-AT/robots/__pycache__/robot_system.cpython-35.pyc
index 4245e57f813a10c01fb0bbf3c3b16c617c77020c..25d23386e6336389cfb87686ae89bd28cf844cda 100644
GIT binary patch
delta 1344
zcmZ`&&u<$=6#iz`U1!(pABmmVOWedMy@a-kxB;cAP*u3}1_vZ6MV4pfuI=n5({0+i
zv$E81K-|zuNO0&sfP_?)OOX(VoVbFxm=hQN20%jM&FnfDO8sN~X6AkG``$OtKW_ik
zb2h3~_qX3a`)K~u`NrCOeeX_u5bWI#CzG&$uy-TA6`RMEcRqOLl*(3Z3xG@C8}z;V
zN48bIHTx67Mp}j{L6qUAD<)c@$U$OZMB5I8iy_xfQPBZ?P8d=b%0*f+0FMA3l&6!%
zfcy!`If$w`WAINnnBJ&C0V5mAD)we&h`O%!m}#`i=^OBe7Sc2L8R+fk2d1dbD#jeL
z!wZFLOA4JOg+y0dC~T6#y3y03s9~aY%=~oKgmf%`u?6`p(_ZNH2F4}57H#7YDPO_J
zfii;AH7MfEF;5xAii+mNsAEbRp3^Oe5>gvS>s)k9aLVaOH$+C+m`RHZe^K7Wi0sVE
zHw&a{zNGNcDHpG3WL}t*5L@&Wv_G51pXZ!{;%<xf4eviBt(lSHK}K9_BFD*{Q4J@+
zZMuPVq#H=vh&m>(iWiRvpw^+<$Zza_W@h<DDu`S&8eM(qOwDY_pSdDlGg&F|(9VaJ
z7DKAGO+cqKFC(&z=ClJErrTsHxkl~h4d&8yQbS8myEyu;fNGevKwAGr&hoAaMoD_8
zyNH5?C)qOlnI{Y0!mnl0=(QJY`gis+k2nwIzX`FWsaw9-rsZNKdT9G%2dWEk0csQC
zRoy~J6Gf==cz^d|_Pu*`Hf{8y;k`*;_Kt%|JP4DA!$+;J-}`m?@pZ}QPd@ze3;lY2
zeO?y9B<b%b)7s#kltGmAlJFpSt2b>N%HVkaP{xP*#ZKp@l(FoE(IBSGLDDOrJM&5`
zLxT2`FplPxgCM(989UQv7#;T~p~yb1oUqlb=Iyo`#4Wq@m^(Kg41z-q$gX;qvx&FM
zwzAK?cT01tzyItP?+te6G39}Ktjb(^d)%hj*0Hste`&SKTD0F}E$(r%XEJ+qy2d=_
oGlx68ZdNwya+@NjsIkTzw#_@N&MsP6T>FGw%bwKRS-pPxH_^rm)&Kwi
delta 1152
zcmZuw&u<bz7=1Gf3%l&{TVR1&P4rMB!IPTQ#6zQ}-g>hniXB>^bg^678l4S?CdT`E
z)RPwzy%;>0c=J#2WP0*${0oeU-wa?O3cKXZe&2iVn>Wkv3tyM5lIz;F^7GBNAFPkY
z`ord<!Ct3%-0k<f?Y-tkcki&@9(D(Q<BosS4F>*R#~(Jf2SH<PFnHVvsCLw8tPi%@
z{q9rU*xWw!w-n=G{LM%h=_LR*-D`9oyo!FB>!UG*i7){fgGj&&lNcK4(IN?9VTY?<
zVJkq_2}5W>S_o}TND(0gNmo*@9e6tiLPswcJPGvTkOpq~V<zJeE_OMB&ur?E#^}r>
zIcMTRx-*junWVL1mQY5c9HFd6R26yb#t>{VQP4XfQxiSfQd$q3gUmqyy9R=LjIbn)
z37KK!QP)mf%`9RFP3(VSqNu?(7w5DdnUiD^yC#D7Y=V0f!$dwmCCdDQ@R{yD328-|
zfo%MDP)U?@60H#@G>w0{QSEi6U(oJ0f*e=Ylyw1K&%p`MqO3`orYym)=)M&GEfGN$
zAxoH?&BFPPCI>v9y$#>S(1j?g_?V<!KjD(jRKEpb8S2R|Jq%6tWj2*TQ&CbBJMlPL
zlx78?hp;+36dGMMWz^_l4f_uaiILio;u$ll@qa4Os7JkkHTa!bCf7+qXQG7vW-{}g
ztIbUPD~mC0QiO}`^G1R~tw4Cny6B1O+)C6?_Cy`B2C)QLg*dNv(4|+29@eq3vK;-e
zSEHM$Opg9-^!a}KG$A_uVf+3tdYO{ZW#^b>qu0(#tWMSF)VUfDvI?2*3R{Y<xp&9U
z+#9TQq{!T1F0<+DaFejfY<`iM+)fl&kvj_Os?D;5^2}i#v$(}gmZ6o&>V&KbkJc7j
T<`t@3;!!p8f~`hxGN*q41J&0-
diff --git a/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc
index 7aadf383202797e3ccda9258a470de1557b1c484..51373f8b840f6867913e1a6dffbd7dfe00f91edd 100644
GIT binary patch
delta 38
scmX>jdq$Sin3tF9*^i0Qw>EONbF$Bx2?9GePv@*>W@O$h&byTn04}!<-T(jq
delta 38
qcmX>jdq$Sin3tDJRDVMBwT+zZoa_u>@L=<F&U$7>rp@BKTNwezHwn%F
diff --git a/CoLo-AT/robots/robot_system.py b/CoLo-AT/robots/robot_system.py
index 14d20e3..079d1ad 100644
--- a/CoLo-AT/robots/robot_system.py
+++ b/CoLo-AT/robots/robot_system.py
@@ -76,8 +76,9 @@ class RobotSystem:
update_type = 'propagation'
v = message_data['velocity']
a_v = message_data ['angular velocity']
+
#sigma_odo = np.matrix([[5.075*v, 0], [0, 0.1]]) #with respect to velocity and orientation for utias datasets
- sigma_odo = np.matrix([[0.01, 0], [0, 0.5]]) #with respect to velocity and orientation
+ sigma_odo = np.matrix([[0.01, 0], [0, 0.1]]) #with respect to velocity and orientation
sensor_covariance = sigma_odo
delta_t = message_data['delta_t']
diff --git a/CoLo-AT/ros_compatibility.py b/CoLo-AT/ros_compatibility.py
index d32cc35..fd4b125 100644
--- a/CoLo-AT/ros_compatibility.py
+++ b/CoLo-AT/ros_compatibility.py
@@ -16,33 +16,24 @@ from robots.robot_system import RobotSystem
from simulation_process.state_recorder import StatesRecorder
from data_analysis.data_analyzer import Analyzer
from data_analysis.realtime_plot import animate_plot
-from pprint import pprint
# load algorithms
sys.path.append(os.path.join(os.path.dirname(__file__), "localization_algos"))
from centralized_ekf import Centralized_EKF
-from simple_ekf import Simple_EKF
-from ekf_ls_bda import EKF_LS_BDA
-from ekf_gs_bound import EKF_GS_BOUND
-from ekf_gs_ci import EKF_GS_CI
-from ekf_gs_sci2 import EKF_GS_SCI2
-
-#dataset_path = '/home/william/catkin_ws/ros_colo_dataset/'
-#dataset_path = '/home/william/UTIAS-dataset/MRCLAM_Dataset3/'
-#dataset_path = '/home/william/full_tests/full_test9/'
+
compname = getpass.getuser()
dataset_path = "/home/"+ compname +"/full_tests/full_test_v3_6/"
dataset_labels = [1,2,3]
-duration = 300 # duration for the simulation in sec
+duration = 310 # duration for the simulation in sec
testing_dataset = Dataset('testing')
start_time, starting_states, dataset_data, time_arr = testing_dataset.load_MRCLAMDatasets(dataset_path, dataset_labels, duration, delay_start = 0)
-loc_algo = EKF_GS_SCI2('algo')
-robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = True)
+loc_algo = Centralized_EKF('algo')
+robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = False)
-sim = SimulationManager('PE-CoLo EKF_GS_SCI2')
+sim = SimulationManager('CoLo: Centralized_EKF')
state_recorder = StatesRecorder('recorder',dataset_labels)
-sim.sim_process_naive(dataset_labels, testing_dataset, robot, state_recorder, simple_plot = True)
+sim.sim_process_native(dataset_labels, testing_dataset, robot, state_recorder, simple_plot = True)
analyzer = Analyzer('analyzer', dataset_labels)
@@ -52,4 +43,4 @@ data_in_time_order = state_recorder.get_data_in_time_order()
update_in_time_order = state_recorder.get_updata_type_in_time_order()
update_type_arr = state_recorder.get_update_type_arr()
recorded_data = state_recorder.get_recorded_data()
-#recorded_dataline = [time, robot_label, est_x_pos, est_y_pos, trace_state_var, gt_x_pos, gt_y_pos, loc_err]
+#Format: recorded_dataline = [time, robot_label, est_x_pos, est_y_pos, trace_state_var, gt_x_pos, gt_y_pos, loc_err]
diff --git a/CoLo-AT/ros_demo.py b/CoLo-AT/ros_demo.py
index 7f43535..1915587 100644
--- a/CoLo-AT/ros_demo.py
+++ b/CoLo-AT/ros_demo.py
@@ -20,26 +20,23 @@ from pprint import pprint
# load algorithms
sys.path.append(os.path.join(os.path.dirname(__file__), "localization_algos"))
from centralized_ekf import Centralized_EKF # works
-from simple_ekf import Simple_EKF
-from ekf_ls_bda import EKF_LS_BDA
-from ekf_gs_bound import EKF_GS_BOUND
-from ekf_gs_ci2 import EKF_GS_CI2
+
#dataset_path = '/home/william/catkin_ws/ros_colo_dataset/'
#dataset_path = '/home/william/UTIAS-dataset/MRCLAM_Dataset3/'
compname = getpass.getuser()
-dataset_path = "/home/"+ compname +"/full_tests/full_test_v3_2/"
+dataset_path = "/home/"+ compname +"/full_tests/full_test_v3_6/"
-dataset_labels = [1, 2]
+dataset_labels = [1, 2,3]
duration = 240 # duration for the simulation in sec
testing_dataset = Dataset('testing')
start_time, starting_states, dataset_data, time_arr = testing_dataset.load_MRCLAMDatasets(dataset_path, dataset_labels, duration, delay_start = 0)
-loc_algo = Simple_EKF('algo')
-robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = True)
+loc_algo = Centralized_EKF('algo')
+robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = False)
-sim = SimulationManager('Simple_EKF')
+sim = SimulationManager('Centralized_EKF')
state_recorder = StatesRecorder('recorder',dataset_labels)
-sim.sim_process_naive(dataset_labels, testing_dataset, robot, state_recorder, simple_plot = False)
+sim.sim_process_native(dataset_labels, testing_dataset, robot, state_recorder, simple_plot = False)
analyzer = Analyzer('analyzer', dataset_labels)
diff --git a/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-35.pyc b/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-35.pyc
index b79a7a4b5bb36b2e46ded786be1a2c770c2d019d..d388f62ec168a34b8884ffdd9db950c6475c06b3 100644
GIT binary patch
delta 628
zcmZ`%&1(}u6n}5?wb^DrvPqL{5@XZG76~Czdy&>d5j;sz^d@acGb?7BG$mOS$^?a6
zq&IngfP#1u57MItZ}}sNph!>R#bd#PZ?+dN&c|=&_vXF#n>Wmd`gz?bn5MCD@%nKq
z|H*iVm96&E(a>){?+*t3?y!AlbZ-<vYx~jZ1)e7ychVnrj>e;&Kbdq6yCMBb^{_$r
z^`%PKf9Cg6aM{YdBzeYoPJYvg{w1H*BtQ^{R{&Cn;sL;TPP^${{6nA9`!DhkxG)2l
zE`sEJPVI#T2O5meQ0O2NAQIq73%ts-0FuWd2opSE!Ip~1E5i%{WekES36p6NWE=u^
z=HnvFG{D1s2rUTszyZ$2rfCV}PU#<g3s|tI@|2)3^+587#3G?fVzVm8{704E7*`wb
zwUBFIm8fo|&0H=-mki4gR6u0uhtX_)*VHAeAl%6J3fGRxSMV#Sf};3`*Yi8aEqais
z(mb9$^%v%tK1&II5OzBuHO)P&(TOP<EBxf+x`I^&H3drwmK8J<TqD!+uuN;#M`uGB
zHWjQZxJlovTlj=Ld-FyHUG5f|nvJ=biy6$Kfu`cgEZXQOrY&np%xOmac$L3FjlO1d
ZdTX!aT^cz>`f0D=ZL*vyZd22F`4=77fm#3n
delta 540
zcmXX?&rj4q6n?W^*rm{RVWI1y;3EDaX4yo&ARLT@gaZc&H&C(LmN2#LtZf&A&PEer
z;z3U@`~@anh%tmC1g~7Z?#+b1fg|A!Fq!#YzW2WOYfdNjCUn~{^cVlWu4UzY{U=+B
z8k-{W8y|QWawlr6@TeU+85ePVA>QJNh$BDF>Kh`d&kND=6N+v5^(EmtA^&(OY;3e+
zS7oFJC+xJx8ccb#-b#e)r|Ej^;8DS4lUOU(Mk&)Rz}2ae>7}R9OMl^7@$c|4lK?>;
zz5&Q0RNet3pK!YGHQUGUea+7r1VbQ)VTVCy1Y{n9vZ8VXc5=YtCS)cA#7iJe<?7fV
zZ3rj~(5Wb=M^z=2vBB=#07<t3o&j@IB_IJ2lL+NpMyL-{>g%Q&b;U#dc40M_0eBYv
z*X=?Tf>H2Fc&<NsxSLZm-2`tC)E>8}FJnqNq(;8pZDp!_u-t#f@TKvFjo}}|n;EAc
zNNNi1D40}mSHV36_X)aMx}A3O@7p{}9w=uI!BiEO&ENfx6@Q}ODVo*{dxeYEEdH>@
qn1*}STeg4$rDx-FBv>7Y+fi$KI&!j*H@kVVo86p9ljrD_zFYvAcZF8~
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 1f52de8ccdebd6de988d3d8bcff8fa717cd86d3d..b567d4d86a5ddcbd5ab83757cc5b2d448d6a3b98 100644
GIT binary patch
delta 54
zcmca4cSVlVn3tE!<mbd_-Hn{b*aU=%Gjrn$it>|Fi;Ls)5=$oEV@qaKnQY4~#|-3c
JPGbMS4gdj863YMp
delta 52
zcmca2cS(-Zn3tC;QMEr>XCvn^Hh#h4%-r~bqWt94;^O$c#L4&Bk{OjJ+jGk?<t1)T
IVgJAm0KLi)IsgCw
diff --git a/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-35.pyc b/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-35.pyc
index 6866b0b74fece4e734a2117774892407b77ea342..3b7e1eaa2350cb227f65d1287e931c9781c61a44 100644
GIT binary patch
delta 418
zcmbQOzE+(}jF*?|+pmeyLP8t4{FtTf^)vEwQ}xR;b8<2hbM>9`ee%JCu4BmNCCqWG
zeCZ5e@W8%`&(2U`GdHINBO~)<7p@M*(8*`HCNf4$j_2-SjGp|4`w(N?<n=t&j7gKV
zd7BthC$HmeW6YRr%IC(oW^xT*xZpm95;le!28JwlhGs?vm)NzFpYS!*mvAwp0EIai
zQaBh&xEX4|Y)PPk5*{RubTbPBV_`Bwcnvc{7B52zC&&m!5QmQ;g$vH%XGr0Oa|9St
zco<TEih)YR8JZcG7;9L-ngt<BfE+#suM{CT52#xh&Oz2K0nyFGkR`~FBEpa&3gj~~
zmH@p1)Xc!Zm?FjytSLE}Nx+4VzoaNJIaMJzGd(vkzId~<z$!+CY+wi$>w$uik%v)`
bksk<ofKUcV%K@SGWNx7|jH;7w2pt9he>_?y
delta 434
zcmYk2O-lk%6o$_k`_OS_obf9cWg8&^hoN1oHd!l)$Vey*2Cvo(^I_v?U_zyvTFPCv
zDu`As!i~!)Xw`pc-=C;E3L*#Y<vHhl;GX-s_OQmMMUnp+-Jd)wZ~R`U*wD^hyQW<l
zmSyO+Rx<3CrTd2Ks5?&E@Lb2PIlfwVJ$2i48#Ry3+BLQ4R&~qhjDuRe<x~TsIc$bb
z=ObPU06OB<#_%V+iOA6me}{+kkspX<HfeI~0PN)HKuFMosEx<;J^Fza+83@+r+-3E
zBr~}j%O&ZPSjRIOk#_Nx?n`Bbg1-P=0CN%g2!%?Ij-;FGiz<}6AuvTC;h>KJNI)V}
zhymfJAOnd45rD9K8v4LtcWx}plL!MQD^p-{d<wJj(nMb6z*K-FfFxN1(P!^t0^63P
pptPQ$C>MrD@(X6oV8Q(OBLq}1#!6td%<e2^=}P<?bM!a<`45t7W%~dC
diff --git a/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-36.pyc b/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-36.pyc
index 2afa61a8e0fddd99c65d8befd44562410b9cfcd7..79b36539455ad2d99eb5288bc0f185b944a12294 100644
GIT binary patch
delta 234
zcmcbwJVk}en3tF9+pmeyLP8t4xOo{JCX4af@lGjWt6|7uZ)S91h;5sk$eUDO!j-~S
z!;!*K!d(NTB^jC-OL$-`X@+JNMutMSaE2P@EZ!8(W=2L3i!X%>%HmJqhOz`wcv8et
z_<NavDp-JO1;FAgK(&HU7JrHmlm${N4pl43lqHxVoFdW-G`fT#OQ@NFks(Dim_buw
qvL&AzAAd<vVsffNaAtaLVtn!D8otepjCzv|1SA=iHhTz6X9NH*%{BV~
delta 211
zcmbQDa$lLtn3tF9oN9mcD#49h+`No(lf`)Lc#BHdY8bNEn;BggVpAq3@+Q?6x`i{;
zFlX_ma5OVAf>?YhoKO~j3Kx_mkiwlJn!?x11XRHSRLc(*X921efU@{g1feXDS}~|v
zrYyk}p%md>pvff+SwhVWj0`Cv!3>(>epR9h&iO?}sgqOqY&UP>+rTKP!vF+~9E^O7
UTtLVHgd#v%f>B~~t-y3f001{EPyhe`
diff --git a/CoLo-AT/simulation_process/sim_manager.py b/CoLo-AT/simulation_process/sim_manager.py
index d6bd79d..d2f6e8d 100644
--- a/CoLo-AT/simulation_process/sim_manager.py
+++ b/CoLo-AT/simulation_process/sim_manager.py
@@ -14,7 +14,7 @@ class SimulationManager():
def __init__(self, name):
self.name = name
- def sim_process_naive(self, dataset_labels, dm, robot_system, state_recorder, comm = True, simple_plot = False):
+ def sim_process_native(self, dataset_labels, dm, robot_system, state_recorder, comm = True, simple_plot = False):
#dm: dataset manager
print('******** Simulation Process Started! ********')
print('communication: ', comm)
diff --git a/CoLo-AT/simulation_process/state_recorder.py b/CoLo-AT/simulation_process/state_recorder.py
index b9ae1f5..245117c 100644
--- a/CoLo-AT/simulation_process/state_recorder.py
+++ b/CoLo-AT/simulation_process/state_recorder.py
@@ -116,6 +116,7 @@ class StatesRecorder():
fig1.plot(time_arr, loc_err_arr[label], label= 'Robot %d' %label)
fig2.plot(time_arr, trace_sigma_s_arr[label], label= 'Robot %d' %label)
print('Robot',label, 'loc err: ', sum(loc_err_arr[label])/len(loc_err_arr[label]))
+ print('Robot',label, 'trace Sigma_s: ', sum(trace_sigma_s_arr[label])/len(trace_sigma_s_arr[label]))
fig1.set_title('Estimation deviation error')
fig1.set_xlabel('Time[s]')
fig1.set_ylabel('RMS[m]')
diff --git a/CoLo-PE/get_sensor_variances.py b/CoLo-PE/get_sensor_variances.py
index 620e9bc..8c701dc 100644
--- a/CoLo-PE/get_sensor_variances.py
+++ b/CoLo-PE/get_sensor_variances.py
@@ -254,7 +254,7 @@ class DatasetAnalyzer:
compname = getpass.getuser()
-dataset_path = "/home/"+ compname +"/full_tests/full_test_v3_2/"
+dataset_path = "/home/"+ compname +"/full_tests/full_test_v3_6/"
dataset_labels = [1,2,3]
duration = 200 # duration for the simulation in sec
dataset = DatasetAnalyzer('DatasetAnalyzer')
--
GitLab