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 71e848bdc21f0e4085fd1f8b06af55246286a44c..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/data_analyzer.py b/CoLo-AT/data_analysis/data_analyzer.py
index b5be8b81c25eab0ac2cd168e99e382477eaebf6f..f608879cdffc056f76489078cb7a4b1faa586bda 100644
--- a/CoLo-AT/data_analysis/data_analyzer.py
+++ b/CoLo-AT/data_analysis/data_analyzer.py
@@ -26,6 +26,8 @@ class Analyzer():
self.dataset_labels = dataset_labels
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)
@@ -34,8 +36,9 @@ class Analyzer():
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)
-
+ #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]')
@@ -78,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 = []
@@ -86,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:
@@ -96,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]
@@ -116,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
@@ -126,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, recorder_name = recorder_name)
+ 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
@@ -222,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/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/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__/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
index 83bf5d28309a25e87b1bc794f5adbd1a8d27d0d4..0011e92cb63c4fc5e20457ee0cf17255617a39e5 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-36.pyc differ
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 21f007c61988f990f1272db1bae82861295e9249..da3a82097d2c3bd74131201efc44ea3827bb96f0 100644
--- a/CoLo-AT/localization_algos/gs_ci_bound.py
+++ b/CoLo-AT/localization_algos/gs_ci_bound.py
@@ -12,7 +12,7 @@ class GS_CI_Bound(ekf_algo_framework):
self.d_max = 2 # max measurement distance
self.d_var = 0.2
self.bearing_var = 0.2
- self.var_v = 0.2
+ self.var_v = 0.1
def state_variance_init(self, num_robots):
return 0.01*np.matrix(np.identity(2*num_robots), dtype = float)