added isrr2017 sim

CoLo-AT/data_analysis/data_analyzer.py

@@ -26,6 +26,8 @@ class Analyzer():
 		self.dataset_labels = dataset_labels
 
 	def plot_loc_err_and_trace(self, loc_err, trace, time_arr, measurement_count, recorder_name = None):
+		
+		[lm_measurement_num, relative_measurment_num] = measurement_count
 		fig = plt.figure()
 		plt.suptitle('Correctness analysis')
 		fig1 = fig.add_subplot(311)
@@ -34,8 +36,9 @@ class Analyzer():
 
 		fig1.plot(time_arr, loc_err, label = recorder_name)
 		fig2.plot(time_arr, trace, label = recorder_name)
-		fig3.bar(time_arr, measurement_count, label = recorder_name)
-
+		#fig3.bar(time_arr, lm_measurement_num, label = "landmark observation")
+		fig3.bar(time_arr, relative_measurment_num,  label = "relative observation")
+		fig3.bar(time_arr, lm_measurement_num, bottom = relative_measurment_num, label = "landmark observation")
 
 		fig1.set_title('Estimation deviation error')
 		fig1.set_xlabel('Time[s]')
@@ -78,7 +81,8 @@ class Analyzer():
 		finish_flag = False
 
 		loc_err_per_run = []
-		measurement_num = []
+		lm_measurement_num = []
+		relative_measurment_num = []
 		trace_per_run = []
 		time_arr = []
 
@@ -86,7 +90,8 @@ class Analyzer():
 			loc_err_per_time_iterval = 0
 			trace_per_time_iterval = 0
 			num_dataline_per_time_iterval = 0
-			measurement_count = 0
+			lm_measurement_count = 0
+			relative_measurement_count = 0
 
 			while interval_start_time <= time < interval_start_time+unit_time_interval:
 				try:
@@ -96,8 +101,10 @@ class Analyzer():
 					break
 				time = data_in_time_order[time_index][0]
 
-				if update_in_time_order[time_index] == 'landmark observation' or update_in_time_order[time_index] == 'relative observation':
-					measurement_count +=1
+				if update_in_time_order[time_index] == 'landmark observation':
+					lm_measurement_count +=1
+				if update_in_time_order[time_index] == 'relative observation':
+					relative_measurement_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]
@@ -116,7 +123,10 @@ class Analyzer():
 				loc_err_per_run.append(0)
 				trace_per_run.append(0)
 
-			measurement_num.append(measurement_count)
+			lm_measurement_num.append(lm_measurement_count)
+			relative_measurment_num.append(relative_measurement_count)
+
+
 			time_arr.append((interval_start_time+unit_time_interval+interval_start_time)/2)
 			interval_start_time = interval_start_time+unit_time_interval
 
@@ -126,7 +136,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 = measurement_num, recorder_name = recorder_name)
+			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)
 
 		return loc_err_per_run, trace_per_run, time_arr
 
@@ -222,6 +232,7 @@ class Analyzer():
 
 
 	def algos_comparison(self, arr_data_recorder, only_trace=None):
+		print("************Algorithm Comparison***************")
 		arr_loc_err = []
 		arr_trace = []
 		for data_recorder in arr_data_recorder:

CoLo-AT/gs_sci_vs_ls_ci.py

+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Sun Apr  8 17:55:06 2018
+
+@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 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_dataset1/" # 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 = 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_CI])

CoLo-AT/isrr2017_v2_2019.py

+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Sun Apr  8 17:55:06 2018
+
+@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 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 = 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])

CoLo-AT/localization_algos/ekf_ls_ci.py

@@ -40,8 +40,8 @@ class EKF_LS_CI(ekf_algo_framework):
 		s[i+1,0] = s[i+1,0] + sin(self_theta)*v*delta_t #y
 
 		var_u_v = sigma_odo[0,0]
-		sigma_s[i:i+2, i:i+2] = sigma_s[i:i+2, i:i+2]+ delta_t*delta_t*rot_mtx(self_theta)*matrix([[var_u_v, 0],[0, 0]])*rot_mtx(self_theta).T
-
+		#sigma_s[i:i+2, i:i+2] = sigma_s[i:i+2, i:i+2]+ delta_t*delta_t*rot_mtx(self_theta)*matrix([[var_u_v, 0],[0, 0]])*rot_mtx(self_theta).T
+		sigma_s[i:i+2, i:i+2] = sigma_s[i:i+2, i:i+2]+ delta_t*delta_t*rot_mtx(self_theta)*sigma_odo*rot_mtx(self_theta).T
 		return [s, orinetations, sigma_s]
 
 
@@ -78,8 +78,6 @@ class EKF_LS_CI(ekf_algo_framework):
 		z_hat = rot_mtx(self_theta).getT()*(np.matrix([delta_x, delta_y]).getT())  # shifted to robot frame
 
 
-
-
 		sigma_z = rot_mtx(bearing) * sigma_ob * rot_mtx(bearing).getT() 
 		sigma_invention = H * local_sigma * H.getT()  + sigma_z
 		kalman_gain = local_sigma*H.getT()*sigma_invention.getI()

CoLo-AT/localization_algos/gs_ci_bound.py

@@ -12,7 +12,7 @@ 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.2
+		self.var_v = 0.1
 
 	def state_variance_init(self, num_robots):
 		return 0.01*np.matrix(np.identity(2*num_robots), dtype = float)