diff --git a/CoLo-AT/Cooperative_Localization_Journal1.py b/CoLo-AT/Cooperative_Localization_Journal1.py new file mode 100644 index 0000000000000000000000000000000000000000..d48eb2e4dfd0b8d56a5cf1509c215b3937bf08e2 --- /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 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 f608879cdffc056f76489078cb7a4b1faa586bda..e1d352cc8738a5a72d9ea862f06b63c0e9ca7928 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 e3bba30a0e831272609bb0f52b909aa2f8185632..a9d9c9728b33b62f61c52a6f1e9a01b04f29c303 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 344afecd0dd92e7ecdf14c0654d6f450ab8bbcbd..dc085bfa1cb124bded6b575013fbc0a82cc4634c 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 Binary files a/CoLo-AT/localization_algos/__pycache__/ekf_gs_sci2.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/ekf_gs_sci2.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 6843e8adf7a54bbc8527d351585099c800e71b27..a42dc7eb13d092bdaa51326b6ba1e3eed3daf393 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/ekf_gs_bound.py b/CoLo-AT/localization_algos/ekf_gs_bound.py index 1736d1f0a226851063fcf23b551f7101b6f609aa..cece257226299d8289559bb806900d69ed9dbd4d 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 482547dbca8a5758011615d5ed2b3e63e7b342e1..fa34210e7c17e4c05dad6e27232debbdd82dd412 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 4cde55ada221d9075d853ddf1fdcc6b740b251d9..83d0ad66738a22d3daad226e6b9cda232777ab96 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 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 85c0e77429d59df4555496f0556e72434261e905..b2e41bb739ac4014d2d5469ce928dfdf2596a391 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/sim_manager.py b/CoLo-AT/simulation_process/sim_manager.py index 6bf24cae28d550a2a28a19e0c37dfaf3e75659b9..e454cb1087ef3b1351f4a6e403c5a06a06103a19 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 9a40f1473d162d7d86538578fb72de231307a2c4..be7af24c1c5b50f253a2910e9c4ca6e7a43ad81c 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