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..510b42ad54e08dacc55ab4fa5ab76470af32d3cb
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__/__init__.cpython-36.pyc b/CoLo-AT/data_analysis/__pycache__/__init__.cpython-36.pyc
index a6c36afd728c74bcb0dfce9bb6db5c63562fdade..7951a34af129990fbec3808b660de5d9b931859d 100644
Binary files a/CoLo-AT/data_analysis/__pycache__/__init__.cpython-36.pyc and b/CoLo-AT/data_analysis/__pycache__/__init__.cpython-36.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__/data_analyzer.cpython-36.pyc b/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc
index 9099e8d5a14e68fe056753339e4b41849ba094a8..98deeb48ab51fd14206fa66498458fe60f18dfe4 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 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/__pycache__/realtime_plot.cpython-36.pyc b/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-36.pyc
index 6b1a42fb26002c6c84ef44c593f5cfb6ffc15f86..3a8dd54cec228a7daaec9f21364b2a5091cbcb31 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 96f3abab42f0dac2346158d27daa76d8c73070c1..f608879cdffc056f76489078cb7a4b1faa586bda 100644
--- a/CoLo-AT/data_analysis/data_analyzer.py
+++ b/CoLo-AT/data_analysis/data_analyzer.py
@@ -25,17 +25,20 @@ 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):
+
+ [lm_measurement_num, relative_measurment_num] = measurement_count
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, lm_measurement_num, label = "landmark observation")
+ fig3.bar(time_arr, relative_measurment_num, label = "relative observation")
+ fig3.bar(time_arr, lm_measurement_num, bottom = relative_measurment_num, label = "landmark observation")
fig1.set_title('Estimation deviation error')
fig1.set_xlabel('Time[s]')
@@ -50,12 +53,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 +68,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()
@@ -75,7 +81,8 @@ class Analyzer():
finish_flag = False
loc_err_per_run = []
- measurement_num = []
+ lm_measurement_num = []
+ relative_measurment_num = []
trace_per_run = []
time_arr = []
@@ -83,7 +90,8 @@ class Analyzer():
loc_err_per_time_iterval = 0
trace_per_time_iterval = 0
num_dataline_per_time_iterval = 0
- measurement_count = 0
+ lm_measurement_count = 0
+ relative_measurement_count = 0
while interval_start_time <= time < interval_start_time+unit_time_interval:
try:
@@ -93,8 +101,10 @@ class Analyzer():
break
time = data_in_time_order[time_index][0]
- if update_in_time_order[time_index] == 'landmark observation' or update_in_time_order[time_index] == 'relative observation':
- measurement_count +=1
+ if update_in_time_order[time_index] == 'landmark observation':
+ lm_measurement_count +=1
+ if update_in_time_order[time_index] == 'relative observation':
+ relative_measurement_count +=1
loc_err_per_time_iterval+= data_in_time_order[time_index][7]
trace_per_time_iterval += data_in_time_order[time_index][4]
@@ -113,7 +123,10 @@ class Analyzer():
loc_err_per_run.append(0)
trace_per_run.append(0)
- measurement_num.append(measurement_count)
+ lm_measurement_num.append(lm_measurement_count)
+ relative_measurment_num.append(relative_measurement_count)
+
+
time_arr.append((interval_start_time+unit_time_interval+interval_start_time)/2)
interval_start_time = interval_start_time+unit_time_interval
@@ -123,7 +136,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 = [lm_measurement_num, relative_measurment_num], recorder_name = recorder_name)
return loc_err_per_run, trace_per_run, time_arr
@@ -219,6 +232,7 @@ class Analyzer():
def algos_comparison(self, arr_data_recorder, only_trace=None):
+ print("************Algorithm Comparison***************")
arr_loc_err = []
arr_trace = []
for data_recorder in arr_data_recorder:
diff --git a/CoLo-AT/data_analysis/realtime_plot.py b/CoLo-AT/data_analysis/realtime_plot.py
index a9c65ffcc9c9fd9337916b82700b6ca6e3f51ec7..066239c8021fff68da08888741738a9558d1fb52 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__/__init__.cpython-36.pyc b/CoLo-AT/dataset_manager/__pycache__/__init__.cpython-36.pyc
index d15e459c547ee60f84d0fd9af17f5941c3593eb0..cb88c3505e2b2c5e6c1ee380f0a77cd9c0a344d3 100644
Binary files a/CoLo-AT/dataset_manager/__pycache__/__init__.cpython-36.pyc and b/CoLo-AT/dataset_manager/__pycache__/__init__.cpython-36.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/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc b/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc
index 5ed681eb5c3773249ce5512ba5e08b9c1e174c34..f73b2cf653e3579d4ded1a002c64063cda1cb593 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/gs_ci_journal.py b/CoLo-AT/gs_ci_journal.py
new file mode 100644
index 0000000000000000000000000000000000000000..91ee0212f07fdd62f0520436112a5473b39e36be
--- /dev/null
+++ b/CoLo-AT/gs_ci_journal.py
@@ -0,0 +1,54 @@
+#!/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 ekf_gs_ci2 import EKF_GS_CI2
+from gs_ci_bound import GS_CI_Bound
+
+# dataset_path = "/Users/shengkangchen/Documents/CoLo/CoLo-D/CoLo-Datasets/official_dataset3/"
+dataset_path = "/home/william/CoLo/CoLo-D/CoLo-Datasets/official_dataset3/" # for desktop Ubuntu
+
+robot_labels = [1,2,3]
+duration = 180 # 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 = False)
+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 = GS_CI_Bound('algo')
+robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
+
+sim = SimulationManager('sim gs_ci_bound')
+state_recorder_bound = StatesRecorder('gs_ci_bound', robot_labels, state_var_only = True)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_bound, simple_plot = False)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_bound, plot_graphs = True)
+
+analyzer.algos_comparison([state_recorder, state_recorder_bound], only_trace = ['gs_ci_bound'])
diff --git a/CoLo-AT/gs_sci_vs_ls_ci.py b/CoLo-AT/gs_sci_vs_ls_ci.py
new file mode 100644
index 0000000000000000000000000000000000000000..e3bba30a0e831272609bb0f52b909aa2f8185632
--- /dev/null
+++ b/CoLo-AT/gs_sci_vs_ls_ci.py
@@ -0,0 +1,70 @@
+#!/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 # works
+from ekf_ls_bda import EKF_LS_BDA
+from ekf_ls_ci import EKF_LS_CI
+from ekf_gs_ci2 import EKF_GS_CI2
+from ekf_gs_sci2 import EKF_GS_SCI2
+
+
+# dataset_path = "/Users/shengkangchen/Documents/CoLo/CoLo-D/CoLo-Datasets/official_dataset3/"
+dataset_path = "/home/william/CoLo/CoLo-D/CoLo-Datasets/official_dataset1/" # for desktop Ubuntu
+
+robot_labels = [1,2,3]
+duration = 180 # 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 = False)
+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 = EKF_LS_CI('algo')
+robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
+
+sim = SimulationManager('sim ls_ci')
+state_recorder_LS_CI = StatesRecorder('LS_CI', robot_labels)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_CI, simple_plot = False)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_CI, plot_graphs = True)
+
+##############################################################################
+'''
+testing_dataset.dataset_reset()
+loc_algo = EKF_GS_SCI2('algo')
+robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
+
+sim = SimulationManager('sim gs_sci')
+state_recorder_GS_SCI = StatesRecorder('GS_SCI', robot_labels)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_GS_SCI, simple_plot = False)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_GS_SCI, plot_graphs = True)
+'''
+analyzer.algos_comparison([state_recorder, state_recorder_LS_CI])
diff --git a/CoLo-AT/icra2019_demo.py b/CoLo-AT/icra2019_demo.py
new file mode 100644
index 0000000000000000000000000000000000000000..69956e1540fec673f4c1c484ac9307c57b320484
--- /dev/null
+++ b/CoLo-AT/icra2019_demo.py
@@ -0,0 +1,55 @@
+#!/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/"
+dataset_path = "/home/william/CoLo/CoLo-D/CoLo-Datasets/official_dataset3/" # for desktop Ubuntu
+
+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/isrr2017_v2_2019.py b/CoLo-AT/isrr2017_v2_2019.py
new file mode 100644
index 0000000000000000000000000000000000000000..344afecd0dd92e7ecdf14c0654d6f450ab8bbcbd
--- /dev/null
+++ b/CoLo-AT/isrr2017_v2_2019.py
@@ -0,0 +1,93 @@
+#!/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 # works
+from ekf_ls_bda import EKF_LS_BDA
+from ekf_ls_ci import EKF_LS_CI
+from ekf_gs_ci2 import EKF_GS_CI2
+from ekf_gs_sci2 import EKF_GS_SCI2
+
+
+# dataset_path = "/Users/shengkangchen/Documents/CoLo/CoLo-D/CoLo-Datasets/official_dataset3/"
+dataset_path = "/home/william/CoLo/CoLo-D/CoLo-Datasets/official_dataset3/" # for desktop Ubuntu
+
+robot_labels = [1,2,3]
+duration = 180 # 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 = False)
+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('sim cen_ekf')
+state_recorder_LS_cen = StatesRecorder('LS_cen', robot_labels)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_cen, simple_plot = False)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_cen, plot_graphs = True)
+
+
+##############################################################################
+
+testing_dataset.dataset_reset()
+loc_algo = EKF_LS_BDA('algo')
+robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
+
+sim = SimulationManager('sim ls_bda')
+state_recorder_LS_BDA = StatesRecorder('LS_BDA', robot_labels)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_BDA, simple_plot = False)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_BDA, plot_graphs = True)
+
+
+##############################################################################
+
+testing_dataset.dataset_reset()
+loc_algo = EKF_LS_CI('algo')
+robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
+
+sim = SimulationManager('sim ls_ci')
+state_recorder_LS_CI = StatesRecorder('LS_CI', robot_labels)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_CI, simple_plot = False)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_CI, plot_graphs = True)
+
+##############################################################################
+
+testing_dataset.dataset_reset()
+loc_algo = EKF_GS_SCI2('algo')
+robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
+
+sim = SimulationManager('sim gs_sci')
+state_recorder_GS_SCI = StatesRecorder('GS_SCI', robot_labels)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_GS_SCI, simple_plot = False)
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_GS_SCI, plot_graphs = True)
+
+analyzer.algos_comparison([state_recorder, state_recorder_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI])
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__/centralized_ekf.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/centralized_ekf.cpython-36.pyc
index 841baa9ca29af8beb5f008ea2df788530bc20823..b622a79aa5e6e569d0346267045d4f76a5f9e92b 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/centralized_ekf.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/centralized_ekf.cpython-36.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__/ekf_gs_ci2.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-36.pyc
index 8fcbaef524ac506c9b9676683c55297759c8b32a..87b4e002316795055a7b4197a473a3c2f164b637 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__/ekf_ls_bda.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_ls_bda.cpython-36.pyc
index dfff1580319cd69bccc411af302e0a2a4aeb2dd0..6843e8adf7a54bbc8527d351585099c800e71b27 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/ekf_ls_bda.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/ekf_ls_bda.cpython-36.pyc differ
diff --git a/CoLo-AT/localization_algos/__pycache__/ekf_ls_ci.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_ls_ci.cpython-36.pyc
index 7bbf05027bcebec06b2ffaf2f49d45c423207ea6..8f679589595ce4259c225d0622f73a6b0385245e 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/ekf_ls_ci.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/ekf_ls_ci.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
new file mode 100644
index 0000000000000000000000000000000000000000..0011e92cb63c4fc5e20457ee0cf17255617a39e5
Binary files /dev/null and b/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-36.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/localization_algos/__pycache__/localization_algo_framework.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/localization_algo_framework.cpython-36.pyc
index eec902bc7e08e4b527973ba0b2a34ecf567d64a3..0bca44984a733081df2ca92b4174ffa27ef07b52 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/localization_algo_framework.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/localization_algo_framework.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 5743b3fd332172fcf35da58acb01d5f7a8effba7..19d7cadd0b6d571de4a6b9e63ae6950ced29b2d1 100644
--- a/CoLo-AT/localization_algos/ekf_gs_ci2.py
+++ b/CoLo-AT/localization_algos/ekf_gs_ci2.py
@@ -53,8 +53,8 @@ class EKF_GS_CI2(ekf_algo_framework):
def absolute_obser_update(self, robot_data, sensor_data):
[s, orinetations, sigma_s, index] = robot_data
[measurement_data, sensor_covariance] = sensor_data
-
sigma_ob = sensor_covariance
+
num_robots = int(len(s)/2)
self_theta = orinetations[index]
diff --git a/CoLo-AT/localization_algos/ekf_ls_ci.py b/CoLo-AT/localization_algos/ekf_ls_ci.py
index fe396468252612ed20b838524c8dc6fbf0f9f63b..cfe89cdc573141203a19b937443a38ab7001992b 100644
--- a/CoLo-AT/localization_algos/ekf_ls_ci.py
+++ b/CoLo-AT/localization_algos/ekf_ls_ci.py
@@ -40,8 +40,8 @@ class EKF_LS_CI(ekf_algo_framework):
s[i+1,0] = s[i+1,0] + sin(self_theta)*v*delta_t #y
var_u_v = sigma_odo[0,0]
- sigma_s[i:i+2, i:i+2] = sigma_s[i:i+2, i:i+2]+ delta_t*delta_t*rot_mtx(self_theta)*matrix([[var_u_v, 0],[0, 0]])*rot_mtx(self_theta).T
-
+ #sigma_s[i:i+2, i:i+2] = sigma_s[i:i+2, i:i+2]+ delta_t*delta_t*rot_mtx(self_theta)*matrix([[var_u_v, 0],[0, 0]])*rot_mtx(self_theta).T
+ sigma_s[i:i+2, i:i+2] = sigma_s[i:i+2, i:i+2]+ delta_t*delta_t*rot_mtx(self_theta)*sigma_odo*rot_mtx(self_theta).T
return [s, orinetations, sigma_s]
@@ -78,8 +78,6 @@ class EKF_LS_CI(ekf_algo_framework):
z_hat = rot_mtx(self_theta).getT()*(np.matrix([delta_x, delta_y]).getT()) # shifted to robot frame
-
-
sigma_z = rot_mtx(bearing) * sigma_ob * rot_mtx(bearing).getT()
sigma_invention = H * local_sigma * H.getT() + sigma_z
kalman_gain = local_sigma*H.getT()*sigma_invention.getI()
diff --git a/CoLo-AT/localization_algos/gs_ci_bound.py b/CoLo-AT/localization_algos/gs_ci_bound.py
index 74e482c0b35a179d6fda78b7fb1512de9f406183..da3a82097d2c3bd74131201efc44ea3827bb96f0 100644
--- a/CoLo-AT/localization_algos/gs_ci_bound.py
+++ b/CoLo-AT/localization_algos/gs_ci_bound.py
@@ -1,67 +1,60 @@
import numpy as np
from numpy import matrix
from math import cos, sin, atan2, sqrt
+from localization_algo_framework import ekf_algo_framework
def rot_mtx(theta):
return matrix([[cos(theta), -sin(theta)], [sin(theta), cos(theta)]])
-class gs_ci_bound():
+class GS_CI_Bound(ekf_algo_framework):
def __init__(self, algo_name):
self.algo_name = algo_name
- self.d_max = 4.5 # max measurement distance
+ self.d_max = 2 # max measurement distance
self.d_var = 0.2
self.bearing_var = 0.2
- self.var_v = 0.5
+ self.var_v = 0.1
- def state_veriance_init(self, num_robots):
+ def state_variance_init(self, num_robots):
return 0.01*np.matrix(np.identity(2*num_robots), dtype = float)
- def algo_update(self, robot_data, update_type, sensor_data):
- print 'in th algo update'
- #return [s, orinetations, sigma_s]
- [s, odo_freq, th_sigma_s, index] = robot_data
- [measurement_data, sensor_covariance] = sensor_data
-
- if update_type == 'propagation update':
- print 'th bound propagation'
- return self.propagation_update(s, th_sigma_s, sensor_covariance, index, odo_freq)
- elif update_type == 'landmark observation update':
- return self.absolute_obser_update(s, th_sigma_s, index)
- elif update_type == 'relative observation update':
- obser_index = measurement_data[0]
- return self.relative_obser_update(s, th_sigma_s, index, obser_index)
- elif update_type == 'communication':
- return self.communication(s, th_sigma_s, index, measurement_data)
- else:
- print "invalid update"
-
+ def calculate_trace_state_variance(self, robot_data):
+ [s, orinetations, sigma_s, index] = robot_data
+ i = 2*index
+ trace_state_var = np.trace(sigma_s[i:i+2, i:i+2])
+ return trace_state_var
- def calculate_trace_state_variance(self, state_variance, robot_index):
- #j = robot_index*2
- #return np.trace(state_variance[j:j+1, j:j+1])
- return np.trace(state_variance)
+ #def propagation_update(self, s, th_sigma_s, sigma_odo, index, odo_freq):
+ def propagation_update(self, robot_data, sensor_data):
+ [s, orinetations, sigma_s, index] = robot_data
+ [measurement_data, sensor_covariance] = sensor_data
+ sigma_odo = sensor_covariance
- def propagation_update(self, s, th_sigma_s, sigma_odo, index, odo_freq):
- num_robots = len(s)/2
+ num_robots = int(len(s)/2)
i = 2*index
- delta_t = 1/odo_freq
+ #delta_t = 1/odo_freq
+ delta_t = measurement_data[0]
for j in range(num_robots):
jj = 2*j
if j==index:
- th_sigma_s[jj:jj+2, jj:jj+2] += sigma_odo[0,0]*np.identity(2)*delta_t*delta_t
+ sigma_s[jj:jj+2, jj:jj+2] += sigma_odo[0,0]*np.identity(2)*delta_t*delta_t
else:
#var_v = pow(2*self.d_max, 2)/12
var_v = self.var_v
- th_sigma_s[jj:jj+2, jj:jj+2] += var_v*np.identity(2)*delta_t*delta_t
- print 'bound'
- print np.trace(state_variance)
- return th_sigma_s
+ sigma_s[jj:jj+2, jj:jj+2] += var_v*np.identity(2)*delta_t*delta_t
+
+ #return th_sigma_s
+ return [s, orinetations, sigma_s]
+
+ #def absolute_obser_update(self, s, th_sigma_s, index):
+ def absolute_obser_update(self, robot_data, sensor_data):
+ [s, orinetations, sigma_s, index] = robot_data
+ [measurement_data, sensor_covariance] = sensor_data
+ sigma_ob = sensor_covariance
- def absolute_obser_update(self, s, th_sigma_s, index):
- num_robots = len(s)/2
+ num_robots = int(len(s)/2)
i = 2*index
H_i = np.matrix(np.zeros((2,2*num_robots)))
H_i[0, i] = -1
@@ -71,22 +64,25 @@ class gs_ci_bound():
var_dis = self.d_var
var_phi = self.bearing_var
- #sigma_th_z = np.matrix(max(sigma_d, d_max*d_max*sigma_bearing)*np.identity(2)) #sigme_Ri
- #th_sigma_s = (th_sigma_s.getI()+H_i.getT()*sigma_th_z.getI()*H_i).getI()
-
sigma_th_z = np.matrix(max(var_dis, d_max*d_max*var_phi)*np.identity(2))
- th_sigma_s = (th_sigma_s.getI() + H_i.getT() * sigma_th_z.getI() * H_i).getI()
+ sigma_s = (sigma_s.getI() + H_i.getT() * sigma_th_z.getI() * H_i).getI()
- return th_sigma_s
+ return [s, orinetations, sigma_s]
- def relative_obser_update(self, s, th_sigma_s, index, obser_index):
+ #def relative_obser_update(self, s, th_sigma_s, index, obser_index):
#i obser j
+ def relative_obser_update(self, robot_data, sensor_data):
+ #when robot i observes robot j
+ [s, orinetations, sigma_s, index] = robot_data
+ [measurement_data, sensor_covariance] = sensor_data
+ sigma_ob = sensor_covariance
- i = 2*index
- j = obser_index * 2
+ num_robots = int(len(s)/2)
- num_robots = len(s)/2
+ i = 2*index
+ obser_index = measurement_data[0]
+ j = obser_index * 2
H_ij = np.matrix(np.zeros((2,2*num_robots)))
H_ij[0, i] = -1
@@ -99,43 +95,19 @@ class gs_ci_bound():
var_phi = self.bearing_var
sigma_th_z = np.matrix(max(var_dis, d_max*d_max*var_phi)*np.identity(2))
- th_sigma_s = (th_sigma_s.getI() + H_ij.getT() * sigma_th_z.getI() * H_ij).getI()
- return th_sigma_s
-
-
- def communication(self, s, th_sigma_s, index, measurement_data):
-
- e = 0.5
- '''
- sender_th_sigma_s = measurement_data
- rec_th_sigma_s = th_sigma_s
- sig_inv = e*rec_th_sigma_s.getI() + (1-e)*sender_th_sigma_s.getI()
- th_sigma_s = sig_inv.getI()
-
- '''
- '''
- i = 1*2
- j = 2*2
+ sigma_s = (sigma_s.getI() + H_ij.getT() * sigma_th_z.getI() * H_ij).getI()
+ return [s, orinetations, sigma_s]
- num_robots = len(s)/2
- H_ij = np.matrix(np.zeros((2,2*num_robots)))
- H_ij[0, i] = -1
- H_ij[1, i+1] = -1
- H_ij[0, j] = 1
- H_ij[1, j+1] = 1
-
+ #def communication(self, s, th_sigma_s, index, measurement_data):
+ def communication(self, robot_data, sensor_data):
+ [s, orinetations, sigma_s, index] = robot_data
+ [comm_data, comm_variance] = sensor_data
+ [sender_idx, comm_robot_s, comm_robot_sigma_s]=comm_data
- d_max = self.d_max
- sigma_d = self.d_var
- sigma_bearing = self.bearing_var
- sigma_th_z = np.matrix(max(sigma_d, d_max*d_max*sigma_bearing)*np.identity(2)) #sigme_Ri
- sig_inv = e*th_sigma_s.getI() + (1-e)*(th_sigma_s.getI()+H_ij.getT()*sigma_th_z.getI()*H_ij)
- th_sigma_s = sig_inv.getI()
- '''
+ e = 0.8
- comm_robot_th_sigma = measurement_data
- th_sigma_s = ( e*th_sigma_s.getI() + (1-e)*comm_robot_th_sigma.getI()).getI() # from TK's gs-ci
+ sigma_s = ( e*sigma_s.getI() + (1-e)*comm_robot_sigma_s.getI()).getI() # from TK's gs-ci
- return th_sigma_s
\ No newline at end of file
+ return [s, orinetations, sigma_s]
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/requests/__pycache__/request_response.cpython-36.pyc b/CoLo-AT/requests/__pycache__/request_response.cpython-36.pyc
index 02de2f22087bd952cd1f47b84adff5254474fbc2..02f0b15864eb7b70ac13122aa853f571e3175e7e 100644
Binary files a/CoLo-AT/requests/__pycache__/request_response.cpython-36.pyc and b/CoLo-AT/requests/__pycache__/request_response.cpython-36.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__/__init__.cpython-36.pyc b/CoLo-AT/robots/__pycache__/__init__.cpython-36.pyc
index 9fbfa1b359d1539e86fc883a14c1eca9b609c242..3e4279038eff40af456ec8a9a6ad13d41851a75e 100644
Binary files a/CoLo-AT/robots/__pycache__/__init__.cpython-36.pyc and b/CoLo-AT/robots/__pycache__/__init__.cpython-36.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_centralized.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_centralized.cpython-36.pyc
index f398e784201849b2ada4d3786ab8e9f91f15f080..4fb83e19970af4b54c068338068033b446c1d1c9 100644
Binary files a/CoLo-AT/robots/__pycache__/robot_centralized.cpython-36.pyc and b/CoLo-AT/robots/__pycache__/robot_centralized.cpython-36.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_distributive.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_distributive.cpython-36.pyc
index da90b600c29a7f15a8efa73b8285c7ec01ba6880..5bb557ca470f7cead2cc371006431c89563455b5 100644
Binary files a/CoLo-AT/robots/__pycache__/robot_distributive.cpython-36.pyc and b/CoLo-AT/robots/__pycache__/robot_distributive.cpython-36.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_parameters.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_parameters.cpython-36.pyc
index 5c0a10a0b85351858e365c063ea53d687f40cbba..f3b63698f4f2e7abe941a8abd88bd6650ccf3129 100644
Binary files a/CoLo-AT/robots/__pycache__/robot_parameters.cpython-36.pyc and b/CoLo-AT/robots/__pycache__/robot_parameters.cpython-36.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..a3749f65089c47a4d5adccc0f77e5a142a6440b3 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__/__init__.cpython-36.pyc b/CoLo-AT/simulation_process/__pycache__/__init__.cpython-36.pyc
index 9911a68bb6d5909124222a8960179c2e0a77f33f..69554d904b9048111013c5e978a58d4a14d3ee19 100644
Binary files a/CoLo-AT/simulation_process/__pycache__/__init__.cpython-36.pyc and b/CoLo-AT/simulation_process/__pycache__/__init__.cpython-36.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__/sim_manager.cpython-36.pyc b/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc
index 2abcfecd1515e8497a8c7f92566c428ba0bfdf5e..16eb67c30e1e3d7f4b2e61a5c101b65f43765efa 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 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..85c0e77429d59df4555496f0556e72434261e905 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..9a40f1473d162d7d86538578fb72de231307a2c4 100644
--- a/CoLo-AT/simulation_process/state_recorder.py
+++ b/CoLo-AT/simulation_process/state_recorder.py
@@ -10,9 +10,10 @@ import matplotlib.pyplot as plt
class StatesRecorder():
"""docstring for ClassName"""
- def __init__(self, name, dataset_labels):
+ def __init__(self, name, dataset_labels, state_var_only = False ):
self.name = name
self.dataset_labels = dataset_labels
+ self.state_var_only = state_var_only
self.recorded_data = {}
self.data_in_time_order = []
self.updata_type_in_time_order = []
@@ -41,6 +42,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
@@ -58,9 +63,14 @@ class StatesRecorder():
gt_x_pos = gt['x_pos']
gt_y_pos = gt['y_pos']
robot_label = self.dataset_labels[robot_idx]
- 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 self.state_var_only:
+ loc_err = 0
+ recorded_dataline = [time, robot_label, gt_x_pos, gt_y_pos, trace_state_var, gt_x_pos, gt_y_pos, loc_err]
+ else:
+ 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)
@@ -72,7 +82,7 @@ class StatesRecorder():
print(updata_type)
print('>1 m loc err: ',recorded_dataline)
print(req.get_message())
-
+
#print(recorded_dataline)
self.data_in_time_order.append(recorded_dataline)
self.updata_type_in_time_order.append(updata_type)