diff --git a/CoLo-AT/Algorithm Comparison.pdf b/CoLo-AT/Algorithm Comparison.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..c2a354708623c45db863e612509ca6068795b2f1
Binary files /dev/null and b/CoLo-AT/Algorithm Comparison.pdf differ
diff --git a/CoLo-AT/UTIAS_d1.pdf b/CoLo-AT/UTIAS_d1.pdf
index 6f42f42b0d35481ec85644334f8f340bd875b1d9..dc31ab18dfd28dca78b61bde9585002a34a01abf 100644
Binary files a/CoLo-AT/UTIAS_d1.pdf and b/CoLo-AT/UTIAS_d1.pdf differ
diff --git a/CoLo-AT/__pycache__/Cooperative_Localization_Journal1.cpython-36.pyc b/CoLo-AT/__pycache__/Cooperative_Localization_Journal1.cpython-36.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..f6917882d67cca10eaf21cefccd99f71c58f029a
Binary files /dev/null and b/CoLo-AT/__pycache__/Cooperative_Localization_Journal1.cpython-36.pyc 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 a389a7e49c27d97a6b79803c7c08a5cd8d519b3c..6ecd8a96d9e87cca6ae9fdbe0b0a55b9757dd3b1 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/j1_rw.py b/CoLo-AT/j1_rw.py
index f3d715c6bd5b323435d421d46b73632375a359a7..220bc51f0ddc1042d6e0ac5b90248161fdc0acfd 100644
--- a/CoLo-AT/j1_rw.py
+++ b/CoLo-AT/j1_rw.py
@@ -17,11 +17,13 @@ for i in [1, 2, 3, 4]:
'''
# 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 = "/Users/shengkangchen/Documents/CoLo/CoLo-D/UTIAS-dataset/MRCLAM_Dataset"
+datasets_path = "/home/william/CoLo/CoLo-D/UTIAS-dataset/MRCLAM_Dataset" # for desktop Ubuntu
+
robot_labels = [1,2,3,4,5]
-duration = 600 # duration for the simulation in sec
+duration = 100 # duration for the simulation in sec
-for i in range(1, 9):
+for i in range(1, 2):
dataset_label = str(i)
dataset_path = datasets_path + dataset_label + "/"
graph_name = 'UTIAS_d'+dataset_label
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 4f66e05ad5c60b1729d13e9a81931ba09d7542fb..a64128bd845eeebcb7647366896682e70e402de9 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 fa34210e7c17e4c05dad6e27232debbdd82dd412..9e979327e000407679dc2d4942e971f8345276ad 100644
--- a/CoLo-AT/localization_algos/ekf_gs_sci2.py
+++ b/CoLo-AT/localization_algos/ekf_gs_sci2.py
@@ -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 np.trace(total_sigma)
+ return trace_state_var
def propagation_update(self, robot_data, sensor_data):
@@ -154,24 +154,38 @@ class EKF_GS_SCI2(ekf_algo_framework):
sigma_z = rot_mtx(phi)*sigma_ob*rot_mtx(phi).getT()
[sigma_d, sigma_i] = sigma_s
+ total_sigma = sigma_i + sigma_d
e = 0.83
+ p_1 = 1/e * sigma_d[i:i+2, i:i+2] + sigma_i[i:i+2, i:i+2]
+ p_2 = 1/(1-e) *sigma_d[j:j+2, j:j+2] + sigma_i[j:j+2, j:j+2]
+ kalman_gain = p_1*(p_1+p_2).getI()
+
+ s[i:i+2] = kalman_gain*(s[j:j+2]-s[i:i+2])
+ i_mtx = np.identity(num_robots*2)
+ total_sigma[i:i+2, i:i+2] = (-kalman_gain) * p_1
+ sigma_i[i:i+2, i:i+2] = (np.identity(2) - kalman_gain) * sigma_i[i:i+2, i:i+2] * (np.identity(2) - kalman_gain).getT() + kalman_gain * sigma_i[j:j+2, j:j+2] * kalman_gain.getT()
+ sigma_d[i:i+2, i:i+2] = total_sigma[i:i+2, i:i+2] - sigma_i[i:i+2, i:i+2]
+
+ '''
p_1 = (1/e) * sigma_d + sigma_i
-
- #
p_2 = (1/(1-e)) * H * (sigma_i + sigma_d) * H.getT() + sigma_z
+
sigma_invention = H * p_1 * H.getT() + p_2
kalman_gain = p_1 *H.getT()*sigma_invention.getI()
- #
-
+
s = s + kalman_gain*(z - z_hat)
i_mtx = np.identity(num_robots*2)
+
total_sigma = (i_mtx.copy()-kalman_gain*H) * p_1
sigma_i = (i_mtx - kalman_gain*H) * sigma_i * (i_mtx.copy() - kalman_gain*H).getT() + kalman_gain * sigma_z * kalman_gain.getT()
sigma_d = total_sigma - sigma_i
-
+ '''
+
+
+
sigma_s = [sigma_d, sigma_i]
return [s, orinetations, sigma_s]
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 9ac93229e406d60d4e8032c2aeb523b077c79cdb..38a616d58716ef5379fc7ebcaac974579cb9ff5b 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 b2e41bb739ac4014d2d5469ce928dfdf2596a391..2585faca9cf6926677e2db77c4b5894887f9ad0f 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 944b20f11f02d79650742678fefbdcd3a4b781c8..a7a2a49bac9907b4773ca5986b37d5e93e710633 100644
--- a/CoLo-AT/simulation_process/sim_manager.py
+++ b/CoLo-AT/simulation_process/sim_manager.py
@@ -46,7 +46,7 @@ class SimulationManager():
if rsp.get_type()=='measurement':
rbt_idx = rsp.get_robot_index()
rsp_dasa = rsp.get_data()
- if rsp_dasa['subject_ID'] > 5 and self.robot_labels[rbt_idx] in self.robots_cant_observe_lm:
+ if rsp_dasa['subject_ID'] > 5 and self.robots_cant_observe_lm is not None and self.robot_labels[rbt_idx] in self.robots_cant_observe_lm:
return False
return True
diff --git a/CoLo-AT/simulation_process/state_recorder.py b/CoLo-AT/simulation_process/state_recorder.py
index 9e43672b911cc7a5f40dc651f219a61d5dde0db7..2e1d66a94b0627afddbc705f2058ce59c961d4cf 100644
--- a/CoLo-AT/simulation_process/state_recorder.py
+++ b/CoLo-AT/simulation_process/state_recorder.py
@@ -72,11 +72,13 @@ class StatesRecorder():
loc_err = sqrt((est_x_pos-gt_x_pos)*(est_x_pos-gt_x_pos)+(est_y_pos-gt_y_pos)*(est_y_pos-gt_y_pos))
recorded_dataline = [time, robot_label, est_x_pos, est_y_pos, trace_state_var, gt_x_pos, gt_y_pos, loc_err]
+
+ '''
if(trace_state_var<0):
print('TIME: ', time+self.start_time)
print(updata_type)
print('neg trace: ', recorded_dataline)
-
+ '''
'''
if(loc_err >= 1):
print(updata_type)
diff --git a/CoLo-D/UTIAS-Datasets b/CoLo-D/UTIAS-Datasets
new file mode 160000
index 0000000000000000000000000000000000000000..bf30b31be62c28aaf24b533d3dbbbe4c8dd680ef
--- /dev/null
+++ b/CoLo-D/UTIAS-Datasets
@@ -0,0 +1 @@
+Subproject commit bf30b31be62c28aaf24b533d3dbbbe4c8dd680ef