diff --git a/CoLo-AT/README.md b/CoLo-AT/README.md
index b2cb09ac33f70880c1d25a700cb5924eaf1d17b8..5bb025643841c0e35050955920708ed54d5ff2eb 100755
--- a/CoLo-AT/README.md
+++ b/CoLo-AT/README.md
@@ -1,11 +1,10 @@
-# LEMUR-CoLo-Sim
-
-
+# LEMUR CoLo-AT
## Synopsis
Cooperative localization is still a challenging task for cooperative robot control. CoLo is a robotic simulation environment for cooperative localization or cooperative simultaneous localization and mapping (SLAM) using real world dataset[1] or simulated data. The goal of this project to let users to create and test their own algorithms with some existing algorithms. Compared with other popular robotic simulation environments[2], it has fewer dependencies and more convenient to add localization algorithms without worry much about the robot settings. Moreover, it is able to use both real world data and simulated data to test the robustness and liabilities of these algorithms, whereas other simulation environments use only simulated data.
https://drive.google.com/file/d/1NlUM2QXT_KfZkVOJu3Xsm8_QL8OSy6hz/view?usp=sharing
+
## Features
Real world dataset
@@ -52,8 +51,15 @@ python Localization_envir.py
-## Dataset:
-UTIAS dataset:
+## Compatible Datasets:
+CoLo Datasets: (in CoLo-D folder)
+- official_dataset1
+- official_dataset2
+- official_dataset3
+- official_dataset4
+
+
+UTIAS dataset (created by the Autonomous Space Robotics Lab (ASRL) at the University of Toronto):
- MRCLAM_Dataset1
- MRCLAM_Dataset2
- MRCLAM_Dataset3
@@ -64,11 +70,9 @@ UTIAS dataset:
- MRCLAM_Dataset8
- MRCLAM_Dataset9
details info: [UTIAS Dataset](http://asrl.utias.utoronto.ca/datasets/mrclam/).
-
-
+Note: to use UTIAS dataset, users need to create measurment_x files from measurement files using barcode_2_id.py
## Authors
-
Shengkang Chen
Cade Mallett
Kyle Wong
@@ -77,7 +81,6 @@ Kyle Wong
## Analytical Tool:
-
Plots:
1. Estimation deviation error vs. time
2. Trace of state variance vs. time
diff --git a/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-35.pyc b/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-35.pyc
index ce6de0632fa7a1a0d7e09c19fa612bd312bd0984..957a8ef427c43e0f9c0e22385fa6c384afc58433 100644
Binary files a/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-35.pyc and b/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-35.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 c1276daac2b6a95b0506869361272989136596e3..c17ec2640ce6bff4e56915b4fe2430c236bdde2a 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/__pycache__/realtime_plot.cpython-35.pyc b/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-35.pyc
index 60ef9783aab84de2bb519a65425e593e61c2875b..e02adf784a5865eee4553c1a29f93871a9fe3835 100644
Binary files a/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-35.pyc and b/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-35.pyc differ
diff --git a/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-36.pyc b/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-36.pyc
index d60b97bfd306ab8c036651f323b5266f1c646b1d..edb0d1c6d571e3816fea6931c4a2caf88f608c60 100644
Binary files a/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-36.pyc and b/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-36.pyc differ
diff --git a/CoLo-AT/data_analysis/data_analyzer.py b/CoLo-AT/data_analysis/data_analyzer.py
index a41061c0ab30325a0c99ab3f99cdfe4b831a25ca..96f3abab42f0dac2346158d27daa76d8c73070c1 100644
--- a/CoLo-AT/data_analysis/data_analyzer.py
+++ b/CoLo-AT/data_analysis/data_analyzer.py
@@ -43,19 +43,21 @@ class Analyzer():
#fig1.set_ylim(0, 6)
fig1.legend(loc='center left', bbox_to_anchor=(1, 0.5))
- fig2.set_title('Trace of state variance')
+ fig2.set_title('Trace of state covariance')
fig2.set_xlabel('Time [s]')
fig2.set_ylabel('Sigma_s [m^2]')
#fig2.set_ylim(0, 0.08)
fig2.legend(loc='center left', bbox_to_anchor=(1, 0.5))
- fig3.set_title('Observation counts')
- fig3.set_xlabel('Time[s]')
- fig3.set_ylabel('Num of obser')
+ fig3.set_title('Observation counts', fontsize=20)
+ fig3.set_xlabel('Time[s]', fontsize=16)
+ fig3.set_ylabel('Num of obser', fontsize=16)
#fig1.set_ylim(0, 6)
fig3.legend(loc='center left', bbox_to_anchor=(1, 0.5))
-
+ fig3.tick_params(labelsize=14)
+
+
fig.subplots_adjust(hspace = 1.2)
plt.show()
@@ -197,17 +199,20 @@ class Analyzer():
for trace in arr_trace:
fig2.plot(trace[0], trace[1], label = trace[2])
- fig1.set_title('Estimation deviation error')
- fig1.set_xlabel('Time[s]')
- fig1.set_ylabel('RMS[m]')
+ fig1.set_title('Location error', fontsize = 24)
+ fig1.set_xlabel('Time[s]', fontsize = 22)
+ fig1.set_ylabel('RMS[m]', fontsize = 22)
#fig1.set_ylim(0, 6)
- fig1.legend(loc='center left', bbox_to_anchor=(1, 0.5))
+ fig1.legend(loc=1, fontsize = 20)
+ fig1.tick_params(labelsize=18)
- fig2.set_title('Trace of state variance')
- fig2.set_xlabel('Time [s]')
- fig2.set_ylabel('Sigma_s [m^2]')
+ fig2.set_title('Trace of state covariance', fontsize = 24)
+ fig2.set_xlabel('Time [s]', fontsize = 22)
+ fig2.set_ylabel('Sigma_s [m^2]', fontsize = 22)
#fig2.set_ylim(0, 0.08)
- fig2.legend(loc='center left', bbox_to_anchor=(1, 0.5))
+ fig2.legend(loc=1, fontsize = 20)
+ fig2.tick_params(labelsize=18)
+
plt.subplots_adjust(hspace=.6)
plt.show()
diff --git a/CoLo-AT/data_analysis/data_analyzer.pyc b/CoLo-AT/data_analysis/data_analyzer.pyc
index 44b59bceea581694a219af7ee3b8c4aa2ee15c96..62723c50380466ecc81e42b9f09d4ea9427355aa 100644
Binary files a/CoLo-AT/data_analysis/data_analyzer.pyc and b/CoLo-AT/data_analysis/data_analyzer.pyc differ
diff --git a/CoLo-AT/data_analysis/realtime_plot.py b/CoLo-AT/data_analysis/realtime_plot.py
index 4ae8f2cd8a662c8a409dd8c3a8b732ee9ebc80b2..f8dd0c70f8725b0fb82a13f71e030718bf91df81 100644
--- a/CoLo-AT/data_analysis/realtime_plot.py
+++ b/CoLo-AT/data_analysis/realtime_plot.py
@@ -95,7 +95,7 @@ def update(i, num_robots, fig, l_est, l_gt, scat_est, scat_gt, location_est, loc
res = []
for txt in fig.texts:
txt.set_visible(False)
- tmp = fig.text(0.8, 0.03, "Time: " + str(round(times[0][i], 1)), fontsize=16)
+ tmp = fig.text(0.2, 0.2, "Time: " + str(round(times[0][i], 1)), fontsize=16)
for robotNum in range(0, num_robots):
# factor out common variables
est_x = location_est[robotNum]['x']
@@ -154,6 +154,8 @@ def animate_plot(dataset_labels, data_recorder, analyzer, lm = None):
# determine number of robots under consideration for this animation
num_robots = int(len(dataset_labels))
# initialize graphs: error, demo, state variance
+
+ '''
fig = plt.figure(figsize=(12, 9), tight_layout=True)
fig.subplots_adjust(top=0.88)
gs = gridspec.GridSpec(3, 2)
@@ -161,24 +163,34 @@ def animate_plot(dataset_labels, data_recorder, analyzer, lm = None):
ax_err = plt.subplot(gs[0, 0])
ax_var = plt.subplot(gs[0, 1])
ax = plt.subplot(gs[1:, :])
- #ax.set_xlim([-2, 2])
- #ax.set_ylim([-2, 2])
+ '''
+ fig = plt.figure()
+ gs = gridspec.GridSpec(3, 2)
+ ax_err = plt.subplot(gs[0, 0])
+ ax_var = plt.subplot(gs[0, 1])
+ ax = plt.subplot(gs[1:, :])
+
+ ax.set_xlim([-2, 2])
+ ax.set_ylim([-2, 2])
# configure graph
# set bounds
#fig.set_size_inches((18,18))
#ax = plt.axes(xlim=(-6, 6), ylim=(-6, 8))
#ax_err = plt.axes(xlim=(0, 100), ylim=(0, 0.4))
# assign labels
- ax.axis('equal')
+ plt.axis('equal')
ax.set_title('CoLo Demo', fontsize=16)
ax.set_xlabel('x-axis[m]', fontsize=16)
ax.set_ylabel('y-axis[m]', fontsize=16)
- ax_err.set_title('Estimation deviation error', fontsize=16)
+ ax_err.set_title('Location error', fontsize=16)
ax_err.set_xlabel('Time[s]', fontsize=16)
ax_err.set_ylabel('RMS[m]', fontsize=16)
ax_var.set_title('Trace of state variance', fontsize=16)
ax_var.set_xlabel('Time [s]', fontsize=16)
ax_var.set_ylabel('Sigma_s [m^2]', fontsize=16)
+
+
+
# obtain robot locations from dataset
robot_locations_est = get_robot_locations(num_robots, robot_loc_time_unit, 'est')
robot_locations_gt = get_robot_locations(num_robots, robot_loc_time_unit, 'gt')
@@ -199,7 +211,7 @@ def animate_plot(dataset_labels, data_recorder, analyzer, lm = None):
for landmark_id, [x, y] in lm.items():
lm_x.append(x)
lm_y.append(y)
- ax.scatter(lm_x, lm_y, s=120, marker=(5,0), label='Landmark')
+ ax.scatter(lm_x, lm_y, s=120, marker= 6, label='Landmark')
# obtain robot timestamps
times = get_robot_times(num_robots, robot_loc_time_unit)
@@ -210,11 +222,10 @@ def animate_plot(dataset_labels, data_recorder, analyzer, lm = None):
# Show legend
fontP = FontProperties()
fontP.set_size('x-large')
- ax.tick_params(axis='both', which='major', size=15)
-
- ax.tick_params(labelsize=10)
-
+ #ax.legend(prop=fontP, bbox_to_anchor=(1.0, 0.8), loc=9, ncol=1)
+ ax.legend(prop=fontP, loc =1)
+ ax_var.legend(prop=fontP, bbox_to_anchor=(1.1, 0.8), loc=9, ncol=1)
# Show graph
plt.show()
# (optional) save as GIF, comment the following line if you don't want this
- #ani.save('full_test9.gif', writer="imagemagick", fps=60)
+ ani.save('colo_demo.gif', writer="imagemagick", fps=60)
diff --git a/CoLo-AT/data_analysis/realtime_plot.pyc b/CoLo-AT/data_analysis/realtime_plot.pyc
index 0873796d526d99aa6a6f0b56e08abaa3fe4fc654..5636f0855568a79906ecbb7fd5e3a92ca59d7fc9 100644
Binary files a/CoLo-AT/data_analysis/realtime_plot.pyc and b/CoLo-AT/data_analysis/realtime_plot.pyc differ
diff --git a/CoLo-AT/dataset_manager/__init__.pyc b/CoLo-AT/dataset_manager/__init__.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..11d756c59240b3450235ff457ad1d2955fd3d137
Binary files /dev/null and b/CoLo-AT/dataset_manager/__init__.pyc differ
diff --git a/CoLo-AT/dataset_manager/existing_dataset.pyc b/CoLo-AT/dataset_manager/existing_dataset.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..12409e0618c4c6be934c1e59474de8d881176866
Binary files /dev/null and b/CoLo-AT/dataset_manager/existing_dataset.pyc differ
diff --git a/CoLo-AT/full_test9.gif b/CoLo-AT/full_test9.gif
deleted file mode 100644
index 31759e06f486ca6ddc1867ff549f8b0d90842bcd..0000000000000000000000000000000000000000
Binary files a/CoLo-AT/full_test9.gif and /dev/null 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 557e2e83be46e54cd8480385a25d1c0802ece75f..8df35967a6061613cd38380d1e83eb03ec81166c 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/centralized_ekf.pyc b/CoLo-AT/localization_algos/centralized_ekf.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..93d611749e5ab20eb8bdbefa5868940f564f1f0b
Binary files /dev/null and b/CoLo-AT/localization_algos/centralized_ekf.pyc differ
diff --git a/CoLo-AT/localization_algos/ekf_gs_ci2.py b/CoLo-AT/localization_algos/ekf_gs_ci2.py
index 0c3d51d07c5efb25e62ac8f20109efab288484fc..5743b3fd332172fcf35da58acb01d5f7a8effba7 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.1*np.matrix(np.identity(2*num_robots), dtype = float)
+ return 0.01*np.matrix(np.identity(2*num_robots), dtype = float)
def calculate_trace_state_variance(self, robot_data):
[s, orinetations, sigma_s, index] = robot_data
diff --git a/CoLo-AT/localization_algos/localization_algo_framework.pyc b/CoLo-AT/localization_algos/localization_algo_framework.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..e8e2d22b04a8c318634b3861df4f2e575fd17121
Binary files /dev/null and b/CoLo-AT/localization_algos/localization_algo_framework.pyc differ
diff --git a/CoLo-AT/requests/request_response.pyc b/CoLo-AT/requests/request_response.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..6877f67f023df3e19c239075d4058013b0af4765
Binary files /dev/null and b/CoLo-AT/requests/request_response.pyc differ
diff --git a/CoLo-AT/robots/robot_centralized.pyc b/CoLo-AT/robots/robot_centralized.pyc
index 8919a640757c222bf23ab03e38b3216f6eb69b1f..2a0cdb6bd91dc280a5bd1e6d7ac8704c2b843968 100644
Binary files a/CoLo-AT/robots/robot_centralized.pyc and b/CoLo-AT/robots/robot_centralized.pyc differ
diff --git a/CoLo-AT/robots/robot_distributive.pyc b/CoLo-AT/robots/robot_distributive.pyc
index b333a179b26b0fb5b38fb217928739b4d7d64ced..6dccc8c459229f099f628cb86c45d05b53a2ee19 100644
Binary files a/CoLo-AT/robots/robot_distributive.pyc and b/CoLo-AT/robots/robot_distributive.pyc differ
diff --git a/CoLo-AT/robots/robot_system.pyc b/CoLo-AT/robots/robot_system.pyc
index 621918c1802b1bc0c429dd372643cf257501cd80..d4b958ce743a9523e79572f2bdc5f7b71d8aa114 100644
Binary files a/CoLo-AT/robots/robot_system.pyc and b/CoLo-AT/robots/robot_system.pyc differ
diff --git a/CoLo-AT/ros_compatibility.py b/CoLo-AT/ros_compatibility.py
index fd4b12559eb970b7ed7e7665793c5871ddd8f9b7..26f560aedc492db288f6cd7285628750246d4055 100644
--- a/CoLo-AT/ros_compatibility.py
+++ b/CoLo-AT/ros_compatibility.py
@@ -22,7 +22,8 @@ sys.path.append(os.path.join(os.path.dirname(__file__), "localization_algos"))
from centralized_ekf import Centralized_EKF
compname = getpass.getuser()
-dataset_path = "/home/"+ compname +"/full_tests/full_test_v3_6/"
+dataset_path = "/home/"+ compname +"/CoLo/CoLo-D/CoLo-Datasets/official_dataset3/"
+
dataset_labels = [1,2,3]
duration = 310 # duration for the simulation in sec
diff --git a/CoLo-AT/ros_demo.py b/CoLo-AT/ros_demo.py
index 19155870bbdb0ddc86dbce2c2214b29f4089dcac..6d0d045ac6f40fab857221ac460e5611c7ef678f 100644
--- a/CoLo-AT/ros_demo.py
+++ b/CoLo-AT/ros_demo.py
@@ -25,10 +25,10 @@ from centralized_ekf import Centralized_EKF # works
#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_6/"
+dataset_path = "/home/"+ compname +"/CoLo/CoLo-D/CoLo-Datasets/official_dataset2/"
-dataset_labels = [1, 2,3]
-duration = 240 # duration for the simulation in sec
+dataset_labels = [1,2,3]
+duration = 120 # 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 = Centralized_EKF('algo')
diff --git a/CoLo-AT/simulation_process/__init__.pyc b/CoLo-AT/simulation_process/__init__.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..400ad9f195a1b43c946cf565e35e7a2fd53e3104
Binary files /dev/null and b/CoLo-AT/simulation_process/__init__.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 3b7e1eaa2350cb227f65d1287e931c9781c61a44..0fcd11cf4edee175cfe5045297dadb820276eb77 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/sim_manager.pyc b/CoLo-AT/simulation_process/sim_manager.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..53439f6dc5f040798dd79624f43cb3f4c0d8b051
Binary files /dev/null and b/CoLo-AT/simulation_process/sim_manager.pyc differ
diff --git a/CoLo-AT/simulation_process/state_recorder.pyc b/CoLo-AT/simulation_process/state_recorder.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..60c70abad1ad9cc386d7829e5807f86a1bda8d04
Binary files /dev/null and b/CoLo-AT/simulation_process/state_recorder.pyc differ
diff --git a/CoLo-AT/test_simulation.py b/CoLo-AT/test_simulation.py
index 79192ed5f32b564e1eb3a9dc2a983eff02ddb669..19d3ab9ac7a433d1b877ca9cf44cb43def8ed3e1 100644
--- a/CoLo-AT/test_simulation.py
+++ b/CoLo-AT/test_simulation.py
@@ -7,6 +7,7 @@ Created on Sun Apr 8 17:55:06 2018
"""
import os, sys
+import getpass
sys.path.append(os.path.join(os.path.dirname(__file__), "."))
from dataset_manager.existing_dataset import Dataset
from simulation_process.sim_manager import SimulationManager
@@ -26,33 +27,35 @@ from ekf_ls_ci import EKF_LS_CI
from ekf_gs_bound import EKF_GS_BOUND
from ekf_gs_ci2 import EKF_GS_CI2
-
-dataset_path = '/home/william/UTIAS-dataset/MRCLAM_Dataset3/'
+compname = getpass.getuser()
+#dataset_path = "/home/"+ compname +"/CoLo/CoLo-D/UTIAS-Datasets/MRCLAM_Dataset3/"
+dataset_path = "/home/"+ compname +"/CoLo/CoLo-D/CoLo-Datasets/official_dataset2/"
dataset_labels = [1,2,3]
-duration = 300 # duration for the simulation in sec
+duration = 120 # duration for the simulation in sec
testing_dataset = Dataset('testing')
analyzer = Analyzer('analyzer', dataset_labels)
start_time, starting_states, dataset_data, time_arr = testing_dataset.load_MRCLAMDatasets(dataset_path, dataset_labels, duration)
-loc_algo = EKF_GS_CI2('algo')
+loc_algo = EKF_LS_BDA('algo')
robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = False)
-sim = SimulationManager('sim EKF_GS_CI')
-state_recorder = StatesRecorder('EKF_GS_CI',dataset_labels)
-sim.sim_process_native(dataset_labels, testing_dataset, robot, state_recorder, simple_plot = True)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = True)
+sim = SimulationManager('sim LS-BDA')
+state_recorde_gs_ci = StatesRecorder('LS-BDA', dataset_labels)
+sim.sim_process_native(dataset_labels, testing_dataset, robot, state_recorde_gs_ci, simple_plot = True)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorde_gs_ci, plot_graphs = True)
+
##############################################################################
testing_dataset.dataset_reset()
loc_algo = Centralized_EKF('algo')
-robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = True)
+robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = False)
-sim_n = SimulationManager('Centralized_EKF')
-state_recorder_n = StatesRecorder('Centralized_EKF ',dataset_labels)
-sim_n.sim_process_native(dataset_labels, testing_dataset, robot, state_recorder_n, simple_plot = True)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_n, plot_graphs = True)
+sim = SimulationManager('Centralized_EKF')
+state_recorder_cen = StatesRecorder('Cen-EKF', dataset_labels)
+sim.sim_process_native(dataset_labels, testing_dataset, robot, state_recorder_cen, simple_plot = True)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_cen, plot_graphs = True)
-analyzer.algos_comparison([state_recorder, state_recorder_n])
+analyzer.algos_comparison([state_recorde_gs_ci, state_recorder_cen])
diff --git a/CoLo-D/README.md b/CoLo-D/README.md
new file mode 100755
index 0000000000000000000000000000000000000000..13fffb5881de8a0eefd26a2749d0197f44d30b3e
--- /dev/null
+++ b/CoLo-D/README.md
@@ -0,0 +1,54 @@
+# LEMUR CoLo-D
+
+## Synopsis
+
+CoLo-D provides 2d datasets for studying problem like like cooperative localization (with only a team robots), cooperative localization with a known map, and cooperative simultaneous localization and mapping (SLAM). It includes datasets that are compatible with CoLo-AT, a software analysis tool for localization algorithms. These datases can be collected by CoLo-PE, the physical experiment of CoLo.
+
+
+## CoLo Compatible Datasets:
+
+CoLo Datasets:
+4 individual dataset with 3 robots
+- official_dataset1
+- official_dataset2
+- official_dataset3
+- official_dataset4
+
+
+UTIAS dataset (created by the Autonomous Space Robotics Lab (ASRL) at the University of Toronto):
+9 individual dataset with 5 robots
+- MRCLAM_Dataset1
+- MRCLAM_Dataset2
+- MRCLAM_Dataset3
+- MRCLAM_Dataset4
+- MRCLAM_Dataset5
+- MRCLAM_Dataset6
+- MRCLAM_Dataset7
+- MRCLAM_Dataset8
+- MRCLAM_Dataset9
+details info: [UTIAS Dataset](http://asrl.utias.utoronto.ca/datasets/mrclam/).
+
+## CoLo Dataset format
+
+For a team of 3 robots, there are:
+3 odometry files (filename: Robot[subject #]_Odometry.dat ), one for each robot:
+[time [s] | linear velocity [m/s] | angular velocity [rad/s]]
+
+3 measurement files (filename: Robot[subject id]_Measurement.dat ), one for each robot with respect to its camera
+[time [s] | ArUco Code | range [m] | bearing [rad]]
+
+3 measurement_x files (filename: Robot[subject id]_Measurement_x.dat ), one for each robot with respect to its center and covert ArUco Code to subject id
+[time [s] |subject id | range [m] | bearing [rad]]
+
+3 groudntruth files (filename: Robot[subject id]_Groundtruth.dat ), one for each robot
+[time [s] | x [m] | y [m] | orientation [rad]]
+
+1 landmark groundtruth files (file name: Landmark_Groundtruth.dat ), for conversion between subject id and landmark locations
+[subject id | x [m] | y [m]]
+
+Note: for landmark, subject id is the same as its AruCo code; for robots subject id is from 0 to 5.
+
+## Authors
+Shengkang Chen
+
+
diff --git a/CoLo-PE/README.md b/CoLo-PE/README.md
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..a35b34a44ee03605e73606040cad1445351f71e3 100644
--- a/CoLo-PE/README.md
+++ b/CoLo-PE/README.md
@@ -0,0 +1,36 @@
+# LEMUR CoLo-PE
+
+## Synopsis
+
+CoLo-PE is the physical experiment of CoLo. It is a physical experiment setup for robots to collect 2-D datasets for multi-robot problems like cooperative localization (with only a team robots), cooperative localization with a known map, and cooperative simultaneous localization and mapping (SLAM).
+
+In CoLo-PE, there is a team of ground robots equipped with cameras and a groundtruth data collection system. Each robot can will log its odometry data and measurement data (based on AruCo) into its odometry file and measurement files
+
+The manual will explain the software and hardware used in CoLo-PE for users to help them create their own experiments in details.
+
+## Running CoLo-PE
+
+1. Make sure the groundtruth data collection system can capture all the needed subjects includes landmarks and robots
+2. Start groundtruth data collection
+3. Remotely start robots
+4. Wait for data collection process
+5. Remotely deactivate robots
+6. Stop groundtruth data collection
+7. Organized all the files into a single folder
+8. Data processing for collected data
+
+## Robot Control
+
+In CoLo-PE, we use desktop sharing for the remote control of the robots.
+
+'''
+$ cd catkin_ws
+$ source devel/setup.bash
+$ ./robot_full_run.sh
+'''
+
+
+## Authors
+
+Shengkang Chen
+Clara Chun
diff --git a/README.md b/README.md
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..79fc62df3f04d67a3d795d72ae4c6a9a33b8a8be 100644
--- a/README.md
+++ b/README.md
@@ -0,0 +1,17 @@
+# LEMUR CoLo
+
+## Synopsis
+
+CoLo is a performance evaluation system for two-dimensional cooperative localization algorithms. The system consists of a physical experiment (CoLo-PE) for data collection and a software analysis tool (CoLo-AT) using real-world datasets to evaluate the performances of users’ cooperative localization algorithms.
+
+## Structure
+
+There are two main parts in CoLo: a physical experiment (CoLo-PE) for data collection and a software analysis tool (CoLo-AT)
+In CoLo-PE, it will shows the requriment hardwares and softwares needed for setting up the the physical experiments.
+In CoLo-AT, users can load their localization algorithms and test their performances using different datasets on various settings.
+Each parts can be used independetly for users' needs.
+
+
+## Authors
+
+**Shengkang Chen**