diff --git a/CoLo-AT/dataset_manager/__pycache__/existing_dataset.cpython-35.pyc b/CoLo-AT/dataset_manager/__pycache__/existing_dataset.cpython-35.pyc
index b8f4113524e113b5bc74d0dcc2360c8aac644e3c..355f71f66a3183e2b8a402317cc436c3ad78597f 100644
Binary files a/CoLo-AT/dataset_manager/__pycache__/existing_dataset.cpython-35.pyc and b/CoLo-AT/dataset_manager/__pycache__/existing_dataset.cpython-35.pyc differ
diff --git a/CoLo-AT/full_test9.gif b/CoLo-AT/full_test9.gif
new file mode 100644
index 0000000000000000000000000000000000000000..31759e06f486ca6ddc1867ff549f8b0d90842bcd
Binary files /dev/null and b/CoLo-AT/full_test9.gif differ
diff --git a/CoLo-AT/robots/__pycache__/robot_system.cpython-35.pyc b/CoLo-AT/robots/__pycache__/robot_system.cpython-35.pyc
index 4245e57f813a10c01fb0bbf3c3b16c617c77020c..25d23386e6336389cfb87686ae89bd28cf844cda 100644
Binary files a/CoLo-AT/robots/__pycache__/robot_system.cpython-35.pyc and b/CoLo-AT/robots/__pycache__/robot_system.cpython-35.pyc differ
diff --git a/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc
index 7aadf383202797e3ccda9258a470de1557b1c484..51373f8b840f6867913e1a6dffbd7dfe00f91edd 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/robots/robot_system.py b/CoLo-AT/robots/robot_system.py
index 14d20e3f029026353b72040a69145796622dcc26..079d1ad01092564199180c18042ce245a292232c 100644
--- a/CoLo-AT/robots/robot_system.py
+++ b/CoLo-AT/robots/robot_system.py
@@ -76,8 +76,9 @@ class RobotSystem:
update_type = 'propagation'
v = message_data['velocity']
a_v = message_data ['angular velocity']
+
#sigma_odo = np.matrix([[5.075*v, 0], [0, 0.1]]) #with respect to velocity and orientation for utias datasets
- sigma_odo = np.matrix([[0.01, 0], [0, 0.5]]) #with respect to velocity and orientation
+ sigma_odo = np.matrix([[0.01, 0], [0, 0.1]]) #with respect to velocity and orientation
sensor_covariance = sigma_odo
delta_t = message_data['delta_t']
diff --git a/CoLo-AT/ros_compatibility.py b/CoLo-AT/ros_compatibility.py
index d32cc350e3750653c42fc4ec4433c88fc0834873..fd4b12559eb970b7ed7e7665793c5871ddd8f9b7 100644
--- a/CoLo-AT/ros_compatibility.py
+++ b/CoLo-AT/ros_compatibility.py
@@ -16,33 +16,24 @@ 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
-from pprint import pprint
# load algorithms
sys.path.append(os.path.join(os.path.dirname(__file__), "localization_algos"))
from centralized_ekf import Centralized_EKF
-from simple_ekf import Simple_EKF
-from ekf_ls_bda import EKF_LS_BDA
-from ekf_gs_bound import EKF_GS_BOUND
-from ekf_gs_ci import EKF_GS_CI
-from ekf_gs_sci2 import EKF_GS_SCI2
-
-#dataset_path = '/home/william/catkin_ws/ros_colo_dataset/'
-#dataset_path = '/home/william/UTIAS-dataset/MRCLAM_Dataset3/'
-#dataset_path = '/home/william/full_tests/full_test9/'
+
compname = getpass.getuser()
dataset_path = "/home/"+ compname +"/full_tests/full_test_v3_6/"
dataset_labels = [1,2,3]
-duration = 300 # duration for the simulation in sec
+duration = 310 # duration for the simulation in sec
testing_dataset = Dataset('testing')
start_time, starting_states, dataset_data, time_arr = testing_dataset.load_MRCLAMDatasets(dataset_path, dataset_labels, duration, delay_start = 0)
-loc_algo = EKF_GS_SCI2('algo')
-robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = True)
+loc_algo = Centralized_EKF('algo')
+robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = False)
-sim = SimulationManager('PE-CoLo EKF_GS_SCI2')
+sim = SimulationManager('CoLo: Centralized_EKF')
state_recorder = StatesRecorder('recorder',dataset_labels)
-sim.sim_process_naive(dataset_labels, testing_dataset, robot, state_recorder, simple_plot = True)
+sim.sim_process_native(dataset_labels, testing_dataset, robot, state_recorder, simple_plot = True)
analyzer = Analyzer('analyzer', dataset_labels)
@@ -52,4 +43,4 @@ data_in_time_order = state_recorder.get_data_in_time_order()
update_in_time_order = state_recorder.get_updata_type_in_time_order()
update_type_arr = state_recorder.get_update_type_arr()
recorded_data = state_recorder.get_recorded_data()
-#recorded_dataline = [time, robot_label, est_x_pos, est_y_pos, trace_state_var, gt_x_pos, gt_y_pos, loc_err]
+#Format: recorded_dataline = [time, robot_label, est_x_pos, est_y_pos, trace_state_var, gt_x_pos, gt_y_pos, loc_err]
diff --git a/CoLo-AT/ros_demo.py b/CoLo-AT/ros_demo.py
index 7f435357460cff6645300c2e66a81008fc0284a0..19155870bbdb0ddc86dbce2c2214b29f4089dcac 100644
--- a/CoLo-AT/ros_demo.py
+++ b/CoLo-AT/ros_demo.py
@@ -20,26 +20,23 @@ from pprint import pprint
# load algorithms
sys.path.append(os.path.join(os.path.dirname(__file__), "localization_algos"))
from centralized_ekf import Centralized_EKF # works
-from simple_ekf import Simple_EKF
-from ekf_ls_bda import EKF_LS_BDA
-from ekf_gs_bound import EKF_GS_BOUND
-from ekf_gs_ci2 import EKF_GS_CI2
+
#dataset_path = '/home/william/catkin_ws/ros_colo_dataset/'
#dataset_path = '/home/william/UTIAS-dataset/MRCLAM_Dataset3/'
compname = getpass.getuser()
-dataset_path = "/home/"+ compname +"/full_tests/full_test_v3_2/"
+dataset_path = "/home/"+ compname +"/full_tests/full_test_v3_6/"
-dataset_labels = [1, 2]
+dataset_labels = [1, 2,3]
duration = 240 # duration for the simulation in sec
testing_dataset = Dataset('testing')
start_time, starting_states, dataset_data, time_arr = testing_dataset.load_MRCLAMDatasets(dataset_path, dataset_labels, duration, delay_start = 0)
-loc_algo = Simple_EKF('algo')
-robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = True)
+loc_algo = Centralized_EKF('algo')
+robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = False)
-sim = SimulationManager('Simple_EKF')
+sim = SimulationManager('Centralized_EKF')
state_recorder = StatesRecorder('recorder',dataset_labels)
-sim.sim_process_naive(dataset_labels, testing_dataset, robot, state_recorder, simple_plot = False)
+sim.sim_process_native(dataset_labels, testing_dataset, robot, state_recorder, simple_plot = False)
analyzer = Analyzer('analyzer', dataset_labels)
diff --git a/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-35.pyc b/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-35.pyc
index b79a7a4b5bb36b2e46ded786be1a2c770c2d019d..d388f62ec168a34b8884ffdd9db950c6475c06b3 100644
Binary files a/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-35.pyc and b/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-35.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 1f52de8ccdebd6de988d3d8bcff8fa717cd86d3d..b567d4d86a5ddcbd5ab83757cc5b2d448d6a3b98 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-35.pyc b/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-35.pyc
index 6866b0b74fece4e734a2117774892407b77ea342..3b7e1eaa2350cb227f65d1287e931c9781c61a44 100644
Binary files a/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-35.pyc and b/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-35.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 2afa61a8e0fddd99c65d8befd44562410b9cfcd7..79b36539455ad2d99eb5288bc0f185b944a12294 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 d6bd79daa035e79255ab2ae8becbc770f9f1268d..d2f6e8d87d328d05dae1373b357096e051994eee 100644
--- a/CoLo-AT/simulation_process/sim_manager.py
+++ b/CoLo-AT/simulation_process/sim_manager.py
@@ -14,7 +14,7 @@ class SimulationManager():
def __init__(self, name):
self.name = name
- def sim_process_naive(self, dataset_labels, dm, robot_system, state_recorder, comm = True, simple_plot = False):
+ def sim_process_native(self, dataset_labels, dm, robot_system, state_recorder, comm = True, simple_plot = False):
#dm: dataset manager
print('******** Simulation Process Started! ********')
print('communication: ', comm)
diff --git a/CoLo-AT/simulation_process/state_recorder.py b/CoLo-AT/simulation_process/state_recorder.py
index b9ae1f5c6e3540019e45e4bb772921d9af1fbe84..245117c2ddb56596e4bd7f17684682cc5c3cdcb6 100644
--- a/CoLo-AT/simulation_process/state_recorder.py
+++ b/CoLo-AT/simulation_process/state_recorder.py
@@ -116,6 +116,7 @@ class StatesRecorder():
fig1.plot(time_arr, loc_err_arr[label], label= 'Robot %d' %label)
fig2.plot(time_arr, trace_sigma_s_arr[label], label= 'Robot %d' %label)
print('Robot',label, 'loc err: ', sum(loc_err_arr[label])/len(loc_err_arr[label]))
+ print('Robot',label, 'trace Sigma_s: ', sum(trace_sigma_s_arr[label])/len(trace_sigma_s_arr[label]))
fig1.set_title('Estimation deviation error')
fig1.set_xlabel('Time[s]')
fig1.set_ylabel('RMS[m]')
diff --git a/CoLo-PE/get_sensor_variances.py b/CoLo-PE/get_sensor_variances.py
index 620e9bcf2047e07039198b25c9e1559af240a7d1..8c701dc35e30f0845d40030c75d26e7eaa20f346 100644
--- a/CoLo-PE/get_sensor_variances.py
+++ b/CoLo-PE/get_sensor_variances.py
@@ -254,7 +254,7 @@ class DatasetAnalyzer:
compname = getpass.getuser()
-dataset_path = "/home/"+ compname +"/full_tests/full_test_v3_2/"
+dataset_path = "/home/"+ compname +"/full_tests/full_test_v3_6/"
dataset_labels = [1,2,3]
duration = 200 # duration for the simulation in sec
dataset = DatasetAnalyzer('DatasetAnalyzer')