diff --git a/CoLo-AT/Algorithm Comparison.pdf b/CoLo-AT/Algorithm Comparison.pdf
index ecf54c22f8094da499d2356a5eb9472b84c1e26d..be8592ab63d020456462f6a207fa78bf16221817 100644
Binary files a/CoLo-AT/Algorithm Comparison.pdf and b/CoLo-AT/Algorithm Comparison.pdf differ
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 00026763e21aa8bcb83c2edf9f8367b7c06a0654..7c702c1fe6559289fc6ad2a6503301457bd71125 100644
Binary files a/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc and b/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc differ
diff --git a/CoLo-AT/data_analysis/data_analyzer.py b/CoLo-AT/data_analysis/data_analyzer.py
index a68e60d46dae216d9bf2ed275fbb33b57ad6028d..d1b233b6d10c97d17fc37a6a3cf43cbac14e2116 100644
--- a/CoLo-AT/data_analysis/data_analyzer.py
+++ b/CoLo-AT/data_analysis/data_analyzer.py
@@ -91,14 +91,19 @@ class Analyzer():
 		finish_flag = False
 
 		loc_err_per_run = []
+		state_err_per_run = []
+		trace_per_run = []
+
+
 		lm_measurement_num = []
 		relative_measurment_num = []
 		comm_num = []
-		trace_per_run = []
+		
 		time_arr = []
 
 		while True:
 			loc_err_per_time_iterval = 0
+			state_err_per_time_interval = 0
 			trace_per_time_iterval = 0
 			num_dataline_per_time_iterval = 0
 			lm_measurement_count = 0
@@ -123,7 +128,8 @@ class Analyzer():
 
 				
 				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]
+					loc_err_per_time_iterval += data_in_time_order[time_index][7]
+					state_err_per_time_interval += np.linalg.norm(data_in_time_order[time_index][8] - data_in_time_order[time_index][9])
 					trace_per_time_iterval += data_in_time_order[time_index][4]
 					num_dataline_per_time_iterval+=1
 				
@@ -134,10 +140,12 @@ class Analyzer():
 
 			if num_dataline_per_time_iterval != 0:
 				loc_err_per_run.append(loc_err_per_time_iterval/num_dataline_per_time_iterval)
+				state_err_per_run.append(state_err_per_time_interval/num_dataline_per_time_iterval)
 				trace_per_run.append(trace_per_time_iterval/num_dataline_per_time_iterval)
 				   
 			else:
 				loc_err_per_run.append(0)
+				state_err_per_run.append(0)
 				trace_per_run.append(0)
 
 			lm_measurement_num.append(lm_measurement_count)
@@ -155,7 +163,7 @@ class Analyzer():
 		if plot_graphs:
 			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
+		return loc_err_per_run, state_err_per_run, trace_per_run, time_arr
 
 	def robot_location_at_unit_time_interval(self, data_recorder, unit_time_interval = 0.2):
 		#recorded_dataline = [time, robot_label, est_x_pos, est_y_pos, trace_state_var, gt_x_pos, gt_y_pos, loc_err] 
@@ -262,9 +270,9 @@ class Analyzer():
 		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, selected_labels = selected_labels)
+			loc_err_per_run, state_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_loc_err.append([time_stamps, state_err_per_run, data_recorder.name])
 			arr_trace.append([time_stamps, trace_per_run, data_recorder.name] )
 
 		print('Plotting Comparison Graphs')
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 381de36ccc1f4eb7c7dd8ef1ec3fc445ea46b36d..35c5604c197dbd43ba397d71f598ccbad8207c35 100644
Binary files a/CoLo-AT/dataset_manager/__pycache__/simulated_dataset_manager_w.cpython-36.pyc and b/CoLo-AT/dataset_manager/__pycache__/simulated_dataset_manager_w.cpython-36.pyc differ
diff --git a/CoLo-AT/gs_ci_journal_sim.py b/CoLo-AT/gs_ci_journal_sim.py
index 9769bc128971ac02919e36ca4e946e28d93ff1b3..9d4b5c4d9341091d1b8594dc99f8c602ca94197e 100644
--- a/CoLo-AT/gs_ci_journal_sim.py
+++ b/CoLo-AT/gs_ci_journal_sim.py
@@ -27,7 +27,7 @@ from ekf_gs_sci2 import EKF_GS_SCI2
 
 
 ##############################################################################
-duration = 200
+duration = 100
 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)
@@ -40,7 +40,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 = False, selected_labels = [1])
+loc_err_per_run, state_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = False, selected_labels = [1])
 
 #analyzer.trajectory_plot(state_recorder)
 
@@ -55,10 +55,9 @@ 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_b, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_bound, plot_graphs = False, selected_labels = [1])
+loc_err_per_run, state_err_per_run, trace_per_run_b, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_bound, plot_graphs = False, selected_labels = [1])
 ##############################################################################
 
-
 testing_dataset.dataset_reset()
 loc_algo = Centralized_EKF('algo')
 robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
@@ -66,7 +65,7 @@ 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 = False)
+loc_err_per_run, state_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)
 
 ##############################################################################
 
@@ -77,7 +76,7 @@ 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 = False)
+loc_err_per_run, state_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)
 
 
 ##############################################################################
@@ -89,7 +88,7 @@ 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 = False)
+loc_err_per_run, state_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)
 
 ##############################################################################
 
@@ -100,6 +99,8 @@ 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 = False)
+loc_err_per_run, state_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_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI, state_recorder_bound], only_trace = ['gs_ci_bound'], selected_labels = [1])
+
 
-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'], 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 bac96246273de72566a102ee4ecc5854305bbdd9..a02f049cf7ac54d49143049b5fe2c2769f6fa360 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-36.pyc differ
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 df5af6c196922ce05dc1069d82fc707493feac86..ba0a8a86ae49dfa3867677edda712eb20893b78f 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/ekf_ls_bda.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/ekf_ls_bda.cpython-36.pyc differ
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 ae10aa7792e6ae1c7580c28e4258839aa5d64c59..607a82df754796bfe76937e000767780fb5e9098 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-36.pyc differ
diff --git a/CoLo-AT/localization_algos/ekf_gs_bound.py b/CoLo-AT/localization_algos/ekf_gs_bound.py
index fe91db4da8083b8dc1da422d1765a0e39196a3b5..320691a48391d1d1b93fe89963aa49d7a8f4aaa1 100644
--- a/CoLo-AT/localization_algos/ekf_gs_bound.py
+++ b/CoLo-AT/localization_algos/ekf_gs_bound.py
@@ -10,10 +10,10 @@ def rot_mtx(theta):
 class EKF_GS_BOUND(ekf_algo_framework):
 	def __init__(self, algo_name):
 		ekf_algo_framework.__init__(self, algo_name)
-		self.var_v = 0.04
-		self.var_range = 0.7
-		self.var_bearing = 0.3
-		self.max_range  = 3.5 
+		self.var_v = 0.1**2
+		self.var_range = 0.1**2
+		self.var_bearing = 0.3**2
+		self.max_range  = 0.001
 
 	def state_variance_init(self, num_robots):
 		return 0.1*np.matrix(np.identity(2*num_robots), dtype = float)
@@ -22,7 +22,7 @@ class EKF_GS_BOUND(ekf_algo_framework):
 		[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
@@ -49,6 +49,7 @@ class EKF_GS_BOUND(ekf_algo_framework):
 			jj = 2*j
 			sigma_s[jj:jj+2, jj:jj+2] += delta_t*delta_t*var_v*np.identity(2)
 
+		print(np.trace(sigma_s))
 		return [s, orinetations, sigma_s]
 
 	def absolute_obser_update(self, robot_data, sensor_data):
@@ -71,13 +72,13 @@ class EKF_GS_BOUND(ekf_algo_framework):
 		H = rot_mtx(self_theta).getT()*H_i
 
 		var_range = self.var_range
-		var_range = self.var_range
-		range_max = self.range_max
+		max_range = self.max_range
 		var_bearing = self.var_bearing
 
-		R = np.matrix(max(var_range, range_max, var_bearing)*np.identity(2))
+		R = np.matrix(max(var_range, max_range**2*var_bearing)*np.identity(2))
 		sigma_s = (sigma_s.getI()+H.getT()*R.getI()*H).getI()
 
+
 		return [s, orinetations, sigma_s]
 
 	
diff --git a/CoLo-AT/localization_algos/ekf_gs_ci2.py b/CoLo-AT/localization_algos/ekf_gs_ci2.py
index 2adbad7ba45ad7ce4e03bc59ace54ee46803113b..fd399d232e4f791a519343d5c95e3e4bdaaef87e 100644
--- a/CoLo-AT/localization_algos/ekf_gs_ci2.py
+++ b/CoLo-AT/localization_algos/ekf_gs_ci2.py
@@ -38,7 +38,7 @@ class EKF_GS_CI2(ekf_algo_framework):
 		self_theta = orinetations[index]
 
 		Q = sigma_odo
-		Q[1,1] = Q[1,1]*v*v
+		#Q[1,1] = Q[1,1]*v*v#
 		W = delta_t*rot_mtx(self_theta)
 
 		s[i,0] = s[i,0] + cos(self_theta)*v*delta_t   #x
diff --git a/CoLo-AT/localization_algos/gs_ci_bound.py b/CoLo-AT/localization_algos/gs_ci_bound.py
index 44f08048b41efe65509bdaaef92c4fdef21f60b2..d3d7fbf1835d7b9d0000d93aa3490dadc11ab28c 100644
--- a/CoLo-AT/localization_algos/gs_ci_bound.py
+++ b/CoLo-AT/localization_algos/gs_ci_bound.py
@@ -9,10 +9,11 @@ def rot_mtx(theta):
 class GS_CI_Bound(ekf_algo_framework):
 	def __init__(self, algo_name):
 		self.algo_name = algo_name
-		self.d_max = 2 # max measurement distance 
-		self.d_var = 0.2
-		self.bearing_var = 0.2
-		self.var_v = 0.25
+
+		self.var_v = 0.25**2
+		self.var_range = 0.3**2
+		self.var_bearing = 0.3**2
+		self.max_range  = 1.5
 
 	def state_variance_init(self, num_robots):
 		return 0.04*np.matrix(np.identity(2*num_robots), dtype = float)
@@ -37,10 +38,6 @@ class GS_CI_Bound(ekf_algo_framework):
 		for j in range(num_robots):
 			jj = 2*j
 
-		if j==index:
-			sigma_s[jj:jj+2, jj:jj+2] += sigma_odo[0,0]*np.identity(2)*delta_t*delta_t
-		else:
-			#var_v = pow(2*self.d_max, 2)/12
 			var_v = self.var_v
 			sigma_s[jj:jj+2, jj:jj+2] += var_v*np.identity(2)*delta_t*delta_t
 		
@@ -60,11 +57,11 @@ class GS_CI_Bound(ekf_algo_framework):
 		H_i[0, i] = -1
 		H_i[1, i+1] = -1
 
-		d_max = self.d_max
-		var_dis = self.d_var
-		var_phi = self.bearing_var
+		max_range = self.max_range
+		var_dis = self.var_range
+		var_phi = self.var_bearing
 
-		sigma_th_z =  np.matrix(max(var_dis, d_max*d_max*var_phi)*np.identity(2))
+		sigma_th_z =  np.matrix(max(var_dis, max_range*max_range*var_phi)*np.identity(2))
 		sigma_s = (sigma_s.getI() + H_i.getT() * sigma_th_z.getI() * H_i).getI()
 
 		return [s, orinetations, sigma_s]
@@ -90,11 +87,11 @@ class GS_CI_Bound(ekf_algo_framework):
 		H_ij[0, j] = 1
 		H_ij[1, j+1] = 1
 
-		d_max = self.d_max
-		var_dis = self.d_var
-		var_phi = self.bearing_var
+		max_range = self.max_range
+		var_dis = self.var_range
+		var_phi = self.var_bearing
 
-		sigma_th_z =  np.matrix(max(var_dis, d_max*d_max*var_phi)*np.identity(2))
+		sigma_th_z =  np.matrix(max(var_dis, max_range*max_range*var_phi)*np.identity(2))
 		sigma_s = (sigma_s.getI() + H_ij.getT() * sigma_th_z.getI() * H_ij).getI()
 		return [s, orinetations, sigma_s]
 
diff --git a/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc
index e6f893b9bb34363e909e83cc8442b5731bb39e34..1fd2769d85d4c09a6c7592f51c8ff5792061162a 100644
Binary files a/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc and b/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc differ
diff --git a/CoLo-AT/robots/robot_system.py b/CoLo-AT/robots/robot_system.py
index 075be13efdc5fd156693047456735fc42c3ce749..bea373646609bb56e5ae55314ec53fae447a4576 100644
--- a/CoLo-AT/robots/robot_system.py
+++ b/CoLo-AT/robots/robot_system.py
@@ -31,7 +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:
@@ -40,6 +40,11 @@ class RobotSystem:
 		else:
 			self.robot_sys.set_starting_state(start_state_arr)
 
+
+		self.gt_orientations = np.zeros(self.num_robots)
+		for i, label in enumerate(self.robot_labels):
+			self.gt_orientations[i] = start_state_arr[label][3]
+
 	def request_data(self, time):
 		pass
 	'''
@@ -82,7 +87,7 @@ class RobotSystem:
 			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.1]]) #with respect to velocity and orientation
+			sigma_odo = np.matrix([[0.01, 0], [0, 0.01]]) #with respect to velocity and orientation
 			sensor_covariance = sigma_odo
 			delta_t = message_data['delta_t']
 			
@@ -98,7 +103,7 @@ class RobotSystem:
 
 		elif rsp_type == 'measurement':
 
-			sigma_obser = np.matrix([[0.015, 0], [0,0.01]]) #with respect to range and bearing
+			sigma_obser = np.matrix([[0.1**2, 0], [0, 0.035**2]]) #with respect to range and bearing
 			sensor_covariance = sigma_obser
 			obj_id = message_data['subject_ID']
 			meas_range = message_data['measurment_range'] 
@@ -155,5 +160,5 @@ class RobotSystem:
 				[est_states, est_orientaions, est_state_variance]= self.robot_sys.get_status()
 				trace_sigma_s = self.robot_sys.get_trace_state_variance(robot_index)
 
-		robot_state = {'x_pos':est_states[2*robot_index], 'y_pos': est_states[2*robot_index+1], 'trace of state variance': trace_sigma_s, 'state variance': est_state_variance, 'update_type': update_type }
+		robot_state = {'x_pos':est_states[2*robot_index], 'y_pos': est_states[2*robot_index+1], 'trace of state variance': trace_sigma_s, 'state variance': est_state_variance, 'update_type': update_type, 'est_states': est_states.copy() }
 		return robot_state
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 8406071719767482050a4be41f2168eaa7139445..1450bcf5f5e286f03e7a2161ce1cbf2616342420 100644
Binary files a/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc and b/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc differ
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 7d0b96af6d2bd113c042393330c80a3848183572..fecf62bb365b60850ecd554432651b7f9690698c 100644
Binary files a/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-36.pyc and b/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-36.pyc differ
diff --git a/CoLo-AT/simulation_process/state_recorder.py b/CoLo-AT/simulation_process/state_recorder.py
index 97a6696c01f40786d2c831038387942ced76e1a9..ad6a540a861e827f64668cd5e392325ef8ea9a22 100644
--- a/CoLo-AT/simulation_process/state_recorder.py
+++ b/CoLo-AT/simulation_process/state_recorder.py
@@ -21,6 +21,7 @@ class StatesRecorder():
 		self.loc_err_arr = {}
 		self.trace_sigma_s_arr = {}
 		self.updata_type_arr = {}
+		self.gt_states = np.matrix(np.zeros((2*self.num_robots,1)))
 		for i, label in enumerate(self.dataset_labels):
 			self.recorded_data[label]=[]
 			self.loc_err_arr[label]=[]
@@ -28,6 +29,10 @@ class StatesRecorder():
 			self.updata_type_arr[label]=[]
 
 	def set_starting_state(self, stating_states):
+		for i, label in enumerate(self.dataset_labels):
+			self.gt_states[2*i,0] = stating_states[label][1]
+			self.gt_states[2*i+1,0] = stating_states[label][2]
+
 		for i, label in enumerate(self.dataset_labels):
 			self.start_time = stating_states[label][0]
 			time = 0
@@ -35,7 +40,7 @@ class StatesRecorder():
 			y_pos = stating_states[label][2]
 			initial_trace_state_var = 0.0001
 			loc_err = 0
-			recorded_dataline = [time, label, x_pos, y_pos, initial_trace_state_var, x_pos, y_pos, loc_err] 
+			recorded_dataline = [time, label, x_pos, y_pos, initial_trace_state_var, x_pos, y_pos, loc_err, self.gt_states.copy(), self.gt_states.copy()] 
 			self.data_in_time_order.append(recorded_dataline)	
 			self.recorded_data[label].append(recorded_dataline)	
 			self.loc_err_arr[label].append(0)
@@ -60,17 +65,20 @@ class StatesRecorder():
 		est_y_pos = float(robot_state['y_pos'])
 		trace_state_var = robot_state['trace of state variance']
 		updata_type = robot_state['update_type']
+		est_states = robot_state['est_states'].copy()
 		gt_x_pos = gt['x_pos']
 		gt_y_pos = gt['y_pos']
 		robot_label = self.dataset_labels[robot_idx]
 		
+		self.gt_states[2*robot_idx,0] = gt_x_pos
+		self.gt_states[2*robot_idx+1,0] = gt_y_pos
 		
 		if self.state_var_only:
 			loc_err = 0
-			recorded_dataline = [time, robot_label, gt_x_pos, gt_y_pos, trace_state_var, gt_x_pos, gt_y_pos, loc_err] 
+			recorded_dataline = [time, robot_label, gt_x_pos, gt_y_pos, trace_state_var, gt_x_pos, gt_y_pos, loc_err, self.gt_states.copy(), self.gt_states.copy()] 
 		else:
 			loc_err = sqrt((est_x_pos-gt_x_pos)*(est_x_pos-gt_x_pos)+(est_y_pos-gt_y_pos)*(est_y_pos-gt_y_pos))
-			recorded_dataline = [time, robot_label, est_x_pos, est_y_pos, trace_state_var, gt_x_pos, gt_y_pos, loc_err] 
+			recorded_dataline = [time, robot_label, est_x_pos, est_y_pos, trace_state_var, gt_x_pos, gt_y_pos, loc_err, est_states, self.gt_states.copy()] 
 		
 		#warning (optional)
 		'''
@@ -94,6 +102,7 @@ class StatesRecorder():
 		self.loc_err_arr[robot_label].append(loc_err)
 		self.trace_sigma_s_arr[robot_label].append(trace_state_var)
 		self.updata_type_arr[robot_label].append(updata_type)
+
 		
 
 	def get_data_in_time_order(self):