diff --git a/CoLo-AT/Algorithm Comparison.pdf b/CoLo-AT/Algorithm Comparison.pdf
index a1d4ee81737be9c0f6d25f3cacc10dabf459062f..ecf54c22f8094da499d2356a5eb9472b84c1e26d 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 4e73685ba4848d50830b3fc9c200733bc22d692b..00026763e21aa8bcb83c2edf9f8367b7c06a0654 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 0e2fd6228978f0254cdfaedc08c6b9471097a663..a68e60d46dae216d9bf2ed275fbb33b57ad6028d 100644
--- a/CoLo-AT/data_analysis/data_analyzer.py
+++ b/CoLo-AT/data_analysis/data_analyzer.py
@@ -233,9 +233,9 @@ class Analyzer():
fig2.plot(trace[0], trace[1], label = trace[2])
fig1.set_title('Location error')
- fig1.set_xlabel('Time[s]')
- fig1.set_ylabel('RMS[m]')
- fig1.set_ylim(0, 0.5)
+ fig1.set_xlabel('Time [s]')
+ fig1.set_ylabel('RMS [m]')
+ fig1.set_ylim(0, 0.6)
fig1.legend(loc=1)
fig1.legend(loc='center left', bbox_to_anchor=(1, 0.5))
#fig1.tick_params(labelsize=18)
@@ -243,7 +243,7 @@ class Analyzer():
fig2.set_title('Trace of state covariance')
fig2.set_xlabel('Time [s]')
fig2.set_ylabel('Sigma_s [m^2]')
- #fig2.set_ylim(0, 0.2)
+ fig2.set_ylim(0, 0.6)
fig2.legend(loc=1)
fig2.legend(loc='center left', bbox_to_anchor=(1, 0.5))
#fig2.tick_params(labelsize=18)
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 43e34a0be729cd1dc1ba44c010498b30d68197cb..381de36ccc1f4eb7c7dd8ef1ec3fc445ea46b36d 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/dataset_manager/simulated_dataset_manager_w.py b/CoLo-AT/dataset_manager/simulated_dataset_manager_w.py
index eac9c423c322967505016909dcba1322d54c3757..bd0238ae47b672357527b99485efdb6bca3f99ad 100755
--- a/CoLo-AT/dataset_manager/simulated_dataset_manager_w.py
+++ b/CoLo-AT/dataset_manager/simulated_dataset_manager_w.py
@@ -185,7 +185,7 @@ class Sim_Dataset_Manager:
bearing = math.atan2(delta_y, delta_x) - gt_orientation + np.random.normal(0, self.noise_bearing)
message['data'] = {'time':req_time, 'subject_ID': observed_label, 'measurment_range': meas_range, 'bearing':bearing}
valid_respond = True
- print(self.robot_lables[rbt_idx], observed_label)
+ #print(self.robot_lables[rbt_idx], observed_label)
message['groundtruth'] = {'time':req_time, 'x_pos':gt_x, 'y_pos':gt_y, 'orientation':gt_orientation}
self.current_states[self.robot_lables[rbt_idx]] = {'x': gt_x, 'y': gt_y, 'orientation': gt_orientation}
diff --git a/CoLo-AT/gs_ci_journal_sim.py b/CoLo-AT/gs_ci_journal_sim.py
index 7adbe720599157ce4300882b91e6c03871949a91..9769bc128971ac02919e36ca4e946e28d93ff1b3 100644
--- a/CoLo-AT/gs_ci_journal_sim.py
+++ b/CoLo-AT/gs_ci_journal_sim.py
@@ -18,8 +18,12 @@ 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
##############################################################################
@@ -34,11 +38,11 @@ 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 = True)
+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, selected_labels = [1])
+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])
-analyzer.trajectory_plot(state_recorder)
+#analyzer.trajectory_plot(state_recorder)
@@ -50,8 +54,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 = True)
-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 = True, selected_labels = [1])
+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])
+##############################################################################
testing_dataset.dataset_reset()
@@ -60,41 +65,41 @@ 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, robots_cant_observe_lm = robots_cant_observe_lm)
- 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)
+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)
- ##############################################################################
+##############################################################################
- testing_dataset.dataset_reset()
- loc_algo = EKF_LS_BDA('algo')
- robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
+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, robots_cant_observe_lm = robots_cant_observe_lm)
- 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)
+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)
- ##############################################################################
+##############################################################################
- testing_dataset.dataset_reset()
- loc_algo = EKF_LS_CI('algo')
- robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
+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, robots_cant_observe_lm = robots_cant_observe_lm)
- 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)
+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)
- ##############################################################################
+##############################################################################
- testing_dataset.dataset_reset()
- loc_algo = EKF_GS_SCI2('algo')
- robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = 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, robots_cant_observe_lm = robots_cant_observe_lm)
- 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)
+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)
-analyzer.algos_comparison([state_recorder, state_recorder_bound], only_trace = ['gs_ci_bound'], selected_labels = [1])
\ No newline at end of file
+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__/centralized_ekf.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/centralized_ekf.cpython-36.pyc
index b622a79aa5e6e569d0346267045d4f76a5f9e92b..712e9c467ad91a8c3ed5dc2086cc45ebe01529a6 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/centralized_ekf.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/centralized_ekf.cpython-36.pyc differ
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 c9eaca6174e11a6bd773ac94e0954da503cad1bf..bac96246273de72566a102ee4ecc5854305bbdd9 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_gs_sci2.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_gs_sci2.cpython-36.pyc
index a64128bd845eeebcb7647366896682e70e402de9..c8abb3b50d3e7bbaa105b4b78ae49a4178137b74 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 a42dc7eb13d092bdaa51326b6ba1e3eed3daf393..df5af6c196922ce05dc1069d82fc707493feac86 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__/ekf_ls_ci.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_ls_ci.cpython-36.pyc
index 8f679589595ce4259c225d0622f73a6b0385245e..afce9361e52bcb64064c282a899d61e489672928 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/ekf_ls_ci.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/ekf_ls_ci.cpython-36.pyc differ
diff --git a/CoLo-AT/localization_algos/centralized_ekf.py b/CoLo-AT/localization_algos/centralized_ekf.py
index 821c31e8ad2bdde1010711d564d091b098f08715..507d7a1f3bda020a431ba21fceaeb4fb9262117c 100755
--- a/CoLo-AT/localization_algos/centralized_ekf.py
+++ b/CoLo-AT/localization_algos/centralized_ekf.py
@@ -16,13 +16,13 @@ class Centralized_EKF(ekf_algo_framework):
ekf_algo_framework.__init__(self, algo_name)
def state_variance_init(self, num_robots):
- return 0.01*np.matrix(np.identity(2*num_robots), dtype = float)
+ return 0.04*np.matrix(np.identity(2*num_robots), dtype = float)
def calculate_trace_state_variance(self, robot_data):
[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)
#ef propagation_update(self, s, orinetations, sigma_s, input_data, sigma_odo, index):
def propagation_update(self, robot_data, sensor_data):
diff --git a/CoLo-AT/localization_algos/ekf_gs_ci2.py b/CoLo-AT/localization_algos/ekf_gs_ci2.py
index 13aa50959e24cafbebeea6aa005903fe421858e0..2adbad7ba45ad7ce4e03bc59ace54ee46803113b 100644
--- a/CoLo-AT/localization_algos/ekf_gs_ci2.py
+++ b/CoLo-AT/localization_algos/ekf_gs_ci2.py
@@ -12,7 +12,7 @@ class EKF_GS_CI2(ekf_algo_framework):
ekf_algo_framework.__init__(self, algo_name)
def state_variance_init(self, num_robots):
- return 0.03*np.matrix(np.identity(2*num_robots), dtype = float)
+ return 0.04*np.matrix(np.identity(2*num_robots), dtype = float)
def calculate_trace_state_variance(self, robot_data):
[s, orinetations, sigma_s, index] = robot_data
@@ -133,7 +133,6 @@ class EKF_GS_CI2(ekf_algo_framework):
kalman_gain = sigma_s*H.getT()*sigma_invention.getI()
s = s + kalman_gain*(z - z_hat)
- print(index, z - z_hat)
sigma_s = sigma_s - kalman_gain*H*sigma_s
return [s, orinetations, sigma_s]
diff --git a/CoLo-AT/localization_algos/ekf_gs_sci2.py b/CoLo-AT/localization_algos/ekf_gs_sci2.py
index 9e979327e000407679dc2d4942e971f8345276ad..e95628a36278dbd6abed15734b8396d233059e55 100644
--- a/CoLo-AT/localization_algos/ekf_gs_sci2.py
+++ b/CoLo-AT/localization_algos/ekf_gs_sci2.py
@@ -11,8 +11,8 @@ class EKF_GS_SCI2(ekf_algo_framework):
self.algo_name = algo_name
def state_variance_init(self, num_robots):
- sigma_d = 0.1*np.matrix(np.identity(2*num_robots), dtype = float)
- sigma_i = 0.1*np.matrix(np.identity(2*num_robots), dtype = float)
+ sigma_d = 0.04*np.matrix(np.identity(2*num_robots), dtype = float)
+ sigma_i = 0.04*np.matrix(np.identity(2*num_robots), dtype = float)
state_variance = [sigma_d, sigma_i]
return state_variance
@@ -22,7 +22,7 @@ class EKF_GS_SCI2(ekf_algo_framework):
[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])/2
- return trace_state_var
+ return np.trace(total_sigma)/2
def propagation_update(self, robot_data, sensor_data):
diff --git a/CoLo-AT/localization_algos/ekf_ls_bda.py b/CoLo-AT/localization_algos/ekf_ls_bda.py
index 83d0ad66738a22d3daad226e6b9cda232777ab96..d8dab87da6f02245177f3cf7f6b0f9665f8a5934 100644
--- a/CoLo-AT/localization_algos/ekf_ls_bda.py
+++ b/CoLo-AT/localization_algos/ekf_ls_bda.py
@@ -20,13 +20,13 @@ class EKF_LS_BDA(ekf_algo_framework):
ekf_algo_framework.__init__(self, algo_name)
def state_variance_init(self, num_robots):
- return 0.01*np.matrix(np.identity(2*num_robots), dtype = float)
+ return 0.04*np.matrix(np.identity(2*num_robots), dtype = float)
def calculate_trace_state_variance(self, robot_data):
[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
diff --git a/CoLo-AT/localization_algos/ekf_ls_ci.py b/CoLo-AT/localization_algos/ekf_ls_ci.py
index cfe89cdc573141203a19b937443a38ab7001992b..5f0738d2ef0dee5d14fb588734e7ead5a12aeabe 100644
--- a/CoLo-AT/localization_algos/ekf_ls_ci.py
+++ b/CoLo-AT/localization_algos/ekf_ls_ci.py
@@ -15,13 +15,13 @@ class EKF_LS_CI(ekf_algo_framework):
ekf_algo_framework.__init__(self, algo_name)
def state_variance_init(self, num_robots):
- return 0.01*np.matrix(np.identity(2*num_robots), dtype = float)
+ return 0.04*np.matrix(np.identity(2*num_robots), dtype = float)
def calculate_trace_state_variance(self, robot_data):
[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, s, orinetations, sigma_s, input_data, sigma_odo, index):
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 73c6d492da67ac9b0409341a80ca3aa9d5e6c4c5..8406071719767482050a4be41f2168eaa7139445 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/sim_manager.py b/CoLo-AT/simulation_process/sim_manager.py
index 3024da48a996aa31b354d49d38d0283674617242..a575a10245c019d7943229f498cc773225c44479 100644
--- a/CoLo-AT/simulation_process/sim_manager.py
+++ b/CoLo-AT/simulation_process/sim_manager.py
@@ -128,7 +128,7 @@ class SimulationManager():
comm_rsp.set_message(message)
if comm_rsp != None:
- print('comm', robot_labels[sender_idx], robot_labels[receiver_idx])
+ #print('comm', robot_labels[sender_idx], robot_labels[receiver_idx])
robot_state = robot_system.localization_update(comm_rsp)
state_var.append(robot_state['state variance'])