diff --git a/CoLo-AT/Cooperative_Localization_Journal1.py b/CoLo-AT/Cooperative_Localization_Journal1.py
index 78aa75a08f28d845133f84096c78aaad23a5ecf0..59f954dff36053322ed47bb2aa8f50996a1e470d 100644
--- a/CoLo-AT/Cooperative_Localization_Journal1.py
+++ b/CoLo-AT/Cooperative_Localization_Journal1.py
@@ -41,6 +41,7 @@ def all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots
testing_dataset = RW_Dataset_Manager('testing')
start_time, starting_states, dataset_data, time_arr = testing_dataset.load_datasets(dataset_path, robot_labels, duration)
+ testing_dataset.dataset_analysis(duration)
analyzer = Analyzer('analyzer', robot_labels)
loc_algo = EKF_GS_CI2('algo')
@@ -51,6 +52,7 @@ def all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots
sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = False , robots_cant_observe_lm = robots_cant_observe_lm)
analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = True)
analyzer.trajectory_plot(state_recorder)
+
##############################################################################
'''
testing_dataset.dataset_reset()
diff --git a/CoLo-AT/__pycache__/Cooperative_Localization_Journal1.cpython-36.pyc b/CoLo-AT/__pycache__/Cooperative_Localization_Journal1.cpython-36.pyc
index 994caf1e5942bfd5a5db55f4e442c674b2c52baf..f61628b9573c970d4e070e8d112414736c57bca8 100644
Binary files a/CoLo-AT/__pycache__/Cooperative_Localization_Journal1.cpython-36.pyc and b/CoLo-AT/__pycache__/Cooperative_Localization_Journal1.cpython-36.pyc differ
diff --git a/CoLo-AT/colo_d3.pdf b/CoLo-AT/colo_d3.pdf
index 1180d28aba82324f8033794146bd84a7237a041e..7930a0632892d5b1bebaf4ca89ac69b8548b81ee 100644
Binary files a/CoLo-AT/colo_d3.pdf and b/CoLo-AT/colo_d3.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 487698deadc7a65695954f9daa06aef9cb606afd..18812b6a3f8fed8faf5b81d65bea7d299bf7cf94 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 a6bc95746f241e5cd91553c38bebf06bbc97be71..2614c75e193fdd66dd858563a9a60381fb00b967 100644
--- a/CoLo-AT/data_analysis/data_analyzer.py
+++ b/CoLo-AT/data_analysis/data_analyzer.py
@@ -139,9 +139,9 @@ class Analyzer():
break
if num_dataline_per_time_iterval != 0:
- loc_err_per_run.append(loc_err_per_time_iterval/num_dataline_per_time_iterval)
+ 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)
+ trace_per_run.append(trace_per_time_iterval/num_dataline_per_time_iterval)
@@ -275,7 +275,7 @@ class Analyzer():
for data_recorder in arr_data_recorder:
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, state_err_per_run, data_recorder.name])
+ 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')
diff --git a/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc b/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc
index 7703bdb3d0ca0acc5db1c1d0a506c1a597b0d88b..4c95089bb8c6d74eadc4b62445f43ba21e1d038c 100644
Binary files a/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc and b/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc differ
diff --git a/CoLo-AT/dataset_manager/realworld_dataset_manager.py b/CoLo-AT/dataset_manager/realworld_dataset_manager.py
index e8c7c3c3ca5569506f0f993f0a3b0dacccbe4865..18e0035555991a9bfd7c28c32fe0463d0d298440 100755
--- a/CoLo-AT/dataset_manager/realworld_dataset_manager.py
+++ b/CoLo-AT/dataset_manager/realworld_dataset_manager.py
@@ -85,6 +85,7 @@ class RW_Dataset_Manager:
self.gt_data_odo = [[] for i in range(self.num_robots)]
self.gt_data_meas = [[] for i in range(self.num_robots)]
self.groundtruth_data = [[] for i in range(self.num_robots)]
+ self.velocity_arr = [[] for i in range(self.num_robots)]
#self.groundtruth_time= [[] for i in range(self.num_robots)]
self.time_arr = {'odometry': [[] for i in range(self.num_robots)], 'measurement': [[] for i in range(self.num_robots)], 'groundtruth': [[] for i in range(self.num_robots)]}
@@ -189,6 +190,7 @@ class RW_Dataset_Manager:
self.odometry_data[i].append(odo_info)
self.gt_data_odo[i].append(matched_gt_info)
self.time_arr['odometry'][i].append(time)
+ self.velocity_arr[i].append(velocity)
self.data_trackers = {'odometry': np.ones((self.num_robots,), dtype=np.int), 'measurement':np.ones((self.num_robots,), dtype=np.int)}
# tracker for each robot for both each type of data to keep track of their location in the dataset
@@ -228,7 +230,7 @@ class RW_Dataset_Manager:
t+=sampling_period
- return 1
+ return True
def dataset_reset(self):
self.data_trackers = {'odometry': np.ones((self.num_robots,), dtype=np.int),'measurement':np.ones((self.num_robots,), dtype=np.int), 'groundtruth':np.ones((self.num_robots,), dtype=np.int)}
@@ -491,5 +493,8 @@ class RW_Dataset_Manager:
return gt
+
+
+
diff --git a/CoLo-AT/j1_rw.py b/CoLo-AT/j1_rw.py
index 5ddda45e05241bb70f7882f6e1b9001f6b1baa74..cec257a02faea062ce083754443cad83d84f67b7 100644
--- a/CoLo-AT/j1_rw.py
+++ b/CoLo-AT/j1_rw.py
@@ -1,21 +1,23 @@
from Cooperative_Localization_Journal1 import all_algorithms_comp
-'''
+
# for all colo datasets, may need to change scale on comparison graph: loc[0,0.5], trace[0,0.05]
-datasets_path = "/Users/shengkangchen/Documents/CoLo/CoLo-D/CoLo-Datasets/official_dataset"
+datasets_path = "/home/william/CoLo/CoLo-D/CoLo-Datasets/official_dataset"
robot_labels = [1,2,3]
duration = 180 # duration for the simulation in sec
-for i in [1, 2, 3, 4]:
+for i in [3]:
dataset_label = str(i)
dataset_path = datasets_path + dataset_label + "/"
graph_name = 'colo_d'+dataset_label
print(dataset_path)
all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots_cant_observe_lm = [2])
-'''
+
+
+'''
# for all UTIAS datasets, may need to change scale on comparison graph: loc[0,1.5], trace[0,0.2]
#datasets_path = "/Users/shengkangchen/Documents/CoLo/CoLo-D/UTIAS-dataset/MRCLAM_Dataset"
datasets_path = "/home/william/CoLo/CoLo-D/UTIAS-dataset/MRCLAM_Dataset" # for desktop Ubuntu
@@ -28,4 +30,5 @@ dataset_label = str(i)
dataset_path = datasets_path + dataset_label + "/"
graph_name = 'UTIAS_d'+dataset_label
print(dataset_path)
-all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots_cant_observe_lm = [4, 5])
\ No newline at end of file
+all_algorithms_comp(dataset_path, robot_labels, duration, graph_name, robots_cant_observe_lm = [4, 5])
+'''
\ 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 a02f049cf7ac54d49143049b5fe2c2769f6fa360..fa4ee702c06f4060b139d73d1c14ff75b71df710 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__/gs_ci_bound.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-36.pyc
index 607a82df754796bfe76937e000767780fb5e9098..dd2f23e76b39c9d3ddfc8c0ce01ec0202a61d009 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_ci2.py b/CoLo-AT/localization_algos/ekf_gs_ci2.py
index fd399d232e4f791a519343d5c95e3e4bdaaef87e..d31cbae7a7d4813733901cdf8adf03bc8daf7e49 100644
--- a/CoLo-AT/localization_algos/ekf_gs_ci2.py
+++ b/CoLo-AT/localization_algos/ekf_gs_ci2.py
@@ -25,7 +25,7 @@ class EKF_GS_CI2(ekf_algo_framework):
[measurement_data, sensor_covariance] = sensor_data
sigma_odo = sensor_covariance
- var_v = 0.25# variance of the velocity
+ var_v = 0.1**2# variance of the velocity
exp_v = 0 # expected vecolcity for other robots
i = 2*index
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 cd22333eeb9494a4a8108613451740afd219fd98..df0b7434989e93a8cd849fc74f56fe079bedb67f 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 9f2fabc242f24ac4994e0ee86ecf4056f9edb8cb..984d28764899a63cdf82da320e92be3641f68bdf 100644
--- a/CoLo-AT/simulation_process/sim_manager.py
+++ b/CoLo-AT/simulation_process/sim_manager.py
@@ -19,18 +19,20 @@ class SimulationManager():
receiver_idx = rbt_idx
#sender_idx = (rbt_idx+1)%(num_robots)
sender_idx = None
- '''
+
+ #if i observe j, j will send data to i
if rsp.get_type()=='measurement' and rsp.get_data()['subject_ID'] <= 5:
sender_id = rsp.get_data()['subject_ID']
sender_idx = self.robot_labels.index(sender_id)
- '''
+
+ '''
if rbt_idx == 0:
sender_idx = 1
if rbt_idx == 1:
sender_idx = 2
-
+ '''
return receiver_idx, sender_idx
def simulated_communication(self, receiver_status, sender_status):
@@ -46,14 +48,13 @@ class SimulationManager():
mean = math.exp(-1.5*dis)
std = math.exp(-0.5*dis)
- '''
+
if numpy.random.normal(mean, std) > threshold:
return True
else:
return False
- '''
-
- return True
+
+ #return True
def allow_operation(self, rsp):
'''
if rsp.get_type()=='measurement':
@@ -89,7 +90,7 @@ class SimulationManager():
state_var = []
while self.time < start_time+duration:
- #print("remaining time:", start_time+duration-self.time)
+ # print("remaining time:", start_time+duration-self.time)
g_req= request_response.Request_response(None, None)
prev_time = self.time
valid_op, self.time , rsp = dm.respond(g_req, self.time)