From f53cabd6773f5a5e88b583f36496d1538220e406 Mon Sep 17 00:00:00 2001
From: William Chen <billyskc@ucla.edu>
Date: Thu, 16 May 2019 16:15:35 -0700
Subject: [PATCH] updated analysis and recorder modules

---
 CoLo-AT/Cooperative_Localization_Journal1.py  | 105 ++++++++++++++++++
 .../__pycache__/data_analyzer.cpython-36.pyc  | Bin 5882 -> 6943 bytes
 CoLo-AT/data_analysis/data_analyzer.py        |  66 ++++++++---
 CoLo-AT/gs_sci_vs_ls_ci.py                    |   8 +-
 CoLo-AT/isrr2017_v2_2019.py                   |   2 +-
 .../__pycache__/ekf_gs_sci2.cpython-36.pyc    | Bin 5033 -> 5197 bytes
 .../__pycache__/ekf_ls_bda.cpython-36.pyc     | Bin 4770 -> 4814 bytes
 CoLo-AT/localization_algos/ekf_gs_bound.py    |   2 +-
 CoLo-AT/localization_algos/ekf_gs_sci2.py     |   8 +-
 CoLo-AT/localization_algos/ekf_ls_bda.py      |   3 +-
 .../__pycache__/sim_manager.cpython-36.pyc    | Bin 4816 -> 4816 bytes
 .../__pycache__/state_recorder.cpython-36.pyc | Bin 4829 -> 4829 bytes
 CoLo-AT/simulation_process/sim_manager.py     |   4 +-
 CoLo-AT/simulation_process/state_recorder.py  |   5 -
 14 files changed, 168 insertions(+), 35 deletions(-)
 create mode 100644 CoLo-AT/Cooperative_Localization_Journal1.py

diff --git a/CoLo-AT/Cooperative_Localization_Journal1.py b/CoLo-AT/Cooperative_Localization_Journal1.py
new file mode 100644
index 0000000..d48eb2e
--- /dev/null
+++ b/CoLo-AT/Cooperative_Localization_Journal1.py
@@ -0,0 +1,105 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Thu May 16 15:41:51 2019
+
+@author: william
+"""
+
+import os, sys
+import getpass
+sys.path.append(os.path.join(os.path.dirname(__file__), "."))
+from dataset_manager.realworld_dataset_manager import RW_Dataset_Manager
+from simulation_process.sim_manager import  SimulationManager
+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
+
+# load algorithms 
+sys.path.append(os.path.join(os.path.dirname(__file__), "localization_algos"))
+from centralized_ekf import Centralized_EKF # works
+from ekf_ls_bda import EKF_LS_BDA
+from ekf_ls_ci import EKF_LS_CI
+from ekf_gs_ci2 import EKF_GS_CI2
+from gs_ci_bound import GS_CI_Bound
+from ekf_gs_sci2 import EKF_GS_SCI2
+
+
+# dataset_path = "/Users/shengkangchen/Documents/CoLo/CoLo-D/CoLo-Datasets/official_dataset3/"
+dataset_path = "/home/william/CoLo/CoLo-D/CoLo-Datasets/official_dataset3/" # for desktop Ubuntu
+
+robot_labels = [1,2,3]
+duration = 180 # duration for the simulation in sec
+testing_dataset = RW_Dataset_Manager('testing')
+start_time, starting_states, dataset_data, time_arr = testing_dataset.load_datasets(dataset_path, robot_labels, duration)
+analyzer = Analyzer('analyzer', robot_labels)
+
+loc_algo = EKF_GS_CI2('algo')
+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)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = True)
+
+#animate_plot(robot_labels, state_recorder, analyzer, testing_dataset.get_landmark_map())
+
+##############################################################################
+
+testing_dataset.dataset_reset()
+loc_algo = GS_CI_Bound('algo')
+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)
+
+
+##############################################################################
+
+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)
+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 = True)
+
+##############################################################################
+
+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)
+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 = True)
+
+
+##############################################################################
+
+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)
+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 = True)
+
+##############################################################################
+
+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)
+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 = True)
+
+analyzer.algos_comparison([state_recorder, state_recorder_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI, state_recorder_bound], only_trace = ['GS_CI_bound'])
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 98deeb48ab51fd14206fa66498458fe60f18dfe4..dc6edb64e90bd2206b2c9cd8468823513f13048e 100644
GIT binary patch
delta 3077
zcmai0TWlOx8J;t@ot>S%c(1;uYlo)Zs7?uqfRd($CZ$kCPH0RbH3?gL&&J*LUYs*y
zWN&BL2S=3vS&cX#6$<o)S3ra(1QG}d3Gq}AAT(TFcxfMa>I*_3`2MrwO)pZ}_4()f
zFX#8n+3)}7|JTavA9c^9)9xGp`0LI4Cw}C9kFjg=hirbe#BH9q%bvG*nY%m%Wb+D7
z^9)deXL;_f_PoQV`4rE?lH@bIz>7dGx2|fn+57q<Y?RS7&BnTb<$r-`T4>%hhW0Qq
zbONo-4zytsSDmv1#<VcmHn@JkhOR;eP)Z>a$WzDyN{4RS42_V57Pk)#>`N(KpXgrO
zf<M6%qc~t}uwim|^3cSVv<h+XVB-{S+vX{C6K*@fy%;$>9it@A@L4`}sH=Zx^SrXB
z5L<|m!;3MZ*f|wD(n1f3OUmI*5_@fjmtz|R&&P==w(@^s7vk7t+r`<>9OB$?f~f<I
zX<Wm(A6pv~FZ6|IHiBMr-uJ5;y^YSk-|}0p;rLtMCh;8--<4lApUFn)M!(zL?X?;k
zL95@Bzcar+TE4vLH^mo~p~_o+Aa*x*$y>eA-)RbEf6Q+-ZVkM(-EOsiyV|D>gOl${
z^?^IL&Zy7Zt2GwcFZw$hjb=?323R7BHOPtbLD-YOus-}X`>Bs)1o13Nboz~YQ;5h4
z1QJG0(CRkp8!+T1_wCo!1Ib<cyyejS%J18Ac4XdeZJm|BwjW)10+}@3We#&8)z`?e
zoStKOJtwn?>7=2IdPbMaiQ~uAO|Lgh+J?_GRtifIzO2h`V(r?NHk`W0_Fmv@n8zw$
z6|qWKWvt5HFGDci)C_H#_3r}f6>VGVnL2*cdrX|#_rgTz%s{)Y^=3^iFt=TCUnZT}
z==WiXTOZSgrO>7bZg1=2&9JO&<=B>h?cZTV*(zavoQoqG`|@ES){{yvghdrs3>|fK
z?)0vszHk>GzoNm%whiDHz-kMbilaGA(KQDZTgO_jNbb0#;fWDd+_(@jKx6u6fyRWN
z0#Xkix)qJPab9H^x_8#EYk?gy6j15_6&Tpu3!Kn9xXgkiE)3yga~kH{0a+o(*3h`8
z3oq2#Zs>)%a4JlPnJ~L$pj%`PEtIpe-ZoG>Sy@j$ZixbZKly5M%YFnvEKAGnE=1Oj
zX!Qc&!4w((ZZ|SI&7L?7wKgpe-6z<x{EaJ2BCkA`nq?>CwbaXY<SLH+G4<KnWqAA@
z2+eSqqth~($4qA8I{5<2i{9cXcpTy#qCDMZI=Gl&IilW}l}4F)hR4dR4DalZ552$X
zxz8cDc#gzX66^A%?EFP#JbUDMOO+V8-?CH;A!^X#^CWbLnkn9w=+D`6_LCH_Di?Ew
z+{L3<@id`N%hlYg>~;C4+*ckys)HmP2`p6y>*zOeyEj8~-0p{#Le%c<z2%U}Urik!
zEnV04)<gZGcI(p6AqviDp|Nd?FtpnSbOXA>O%NRgL0)ukbn`ox-s_)u@p`y(2^6;4
zCiQ;qst#^}l_~B`G#jrqXrtJf;08B|QP_rmhYp>MJxQKJ|D|`8jfDA0LK_JSlY}<<
zc<~(?<twRt_Pg@g?MXfykIPGCUU^?b2fOl1hQ6{Qe~@3bOLX8(`F8%<=C{5{pP#J8
zz4Y(srJ@3N%@fpx#0-f!67wXEk$8wi6(Vx{W>DYnw7O!E)Kt)FY}a=-#75T_OB8h+
zqL!4e6c&~ux8LjR*Hu>)kCJnR#3>RVBJr^NtzM9SC{#}W1*%UDjXJ1eYZZ+3SXHm;
z8E`lQj5WcsR6NgKmRE}}nW27Q$Tx~lu}{iEX{mIMGE>#4q7$DW)R)he7Uo2caEC;f
zgrep%ggzs`TsqDEBY#?&H#2N7BNvJ@qXv<lK2E$8Dc^(=>Rdy2%%D4HLp@+^9TSOx
zs}VOX4U+|ClcBlS3oP6p12s2q>O*U2a~&-S^GHP-Ca@zBnxPFs5=F;|Xlh~P7}=p+
z24kt&ydw||I1WVXMVK2$5ueYZ-!V{yIGQHvL8C$8f#!l>S=IDdJH<;A?17+VUWoUk
zX)GA2xuCK(D2x7PKk%#g9orZW`eMJj)9D8hyYQgm2O!#@@EiqJALiAjk9Vkg;n(!Y
zYV<ppET(X9afbmK6eq!3qQBeYm==N;2g$vajlGtCqkBs{bQ@vdW|N|m`$zUIsQCPI
zRsPpp7zst}G7LxdpHINiFt7`6EhEynJYYCp_N}0Usf4UIAkduyp(r2Od!3EVW+$@u
z$5(R?e2>ft>&F~-Fg=X0nOGEg>Wu0t-XC-QCS@k-h`LJO=m<e=ZY-$LMcD^OTBjrR
z(C~aJII030y`Cx|59TkklZyGCs)E7vr)W4EG{wnN@5xYOYw=^R<LIs+6It+=lBx1z
zv!`Jg*M@rTKB<Dlm*nO0;)xs}UNEL(xM$oe?s;d?%?cVzYRSmz?RIzeBfYmHzfnHH
KrsdDdPyP?P{-S{Z

delta 2080
zcmZ`)No-q17@m3i=6%aXaZ;N0wdqbN1(HHjs8p4-&`6Q0R*~^I!7s6$jvbVgv2sY&
z11M5qI8<D?a6&AC-VmI)A(jK;Vx+1p5{iVlaN`31KYoNB;92v%?_cJh@%W$5?>zSB
zLGO4X;XT>-;p*(@$KJb?UXUNrkrO#)G5aRHVzNB*m=DZi1(sk*U^Yv!^i6WbVMUf<
zS@5{byiCH<T~FIfHv&S41!EdU`w%5W7+1Bf)wMe$qMM}az*}Q<lTsp_NN3t6?YfHT
zz&ynaV7_o8L+FAElbKsOWP8fic5FW~A#TG~`*CG&2n$>`bG8ghI#$Py9AR%#^|m(Y
z08tCOk;~kedCZHM&-|DLEMO&;+S1grE=w!F7kMlbGoNK+hPeZAt{*M9ET;lKB=-yZ
zk<aq650eMuWG70%p%%B`B;ZVvn}iZZ;2?))Zsez1oY$(IX06>WSDKaO^>)2oKMAK<
ze;nbI{L(ll%jR2GvKL=!*Z3=yPQBGES6i#iPWwhMv)U-P7RoIQEgo0TAE!gwvtDkm
zRI0U5<2w90JO}W<JSDGMM^feruQxkAt+~QY$d<2L(=?FZTIFn}QLD69d96`vcIMT>
zZ`zYZeWAYCGXRb%Jf`rttlAU9$6!9u9O_W7|IyMk4I`@!$oK65YRIqc-DAIE79saf
z&sL2-B}I7qFjPnVPG!=WnYm5ZrWoyJU}RzBVB}#G);<)Ta1El9CE60ewGvq(O+$mj
zy-j&`JrK5V2Em#m%^`zyj3tl1CqHq*jju$WnI}m%4;=&<%v#d;eNj-pg2?peF?*0e
zPDW&78#2Mpik!;J2}ix1>kD&4_^{&0B?2+Jqys-I0%+Nc$PM?qFa;LO>{}bnJci?&
z0il>BAgSLyNni!lM`>W{s|l|}m>ZW<p00QO(i}9tppgyz2Mye@nBQ?k0GGf;gzUaI
z0B=fp0f!GOx4?b=B1q(kK%_)kBt&u%uUulwgu9@83pzAuQttCk$pvrG+6O-k59N1W
zW0*gL92KCaw^tiIeYw`;N5Bk=@|yn?4dvTDmziLY?v)3ETk`W@+Byf}_JXt$yQn6^
z#3<m!#9&e{(m)Srkrp+J4%}T!+|xELV)aW1FCe@K(9@f%joF_6Kr{AxTjf6b&xCE&
z?>K84+gMHAMwQ~15j23%;Qvv7`!9Gxo1cIT`F(mKoW%Sogr^anK{$he-Kh%kv&f#6
z`!jdw8}f_HYgJqv9|y*cG^XJm$C9WEy)TJf5IkTgiRRiqLC=zF<*p@+NEa5<2MM5|
z(8T^kQM^7jM<7#^x3ar8E<n#2k%8*RT%}zGv=Qj{j%9)T{A=0FKuwP`9n^IcBVj^~
z%uWpxYGik2nUTeE4_FV>7>9{FMc}Ks@aIv8)y>;cx1#(kHzt3|O<8$34qlbx`STln
zjrtihisNk$eO|)g0KyQ$Fv7zKWrR_LF@T=msaKcgS1P>HX!G6Zegq(Ncv3|xoX;<}
zs`E9@doCIs4g$z0F!3P5Ap~m=;<(Hg_J`j9ZI1(xzEWu(Evu2F@X?YqfPqZ53X}Av
z{GjlvwyDX9;u(5cwu)o3>JB`Q1@R*AClHnZLbt!C3~qx}I+b})_!eh1-t$_`<@I=L
zb<B?tRI5FQ?3}z;JW79)>Cy<O@4?bYDWo%?+(#ie3uiV1Z}fO_|IpzAUO28cFpmt^
v<}KMC8r_3u0s?F}x_7`k?@c<pI0|d%_Dr}&E9-n0Sn|`Maaxi;4o&?H)v>-y

diff --git a/CoLo-AT/data_analysis/data_analyzer.py b/CoLo-AT/data_analysis/data_analyzer.py
index f608879..e1d352c 100644
--- a/CoLo-AT/data_analysis/data_analyzer.py
+++ b/CoLo-AT/data_analysis/data_analyzer.py
@@ -25,14 +25,17 @@ class Analyzer():
 	def set_dataset_label(self, dataset_labels):
 		self.dataset_labels = dataset_labels
 
-	def plot_loc_err_and_trace(self, loc_err, trace, time_arr, measurement_count, recorder_name = None):
+	def plot_loc_err_and_trace(self, loc_err, trace, time_arr, operations_distr, recorder_name = None):
 		
-		[lm_measurement_num, relative_measurment_num] = measurement_count
+		[lm_measurement_num, relative_measurment_num, comm_num] = operations_distr
 		fig = plt.figure()
 		plt.suptitle('Correctness analysis')
-		fig1 = fig.add_subplot(311)
-		fig2 = fig.add_subplot(312)
-		fig3 = fig.add_subplot(313)
+
+		fig1 = fig.add_subplot(411)
+		fig2 = fig.add_subplot(412)
+		fig3 = fig.add_subplot(413)
+		fig4 = fig.add_subplot(414)
+
 
 		fig1.plot(time_arr, loc_err, label = recorder_name)
 		fig2.plot(time_arr, trace, label = recorder_name)
@@ -40,6 +43,8 @@ class Analyzer():
 		fig3.bar(time_arr, relative_measurment_num,  label = "relative observation")
 		fig3.bar(time_arr, lm_measurement_num, bottom = relative_measurment_num, label = "landmark observation")
 
+		fig4.bar(time_arr, comm_num,  label = "communication")
+
 		fig1.set_title('Estimation deviation error')
 		fig1.set_xlabel('Time[s]')
 		fig1.set_ylabel('RMS[m]') 
@@ -53,20 +58,25 @@ class Analyzer():
 		fig2.legend(loc='center left', bbox_to_anchor=(1, 0.5))
 
 
-		fig3.set_title('Observation counts')
+		fig3.set_title('Observation distrubution [operation/sec]')
 		fig3.set_xlabel('Time[s]')
 		fig3.set_ylabel('Num of obser') 
 		#fig1.set_ylim(0, 6)
 		fig3.legend(loc='center left', bbox_to_anchor=(1, 0.5))
 		#fig3.tick_params(labelsize=14)
 
+		fig3.set_title('communication distrubution [operation/sec]')
+		fig3.set_xlabel('Time[s]')
+		fig3.set_ylabel('Num of obser') 
+		#fig1.set_ylim(0, 6)
+		fig3.legend(loc='center left', bbox_to_anchor=(1, 0.5))
 
 		fig.subplots_adjust(hspace = 1.2)
 		plt.show()
 
 
 
-	def calculate_loc_err_and_trace_state_variance_per_run(self, data_recorder, unit_time_interval = 0.5, plot_graphs = True):
+	def calculate_loc_err_and_trace_state_variance_per_run(self, data_recorder, unit_time_interval = 1, plot_graphs = True):
 		#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()
@@ -83,6 +93,7 @@ class Analyzer():
 		loc_err_per_run = []
 		lm_measurement_num = []
 		relative_measurment_num = []
+		comm_num = []
 		trace_per_run = []
 		time_arr = []
 
@@ -92,6 +103,7 @@ class Analyzer():
 			num_dataline_per_time_iterval = 0
 			lm_measurement_count = 0
 			relative_measurement_count = 0
+			comm_count = 0
 
 			while interval_start_time <= time < interval_start_time+unit_time_interval:
 				try:
@@ -105,6 +117,9 @@ class Analyzer():
 					lm_measurement_count +=1
 				if update_in_time_order[time_index] == 'relative observation':
 					relative_measurement_count +=1
+				if update_in_time_order[time_index] == 'communication':
+					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]
@@ -125,6 +140,7 @@ class Analyzer():
 
 			lm_measurement_num.append(lm_measurement_count)
 			relative_measurment_num.append(relative_measurement_count)
+			comm_num.append(comm_count)
 
 
 			time_arr.append((interval_start_time+unit_time_interval+interval_start_time)/2)
@@ -136,7 +152,7 @@ class Analyzer():
 		print('Avg trace of state variances per run: ', sum(trace_per_run)/len(trace_per_run))
 		
 		if plot_graphs:
-			self.plot_loc_err_and_trace(loc_err_per_run, trace_per_run, time_arr, measurement_count = [lm_measurement_num, relative_measurment_num], recorder_name = recorder_name)
+			self.plot_loc_err_and_trace(loc_err_per_run, trace_per_run, time_arr, operations_distr = [lm_measurement_num, relative_measurment_num, comm_num], recorder_name = recorder_name)
 
 		return loc_err_per_run, trace_per_run, time_arr
 
@@ -201,7 +217,7 @@ class Analyzer():
 		return robot_loc_time_unit
 
 
-	def algos_comparison_graph(self, arr_loc_err, arr_trace):
+	def algos_comparison_graph(self, arr_loc_err, arr_trace, only_trace):
 		fig = plt.figure()
 		fig1 = fig.add_subplot(211)
 		fig2 = fig.add_subplot(212)
@@ -210,19 +226,22 @@ class Analyzer():
 			fig1.plot(loc_err[0], loc_err[1], label = loc_err[2])
 		
 		for trace in arr_trace:
-			fig2.plot(trace[0], trace[1], label = trace[2])
+			if trace[2] in only_trace:
+				fig2.plot(trace[0], trace[1], '--' ,label = trace[2])
+			else:
+				fig2.plot(trace[0], trace[1], label = trace[2])
 
 		fig1.set_title('Location error', fontsize = 24)
 		fig1.set_xlabel('Time[s]', fontsize = 22)
 		fig1.set_ylabel('RMS[m]', fontsize = 22) 
-		#fig1.set_ylim(0, 6)
+		fig1.set_ylim(0, 1)
 		fig1.legend(loc=1, fontsize = 20)
 		fig1.tick_params(labelsize=18)
 
 		fig2.set_title('Trace of state covariance', fontsize = 24)
 		fig2.set_xlabel('Time [s]', fontsize = 22)
 		fig2.set_ylabel('Sigma_s [m^2]', fontsize = 22)
-		#fig2.set_ylim(0, 0.08)
+		fig2.set_ylim(0, 0.1)
 		fig2.legend(loc=1, fontsize = 20)
 		fig2.tick_params(labelsize=18)
 
@@ -237,19 +256,34 @@ class Analyzer():
 		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)
-			if only_trace ==None or data_recorder.name not in only_trace:
+			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] )
 
 		print('Plotting Comparison Graphs')
-		self.algos_comparison_graph(arr_loc_err, arr_trace)
+		self.algos_comparison_graph(arr_loc_err, arr_trace, only_trace)
 		return arr_loc_err, arr_trace
 
 
+	def trajectory_plot(self, data_recorder, robot_labels = None):
+		if robot_labels  == None:
+			robot_labels = self.dataset_labels
 
+		fig = plt.figure()
+		plt.title('robots trajectory plot')
+		robot_loc_time_unit = self.robot_location_at_unit_time_interval(data_recorder)
 
-
-
+		for i, robot_label in enumerate(robot_labels):
+			clr = 'C'+str(i)
+			plt.plot(robot_loc_time_unit[robot_label]['est_x'], robot_loc_time_unit[robot_label]['est_y'], '--',  color=clr, label= 'Robot %d estimation' %robot_label)
+			plt.plot(robot_loc_time_unit[robot_label]['gt_x'], robot_loc_time_unit[robot_label]['gt_y'],  color=clr, label= 'Robot %d groundtruth' %robot_label)
+		
+		plt.xlabel('x-axis[m]', fontsize = 22)
+		plt.ylabel('y-axis[m]', fontsize = 22) 
+		plt.xlim(-1.5, 1.5)
+		plt.ylim(-1.5, 1.5)
+		plt.legend(loc='center left', bbox_to_anchor=(1, 0.5))
+		plt.show()
 
 
 
diff --git a/CoLo-AT/gs_sci_vs_ls_ci.py b/CoLo-AT/gs_sci_vs_ls_ci.py
index e3bba30..a9d9c97 100644
--- a/CoLo-AT/gs_sci_vs_ls_ci.py
+++ b/CoLo-AT/gs_sci_vs_ls_ci.py
@@ -41,7 +41,7 @@ state_recorder = StatesRecorder('GS_CI', robot_labels)
 
 sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = False)
 loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = True)
-
+analyzer.trajectory_plot(state_recorder)
 #animate_plot(robot_labels, state_recorder, analyzer, testing_dataset.get_landmark_map())
 
 ##############################################################################
@@ -57,7 +57,7 @@ sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_C
 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 = True)
 
 ##############################################################################
-'''
+
 testing_dataset.dataset_reset()
 loc_algo = EKF_GS_SCI2('algo')
 robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
@@ -66,5 +66,5 @@ 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)
 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 = True)
-'''
-analyzer.algos_comparison([state_recorder, state_recorder_LS_CI])
+
+analyzer.algos_comparison([state_recorder, state_recorder_LS_CI, state_recorder_GS_SCI])
diff --git a/CoLo-AT/isrr2017_v2_2019.py b/CoLo-AT/isrr2017_v2_2019.py
index 344afec..dc085bf 100644
--- a/CoLo-AT/isrr2017_v2_2019.py
+++ b/CoLo-AT/isrr2017_v2_2019.py
@@ -90,4 +90,4 @@ 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)
 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 = True)
 
-analyzer.algos_comparison([state_recorder, state_recorder_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI])
+analyzer.algos_comparison([state_recorder, state_recorder_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI], only_trace = ['gs_ci_bound'])
diff --git a/CoLo-AT/localization_algos/__pycache__/ekf_gs_sci2.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_gs_sci2.cpython-36.pyc
index 39fb3191fa91a4062b35e5ae88f5b1d2fec4f8b1..4f66e05ad5c60b1729d13e9a81931ba09d7542fb 100644
GIT binary patch
delta 249
zcmZ3fepZ9in3tEUwdqbw-9}DvMrj-UjQreG{qoG5oXo^redm0id@!Nw7_vEsF@lLP
zWb#pFca{`JX@<!>EQcA@CSPE2W}3z{xqw4zvOeoMl?jX$vr5=&xSJVW7$z{r_SA}`
zFx7~NFw~0Hh}5uUGfiMDIxtz9Z7bs^AQ`I5Q^O;|P{Sp{P{NwUnZne}D8gXQz|3I7
zP$O1@Avbv<n>|q9Wi|swuE{Ly2CPNG3=9mDP1sK{+D!h>t}Z6T$ic+LXu)W}D8gvP
dV!>#}D8*>QyxELHj+1@XOb~dvIa<(<2>|}&K@$J~

delta 223
zcmX@Bu~MDWn3tC;>Stec(?(8lMxR{$jQreG{qoG5oXo^r{ou^p(wxMS%=|oE*SxaK
zqWrwv)VvbiwEQAn=luMF)FPl<S*os2esW??W))bWep+c>GDt`9W^2X>CdPotQ<&W+
z-)BC|s5E&oi!;*&#>p>OUMN&tD`BtUZf100h&@#+lEPFYBEnEBS|d`!md#Z3p+<PJ
z0Gq^QKDOD+9|}KA&S5uT<eWU0-C*)1_EU^zljm}%3-B>=FmW(iFd8t5Fj{f2Fm1lY
SA;-yn_6!KT+5BJ7j|l*(*-at<

diff --git a/CoLo-AT/localization_algos/__pycache__/ekf_ls_bda.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_ls_bda.cpython-36.pyc
index 6843e8adf7a54bbc8527d351585099c800e71b27..a42dc7eb13d092bdaa51326b6ba1e3eed3daf393 100644
GIT binary patch
delta 623
zcmajdJxe1&7zgl~&F*HC7|e?(zDA=_18Iz)pr^-qMsrsj2e!fzv#V@~2A?Egv5SRR
zSXoSGX(I?%NJ@nRKfpD1d-ny>Ty3ZGkD%d-b76k_?8D5?e_t*$zcQiuXf%9&{AYXh
zCbSkP8;mhsj?Z$`mn1IVRz1ZFc&eva+|~M+r#D!IyQ-_Zy2S_Bsq$9sDue8VdqFq2
z#j?sbipx7>6-vsJuQjMtwzGyf3(PCr5x0Rt1luv*!wsHph14san2^^5<f@li+-uhw
zjmeg%>3N=NUFr|Y*Ul793E6Hx$%FFUYyPdHl$I@;1PV}r2ByT%&|+a4<q#MKBVZKF
zfLV!e)SbQkl3ClKe)Mtxsr+EIRxVY|x-XW)Kln<k9sa8pk+0p^0KxvBF4C0_Z;CRY
z2TX}nEPMQZI$FT&*I<66wo|vsbPg>0i0~O{8FcdUimw(;r$V35`UpNq{3zL0vus;t
z=Rs)$z2JVc+8A@p3aw!t{t8+L1+WQ<;xLgEkFkDU6}|Dh*f^SV;0yRJe#diM7e?aZ
E834t5$^ZZW

delta 561
zcmajcJ4?e*6bJC!=0#s%TXgWz*48S)F1F(PRo|$%xK$z4Hk8z=aN2@Hqk|veZo0V$
zf`WAQGjwsw>{1lJg5JL_;-G>1<K*Pr%Rfmwb(8XDgF)Z9ySATr^e)Q_x+F=8SSw=H
zdIe2(pg0~!b`(dgOBL2sE}5e>wLQsTyM~7%V#ZbLR47`8&L89~f00+cn&?U_z<jN&
za?8STaYsOG_^$bzdMCTg=GR(=MO$s{*|YUEz$h%{Iv?Vq%+DRa>mUt^7A1fOAU3HW
z$zQ#bu@p+Y0`-A@FaU<Z2+#Q<kx`UsL0+$(77JCgmgi;PJ)3KF``?w#xOmd}IucR-
z&g1`)2OouoKma89c9(JSPaX_TfF6KguK)C~W|3*1l&mwFLVFq@ugfZ~vT52CT10Ce
z%n0&9vPzAjRWfUK`M5x<=;_CeYCUS36<Wev4lD!wWm*I4JQOn65uXh`b;Zya0~=t6
M>)|w0`FQy16DK2Vb^rhX

diff --git a/CoLo-AT/localization_algos/ekf_gs_bound.py b/CoLo-AT/localization_algos/ekf_gs_bound.py
index 1736d1f..cece257 100644
--- a/CoLo-AT/localization_algos/ekf_gs_bound.py
+++ b/CoLo-AT/localization_algos/ekf_gs_bound.py
@@ -122,7 +122,7 @@ class EKF_GS_BOUND(ekf_algo_framework):
 		self_theta = orinetations[index]
 		i = index * 2 
 		k = sender_idx*2
-		e =  0.9 # (iii+1)*0.01
+		e =  0.8 # (iii+1)*0.01
 		#e = comm_e
 
 		H_k = np.zeros((2,2*num_robots))
diff --git a/CoLo-AT/localization_algos/ekf_gs_sci2.py b/CoLo-AT/localization_algos/ekf_gs_sci2.py
index 482547d..fa34210 100644
--- a/CoLo-AT/localization_algos/ekf_gs_sci2.py
+++ b/CoLo-AT/localization_algos/ekf_gs_sci2.py
@@ -21,7 +21,7 @@ class EKF_GS_SCI2(ekf_algo_framework):
 		i = 2*index
 		[sigma_d, sigma_i] = sigma_s
 		total_sigma = sigma_d + sigma_i
-		trace_state_var = np.trace(total_sigma[i:i+2, i:i+2])
+		trace_state_var = np.trace(total_sigma[i:i+2, i:i+2])/2
 		return np.trace(total_sigma)
 
 
@@ -61,12 +61,12 @@ class EKF_GS_SCI2(ekf_algo_framework):
 				sigma_d[jj:jj+2, jj:jj+2] = total_sigma - sigma_i[jj:jj+2, jj:jj+2]
 
 			else:
-				'''
+				
 				total_sigma = sigma_i[jj:jj+2, jj:jj+2] + sigma_d[jj:jj+2, jj:jj+2] #repeated
 				sigma_i[jj:jj+2, jj:jj+2] += delta_t*delta_t*var_v*np.identity(2)	#unknow other robot's movement
 				total_sigma += delta_t*delta_t*var_v*np.identity(2)	
 				sigma_d[jj:jj+2, jj:jj+2] = total_sigma - sigma_i[jj:jj+2, jj:jj+2]
-				'''
+				
 
 		sigma_s = [sigma_d, sigma_i]
 		
@@ -183,7 +183,7 @@ class EKF_GS_SCI2(ekf_algo_framework):
 		[comm_robot_s, orinetations, comm_robot_sigma_s]=comm_data
 		num_robots = int(len(s)/2)
 
-		e =  0.9 # (iii+1)*0.01
+		e =  0.8 # (iii+1)*0.01
 
 		[sigma_d, sigma_i] = sigma_s
 		[comm_robot_sigma_d, comm_robot_sigma_i] = comm_robot_sigma_s
diff --git a/CoLo-AT/localization_algos/ekf_ls_bda.py b/CoLo-AT/localization_algos/ekf_ls_bda.py
index 4cde55a..83d0ad6 100644
--- a/CoLo-AT/localization_algos/ekf_ls_bda.py
+++ b/CoLo-AT/localization_algos/ekf_ls_bda.py
@@ -24,7 +24,8 @@ class EKF_LS_BDA(ekf_algo_framework):
 
 	def calculate_trace_state_variance(self, robot_data):
 		[s, orinetations, sigma_s, index] = robot_data
-		trace_state_var = np.trace(sigma_s)
+		i = 2*index
+		trace_state_var = np.trace(sigma_s[i:i+2, i:i+2])
 		return trace_state_var
 
 	def propagation_update(self, robot_data, sensor_data):
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 16eb67c30e1e3d7f4b2e61a5c101b65f43765efa..9ac93229e406d60d4e8032c2aeb523b077c79cdb 100644
GIT binary patch
delta 25
gcmcbhdO?-bn3tDpV%wb<-i@4vf{ZMis|7<D0cslueE<Le

delta 25
gcmcbhdO?-bn3tF9tNiVlh>e_uf{bjNs|7<D0d3R=#sB~S

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 85c0e77429d59df4555496f0556e72434261e905..b2e41bb739ac4014d2d5469ce928dfdf2596a391 100644
GIT binary patch
delta 18
acmcbsdRLXxn3tDp>f^gH;u|@y2mt^<L<Y?O

delta 18
ZcmcbsdRLXxn3tD}A>ek5>_*NjLI5^@1*rf4

diff --git a/CoLo-AT/simulation_process/sim_manager.py b/CoLo-AT/simulation_process/sim_manager.py
index 6bf24ca..e454cb1 100644
--- a/CoLo-AT/simulation_process/sim_manager.py
+++ b/CoLo-AT/simulation_process/sim_manager.py
@@ -169,9 +169,7 @@ class SimulationManager():
 				robot_state = robot_system.localization_update(rsp)
 				#state_recorder.record_state(rsp, robot_state)
 
-			#print(req_type)			
-			#print('time:', self.time)
-			#print('#########')
+
 			self.time = next_possible_time_array[req_type_idx][robot_idx]
 			if self.time  < prev_time:
 				print('current time:', self.time)
diff --git a/CoLo-AT/simulation_process/state_recorder.py b/CoLo-AT/simulation_process/state_recorder.py
index 9a40f14..be7af24 100644
--- a/CoLo-AT/simulation_process/state_recorder.py
+++ b/CoLo-AT/simulation_process/state_recorder.py
@@ -144,8 +144,3 @@ class StatesRecorder():
 
 		return True
 
-
-
-
-
-		
\ No newline at end of file
-- 
GitLab