diff --git a/CoLo-AT/Algorithm Comparison.pdf b/CoLo-AT/Algorithm Comparison.pdf
index be8592ab63d020456462f6a207fa78bf16221817..19257be51e0530e4914cf84d90a705e80fff7465 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 7c702c1fe6559289fc6ad2a6503301457bd71125..487698deadc7a65695954f9daa06aef9cb606afd 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 d1b233b6d10c97d17fc37a6a3cf43cbac14e2116..a6bc95746f241e5cd91553c38bebf06bbc97be71 100644
--- a/CoLo-AT/data_analysis/data_analyzer.py
+++ b/CoLo-AT/data_analysis/data_analyzer.py
@@ -76,7 +76,7 @@ class Analyzer():
- def calculate_loc_err_and_trace_state_variance_per_run(self, data_recorder, unit_time_interval = 1, plot_graphs = True, selected_labels = None):
+ def calculate_loc_err_and_trace_state_variance_per_run(self, data_recorder, unit_time_interval = 0.5, plot_graphs = True, selected_labels = None):
#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()
@@ -143,10 +143,12 @@ class Analyzer():
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)
+ loc_err_per_run.append(loc_err_per_run[-1])
+ state_err_per_run.append(state_err_per_run[-1])
+ trace_per_run.append(trace_per_run[-1])
lm_measurement_num.append(lm_measurement_count)
relative_measurment_num.append(relative_measurement_count)
@@ -157,8 +159,9 @@ class Analyzer():
interval_start_time = interval_start_time+unit_time_interval
print(data_recorder.name,': ')
- print('Avg location deviation errors per run: ', sum(loc_err_per_run)/len(loc_err_per_run))
- print('Avg trace of state variances per run: ', sum(trace_per_run)/len(trace_per_run))
+ print('Avg location deviation errors per run: ', np.mean(loc_err_per_run))
+ print('Avg state estimation errors per run: ', np.mean(state_err_per_run))
+ print('Avg trace of state variances per run: ', np.mean(trace_per_run))
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)
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 35c5604c197dbd43ba397d71f598ccbad8207c35..f2a3c9d7665263c49923681c4c514744357791fd 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 bd0238ae47b672357527b99485efdb6bca3f99ad..007c7d291ffe9b6f75afca5d5e81ede385ba257d 100755
--- a/CoLo-AT/dataset_manager/simulated_dataset_manager_w.py
+++ b/CoLo-AT/dataset_manager/simulated_dataset_manager_w.py
@@ -211,14 +211,6 @@ class Sim_Dataset_Manager:
def dataset_reset(self):
- self.start_time = 0
- self.starting_states = {}
- self.starting_states[1] = [self.start_time, 0, 5, pi/2]
- self.starting_states[2] = [self.start_time, 5, 0, pi]
- self.starting_states[3] = [self.start_time, 0, -5, 3*pi/2]
- self.starting_states[4] = [self.start_time, 0, 5, 2*pi]
-
- self.num_robots = len(self.robot_lables)
self.measurement_data = [[] for i in range(self.num_robots)]
self.odometry_data = [[] for i in range(self.num_robots)]
self.groundtruth_data = [[] for i in range(self.num_robots)]
diff --git a/CoLo-AT/gs_ci_journal_sim.py b/CoLo-AT/gs_ci_journal_sim.py
index 9d4b5c4d9341091d1b8594dc99f8c602ca94197e..ed3498b87dde952c4f200cec0b82f0691b121ca2 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 = 100
+duration = 150
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)
@@ -65,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, 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)
+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, selected_labels = [1])
##############################################################################
@@ -76,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, 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)
+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, selected_labels = [1])
##############################################################################
@@ -88,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, 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)
+loc_err_per_run, state_err_per_run_ls_ci, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_CI, plot_graphs = False, selected_labels = [1])
##############################################################################
@@ -99,7 +99,7 @@ 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, 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)
+loc_err_per_run, state_err_per_run_sci, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_GS_SCI, plot_graphs = False, 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])
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 c8abb3b50d3e7bbaa105b4b78ae49a4178137b74..a52f11c5e0e8030a79269ec134d0f7da2a092452 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/ekf_gs_sci2.py b/CoLo-AT/localization_algos/ekf_gs_sci2.py
index e95628a36278dbd6abed15734b8396d233059e55..7bfb64e3feb49bb364ac4113204f425ce51a0b67 100644
--- a/CoLo-AT/localization_algos/ekf_gs_sci2.py
+++ b/CoLo-AT/localization_algos/ekf_gs_sci2.py
@@ -30,7 +30,7 @@ class EKF_GS_SCI2(ekf_algo_framework):
[measurement_data, sensor_covariance] = sensor_data
sigma_odo = sensor_covariance
- var_v = 0.1
+ var_v = 0.25
num_robots = int(len(s)/2)
delta_t = measurement_data[0]
@@ -108,7 +108,6 @@ class EKF_GS_SCI2(ekf_algo_framework):
kalman_gain = total_sigma*H.getT()*sigma_invention.getI()
s = s + kalman_gain*(z - z_hat)
-
total_sigma = total_sigma - kalman_gain*H*total_sigma
i_mtx = np.identity(num_robots*2)
sigma_i = (i_mtx - kalman_gain*H) * sigma_i * (i_mtx.copy() - kalman_gain*H).getT() + kalman_gain * sigma_z * kalman_gain.getT()
@@ -197,7 +196,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.8 # (iii+1)*0.01
+ e = 0.83 # (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/robots/__pycache__/robot_system.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc
index 1fd2769d85d4c09a6c7592f51c8ff5792061162a..f6533f73d45e44a77137725ae12f93b05af62170 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/simulation_process/__pycache__/sim_manager.cpython-36.pyc b/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc
index 1450bcf5f5e286f03e7a2161ce1cbf2616342420..898bd634c5771fc458900a539937375e91f436bf 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 fecf62bb365b60850ecd554432651b7f9690698c..732abd46ebad50eb45b3176b5055226175851428 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 a575a10245c019d7943229f498cc773225c44479..08908cd221926b304699983ddc78d8c89198daa0 100644
--- a/CoLo-AT/simulation_process/sim_manager.py
+++ b/CoLo-AT/simulation_process/sim_manager.py
@@ -13,6 +13,7 @@ from random import randint
class SimulationManager():
def __init__(self, name):
self.name = name
+ np.random.seed(12)
def comm_controller(self, rbt_idx, num_robots, rsp):
receiver_idx = rbt_idx