diff --git a/CoLo-AT/.DS_Store b/CoLo-AT/.DS_Store
index 0a170c81bddf9aed977bbe5e7f11711c1cb66f95..e01a872f7fc14b9beb6328b08d623973ea733361 100644
Binary files a/CoLo-AT/.DS_Store and b/CoLo-AT/.DS_Store differ
diff --git a/CoLo-AT/colo_demo.gif b/CoLo-AT/colo_demo.gif
new file mode 100644
index 0000000000000000000000000000000000000000..11814815779625121911f8e0665deb5244182b7f
Binary files /dev/null and b/CoLo-AT/colo_demo.gif differ
diff --git a/CoLo-AT/data_analysis/__pycache__/__init__.cpython-35.pyc b/CoLo-AT/data_analysis/__pycache__/__init__.cpython-35.pyc
index b50e6bc72aa57d159806ba314e68b284a5e34db3..3f64cf4f459e9cc56d20cea280b6446ee79f42a7 100644
Binary files a/CoLo-AT/data_analysis/__pycache__/__init__.cpython-35.pyc and b/CoLo-AT/data_analysis/__pycache__/__init__.cpython-35.pyc differ
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 957a8ef427c43e0f9c0e22385fa6c384afc58433..66602b7cdaafa5773bc1edfc762f0b78535c5c2a 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__/realtime_plot.cpython-35.pyc b/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-35.pyc
index e02adf784a5865eee4553c1a29f93871a9fe3835..b1e353177970a2d22a0aa09fab3db2049328c9f9 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/data_analyzer.py b/CoLo-AT/data_analysis/data_analyzer.py
index 96f3abab42f0dac2346158d27daa76d8c73070c1..b5be8b81c25eab0ac2cd168e99e382477eaebf6f 100644
--- a/CoLo-AT/data_analysis/data_analyzer.py
+++ b/CoLo-AT/data_analysis/data_analyzer.py
@@ -25,16 +25,16 @@ class Analyzer():
def set_dataset_label(self, dataset_labels):
self.dataset_labels = dataset_labels
- def plot_loc_err_and_trace(self, loc_err, trace, time_arr, measurement_count):
+ def plot_loc_err_and_trace(self, loc_err, trace, time_arr, measurement_count, recorder_name = None):
fig = plt.figure()
plt.suptitle('Correctness analysis')
fig1 = fig.add_subplot(311)
fig2 = fig.add_subplot(312)
fig3 = fig.add_subplot(313)
- fig1.plot(time_arr, loc_err)
- fig2.plot(time_arr, trace)
- fig3.bar(time_arr, measurement_count)
+ fig1.plot(time_arr, loc_err, label = recorder_name)
+ fig2.plot(time_arr, trace, label = recorder_name)
+ fig3.bar(time_arr, measurement_count, label = recorder_name)
fig1.set_title('Estimation deviation error')
@@ -50,12 +50,12 @@ class Analyzer():
fig2.legend(loc='center left', bbox_to_anchor=(1, 0.5))
- fig3.set_title('Observation counts', fontsize=20)
- fig3.set_xlabel('Time[s]', fontsize=16)
- fig3.set_ylabel('Num of obser', fontsize=16)
+ fig3.set_title('Observation counts')
+ fig3.set_xlabel('Time[s]')
+ fig3.set_ylabel('Num of obser')
#fig1.set_ylim(0, 6)
fig3.legend(loc='center left', bbox_to_anchor=(1, 0.5))
- fig3.tick_params(labelsize=14)
+ #fig3.tick_params(labelsize=14)
fig.subplots_adjust(hspace = 1.2)
@@ -65,7 +65,10 @@ class Analyzer():
def calculate_loc_err_and_trace_state_variance_per_run(self, data_recorder, unit_time_interval = 0.5, plot_graphs = True):
#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()
+ recorder_name = data_recorder.get_name()
+
update_in_time_order = data_recorder.get_updata_type_in_time_order()
data_in_time_order = data_recorder.get_data_in_time_order()
@@ -123,7 +126,7 @@ class Analyzer():
print('Avg trace of state variances per run: ', sum(trace_per_run)/len(trace_per_run))
if plot_graphs:
- self.plot_loc_err_and_trace(loc_err_per_run, trace_per_run, time_arr, measurement_count = measurement_num)
+ self.plot_loc_err_and_trace(loc_err_per_run, trace_per_run, time_arr, measurement_count = measurement_num, recorder_name = recorder_name)
return loc_err_per_run, trace_per_run, time_arr
diff --git a/CoLo-AT/data_analysis/realtime_plot.py b/CoLo-AT/data_analysis/realtime_plot.py
index f8dd0c70f8725b0fb82a13f71e030718bf91df81..f9e04e9f96553726785b46deddc43ebbaecb187a 100644
--- a/CoLo-AT/data_analysis/realtime_plot.py
+++ b/CoLo-AT/data_analysis/realtime_plot.py
@@ -69,8 +69,10 @@ def initialize_graphs(num_robots, ax_err, ax_var, loc_err, trace_sigma, time_fun
arr_var = []
for i in range(0, num_robots):
time_arr = time_func(i + 1)
- line, = ax_err.plot(time_arr, loc_err[i+1], colors[i]+'-', label='Robot ' + str(i+1))
- line2, = ax_var.plot(time_arr, trace_sigma[i+1], colors[i]+'-', label='Robot ' + str(i+1))
+ #line, = ax_err.plot(time_arr, loc_err[i+1], colors[i]+'-', label='Robot ' + str(i+1))
+ #line2, = ax_var.plot(time_arr, trace_sigma[i+1], colors[i]+'-', label='Robot ' + str(i+1))
+ line, = ax_err.plot(time_arr, loc_err[i+1], colors[i]+'-')
+ line2, = ax_var.plot(time_arr, trace_sigma[i+1], colors[i]+'-')
arr_err.append(line)
arr_var.append(line2)
return [arr_err, arr_var]
@@ -155,7 +157,6 @@ def animate_plot(dataset_labels, data_recorder, analyzer, lm = None):
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)
@@ -163,6 +164,7 @@ 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:, :])
+
'''
fig = plt.figure()
gs = gridspec.GridSpec(3, 2)
@@ -172,6 +174,7 @@ def animate_plot(dataset_labels, data_recorder, analyzer, lm = None):
ax.set_xlim([-2, 2])
ax.set_ylim([-2, 2])
+ '''
# configure graph
# set bounds
#fig.set_size_inches((18,18))
diff --git a/CoLo-AT/dataset_manager/__pycache__/__init__.cpython-35.pyc b/CoLo-AT/dataset_manager/__pycache__/__init__.cpython-35.pyc
index f3c36bd5c707e061f9d31563c5f2f8b0ccc41c32..109e4a0bcacc9897b7b0a4058eec30b6f11996b3 100644
Binary files a/CoLo-AT/dataset_manager/__pycache__/__init__.cpython-35.pyc and b/CoLo-AT/dataset_manager/__pycache__/__init__.cpython-35.pyc differ
diff --git a/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-35.pyc b/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-35.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..401ad6cc646bff1fee55da87fdee3ea78068da04
Binary files /dev/null and b/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-35.pyc differ
diff --git a/CoLo-AT/icra2019_demo.py b/CoLo-AT/icra2019_demo.py
new file mode 100644
index 0000000000000000000000000000000000000000..a7a21cb03ae8245b7ab3cf744df75538113671d0
--- /dev/null
+++ b/CoLo-AT/icra2019_demo.py
@@ -0,0 +1,53 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Sun Apr 8 17:55:06 2018
+
+@author: william
+"""
+
+import os, sys
+import getpass
+sys.path.append(os.path.join(os.path.dirname(__file__), "."))
+from dataset_manager.realworld_dataset_manager import RW_Dataset_Manager
+from simulation_process.sim_manager import SimulationManager
+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
+
+# load algorithms
+sys.path.append(os.path.join(os.path.dirname(__file__), "localization_algos"))
+from centralized_ekf import Centralized_EKF
+from ekf_gs_ci2 import EKF_GS_CI2
+
+dataset_path = "/Users/shengkangchen/Documents/CoLo/CoLo-D/CoLo-Datasets/official_dataset3/"
+robot_labels = [1,2,3]
+duration = 120 # duration for the simulation in sec
+testing_dataset = RW_Dataset_Manager('testing')
+start_time, starting_states, dataset_data, time_arr = testing_dataset.load_datasets(dataset_path, robot_labels, duration)
+analyzer = Analyzer('analyzer', robot_labels)
+
+loc_algo = EKF_GS_CI2('algo')
+robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
+sim = SimulationManager('sim gs_ci')
+state_recorder = StatesRecorder('gs_ci', robot_labels)
+
+sim.sim_process_native(robot_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)
+
+animate_plot(robot_labels, state_recorder, analyzer, testing_dataset.get_landmark_map())
+
+##############################################################################
+'''
+testing_dataset.dataset_reset()
+loc_algo = Centralized_EKF('algo')
+robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
+
+sim = SimulationManager('Centralized_EKF')
+state_recorder_cen = StatesRecorder('Cen-EKF', robot_labels)
+sim.sim_process_native(robot_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_recorde_gs_ci, state_recorder_cen])
+'''
diff --git a/CoLo-AT/localization_algos/__pycache__/centralized_ekf.cpython-35.pyc b/CoLo-AT/localization_algos/__pycache__/centralized_ekf.cpython-35.pyc
index 81a915cb00b1fdfc5005ee5199ace0d68e5e028d..79c599cd36ed6d568c136407ebd3c80227e748ef 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/centralized_ekf.cpython-35.pyc and b/CoLo-AT/localization_algos/__pycache__/centralized_ekf.cpython-35.pyc differ
diff --git a/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-35.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-35.pyc
index 1bb4b98c836ce1ebbd895f1c63f76b070036ea18..644e3723232ce5b977d0f3d3d8841634c8166d5c 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-35.pyc and b/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-35.pyc differ
diff --git a/CoLo-AT/localization_algos/__pycache__/localization_algo_framework.cpython-35.pyc b/CoLo-AT/localization_algos/__pycache__/localization_algo_framework.cpython-35.pyc
index c70caf19a44a99759548f9b65aac709c0097fd76..88d837a62b2ef777eda88089d67bdb36db91cf81 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/localization_algo_framework.cpython-35.pyc and b/CoLo-AT/localization_algos/__pycache__/localization_algo_framework.cpython-35.pyc differ
diff --git a/CoLo-AT/requests/__pycache__/request_response.cpython-35.pyc b/CoLo-AT/requests/__pycache__/request_response.cpython-35.pyc
index e79d4a633a8d51d0b273c790f2ee29e6e9bbfe70..9d75bf28fb5aff3b1f711a9b3852a24b87ffa9ee 100644
Binary files a/CoLo-AT/requests/__pycache__/request_response.cpython-35.pyc and b/CoLo-AT/requests/__pycache__/request_response.cpython-35.pyc differ
diff --git a/CoLo-AT/robots/__pycache__/__init__.cpython-35.pyc b/CoLo-AT/robots/__pycache__/__init__.cpython-35.pyc
index fde83e0bb0008574e83485b2e7a81e1b926fc965..333254c8ae1a8772b63d1ba9930e6ca381710fe3 100644
Binary files a/CoLo-AT/robots/__pycache__/__init__.cpython-35.pyc and b/CoLo-AT/robots/__pycache__/__init__.cpython-35.pyc differ
diff --git a/CoLo-AT/robots/__pycache__/robot_centralized.cpython-35.pyc b/CoLo-AT/robots/__pycache__/robot_centralized.cpython-35.pyc
index 2923475fb218f4636ecca0048cd1f783bbc77218..40aceca93c112a831eba9eadc798941cd70d2513 100644
Binary files a/CoLo-AT/robots/__pycache__/robot_centralized.cpython-35.pyc and b/CoLo-AT/robots/__pycache__/robot_centralized.cpython-35.pyc differ
diff --git a/CoLo-AT/robots/__pycache__/robot_distributive.cpython-35.pyc b/CoLo-AT/robots/__pycache__/robot_distributive.cpython-35.pyc
index 1900e20db14dbc6e249b365145032c41259d6c67..401aa4cd4e7808a28610b693484913699ac3c43e 100644
Binary files a/CoLo-AT/robots/__pycache__/robot_distributive.cpython-35.pyc and b/CoLo-AT/robots/__pycache__/robot_distributive.cpython-35.pyc differ
diff --git a/CoLo-AT/robots/__pycache__/robot_parameters.cpython-35.pyc b/CoLo-AT/robots/__pycache__/robot_parameters.cpython-35.pyc
index 74164906cd1d1220b0a92c38542c179e50aed113..4c471e33be11b51867fdac09f03a41b7f20ceb8a 100644
Binary files a/CoLo-AT/robots/__pycache__/robot_parameters.cpython-35.pyc and b/CoLo-AT/robots/__pycache__/robot_parameters.cpython-35.pyc differ
diff --git a/CoLo-AT/robots/__pycache__/robot_system.cpython-35.pyc b/CoLo-AT/robots/__pycache__/robot_system.cpython-35.pyc
index 25d23386e6336389cfb87686ae89bd28cf844cda..85a4a486ad21a86b2e443ecf78b4c9641d077723 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 fdf4fa1b6761803dabb7c8be296bda7f1920a661..0a4ab392e06f5b4faa2b3b161afb380bbc100399 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__/__init__.cpython-35.pyc b/CoLo-AT/simulation_process/__pycache__/__init__.cpython-35.pyc
index 1f0f986567d17783310f451baffb6572a4c29fd9..971d47f21061cba8146af7e9127921a36a4e4ad6 100644
Binary files a/CoLo-AT/simulation_process/__pycache__/__init__.cpython-35.pyc and b/CoLo-AT/simulation_process/__pycache__/__init__.cpython-35.pyc differ
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 d388f62ec168a34b8884ffdd9db950c6475c06b3..019e67c982d57ed0600d4481d0dfbe282cbd1f2e 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__/state_recorder.cpython-35.pyc b/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-35.pyc
index 0fcd11cf4edee175cfe5045297dadb820276eb77..74722abe96c6d241dd00ed16acf1fa8b595bc26b 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 18718dff2454bc9ce43b6931a97894a27132023f..31aec99f52c7efbe1e48f8d132f91e5c27c28b48 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/state_recorder.py b/CoLo-AT/simulation_process/state_recorder.py
index 01028dfc1c335bd983fe79c2cb127311487cdcc4..cf310934ebdf925c904a6e101323fdd174bc2a24 100644
--- a/CoLo-AT/simulation_process/state_recorder.py
+++ b/CoLo-AT/simulation_process/state_recorder.py
@@ -41,6 +41,10 @@ class StatesRecorder():
self.trace_sigma_s_arr[label].append(initial_trace_state_var)
self.updata_type_arr[label].append('ini')
self.updata_type_in_time_order.append('ini')
+
+ def get_name(self):
+ return self.name
+
def get_dataset_labels(self):
return self.dataset_labels