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 56fb8249090c574284edf5a20dfca7d00694b64c..c1276daac2b6a95b0506869361272989136596e3 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 3d9551659bc133328d33f6bc01e68622869d0d57..60ef9783aab84de2bb519a65425e593e61c2875b 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 366ecb8d007190f9aa9ccc357bae5995364e12c0..d60b97bfd306ab8c036651f323b5266f1c646b1d 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 2eb2c313aa4cf5e5c32c5581e542feff4caac20b..a41061c0ab30325a0c99ab3f99cdfe4b831a25ca 100644
--- a/CoLo-AT/data_analysis/data_analyzer.py
+++ b/CoLo-AT/data_analysis/data_analyzer.py
@@ -217,8 +217,8 @@ class Analyzer():
arr_loc_err = []
arr_trace = []
for data_recorder in arr_data_recorder:
- loc_err_per_run, trace_per_run, time_stamps = self.calculate_loc_err_and_trace_state_variance_per_run(data_recorder, plot_graphs = True)
- if data_recorder.name not in only_trace:
+ loc_err_per_run, trace_per_run, time_stamps = self.calculate_loc_err_and_trace_state_variance_per_run(data_recorder, plot_graphs = False)
+ if only_trace ==None or data_recorder.name not in only_trace:
arr_loc_err.append([time_stamps, loc_err_per_run, data_recorder.name])
arr_trace.append([time_stamps, trace_per_run, data_recorder.name] )
diff --git a/CoLo-AT/data_analysis/realtime_plot.py b/CoLo-AT/data_analysis/realtime_plot.py
index 436e6fc2e0821c756869e0bf6c5b1dca9a390aca..4ae8f2cd8a662c8a409dd8c3a8b732ee9ebc80b2 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=12)
+ tmp = fig.text(0.8, 0.03, "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']
@@ -157,7 +157,7 @@ def animate_plot(dataset_labels, data_recorder, analyzer, lm = None):
fig = plt.figure(figsize=(12, 9), tight_layout=True)
fig.subplots_adjust(top=0.88)
gs = gridspec.GridSpec(3, 2)
- fig.suptitle('CoLo Robot Demo Simulation', fontsize=14)
+ fig.suptitle('CoLo Robot Demo Simulation', fontsize=20)
ax_err = plt.subplot(gs[0, 0])
ax_var = plt.subplot(gs[0, 1])
ax = plt.subplot(gs[1:, :])
@@ -169,15 +169,16 @@ def animate_plot(dataset_labels, data_recorder, analyzer, lm = None):
#ax = plt.axes(xlim=(-6, 6), ylim=(-6, 8))
#ax_err = plt.axes(xlim=(0, 100), ylim=(0, 0.4))
# assign labels
- ax.set_title('CoLo Demo')
- ax.set_xlabel('x-axis[m]')
- ax.set_ylabel('y-axis[m]')
- ax_err.set_title('Estimation deviation error')
- ax_err.set_xlabel('Time[s]')
- ax_err.set_ylabel('RMS[m]')
- ax_var.set_title('Trace of state variance')
- ax_var.set_xlabel('Time [s]')
- ax_var.set_ylabel('Sigma_s [m^2]')
+ ax.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_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')
@@ -208,10 +209,12 @@ def animate_plot(dataset_labels, data_recorder, analyzer, lm = None):
ani = animation.FuncAnimation(fig, update, fargs = (num_robots, fig, init_path_est, init_path_gt, init_points_est, init_points_gt, robot_locations_est, robot_locations_gt, times, time_func, min_time_arr, arr_err, arr_var, loc_err, trace_sigma), frames=min_length(times), interval=50, blit=False)
# Show legend
fontP = FontProperties()
- fontP.set_size('small')
- ax.legend(prop=fontP, bbox_to_anchor=(1.1, 1.0), loc=9, ncol=1)
- ax_var.legend(prop=fontP, bbox_to_anchor=(1.2, 1.0), loc=9, ncol=1)
+ fontP.set_size('x-large')
+ ax.tick_params(axis='both', which='major', size=15)
+
+ ax.tick_params(labelsize=10)
+
# 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('full_test9.gif', writer="imagemagick", fps=60)
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 79b36539455ad2d99eb5288bc0f185b944a12294..b4fff0660725a669b0823d3b42ca96f078fc881a 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 245117c2ddb56596e4bd7f17684682cc5c3cdcb6..01028dfc1c335bd983fe79c2cb127311487cdcc4 100644
--- a/CoLo-AT/simulation_process/state_recorder.py
+++ b/CoLo-AT/simulation_process/state_recorder.py
@@ -68,7 +68,7 @@ class StatesRecorder():
print('neg trace: ', recorded_dataline)
- if(loc_err >= 0.5):
+ if(loc_err >= 1):
print(updata_type)
print('>1 m loc err: ',recorded_dataline)
print(req.get_message())
diff --git a/CoLo-AT/test_simulation.py b/CoLo-AT/test_simulation.py
index 97c4c37388a5056df0276c6eea66605f3e943709..79192ed5f32b564e1eb3a9dc2a983eff02ddb669 100644
--- a/CoLo-AT/test_simulation.py
+++ b/CoLo-AT/test_simulation.py
@@ -30,67 +30,29 @@ from ekf_gs_ci2 import EKF_GS_CI2
dataset_path = '/home/william/UTIAS-dataset/MRCLAM_Dataset3/'
dataset_labels = [1,2,3]
-duration = 150 # duration for the simulation in sec
+duration = 300 # 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)
+analyzer = Analyzer('analyzer', dataset_labels)
-freqs0 = [[10, 10, 10],[1, 1, 1],[0.5, 0.5, 0.5]]
-freqs1 = [[10, 10, 10],[4, 4, 4],[0.5, 0.5, 0.5]]
+start_time, starting_states, dataset_data, time_arr = testing_dataset.load_MRCLAMDatasets(dataset_path, dataset_labels, duration)
loc_algo = EKF_GS_CI2('algo')
robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = False)
-sim = SimulationManager('sim EKF_GS_CI2')
-state_recorder = StatesRecorder('recorder',dataset_labels)
-sim.sim_process_schedule(dataset_labels, testing_dataset, robot, state_recorder, freqs0, simple_plot = True)
-#sim.sim_process_naive(dataset_labels, testing_dataset, robot, state_recorder, simple_plot = True)
-
-analyzer = Analyzer('analyzer', dataset_labels)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = False)
-robot_loc_time_unit = analyzer.robot_location_at_unit_time_interval(state_recorder)
-data_in_time_order = state_recorder.get_data_in_time_order()
-update_in_time_order = state_recorder.get_updata_type_in_time_order()
-
-'''
+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)
##############################################################################
-testing_dataset.dataset_reset()
-robot = RobotSystem('robot gs ci', dataset_labels, loc_algo, distr_sys = True)
-
-sim1 = SimulationManager('sim')
-state_recorder1 = StatesRecorder('gs ci schedule freq1',dataset_labels)
-sim1.sim_process_schedule(dataset_labels, testing_dataset, robot, state_recorder1, freqs1, simple_plot = True)
-##############################################################################
testing_dataset.dataset_reset()
-robot = RobotSystem('robot gs ci', dataset_labels, loc_algo, distr_sys = True)
-
-sim_n = SimulationManager('sim')
-state_recorder_n = StatesRecorder('gs ci naive ',dataset_labels)
-sim_n.sim_process_naive(dataset_labels, testing_dataset, robot, state_recorder_n, simple_plot = True)
-
-analyzer = Analyzer('analyzer', dataset_labels)
-analyzer.algos_comparison([state_recorder, state_recorder1, state_recorder_n], only_trace=['gs ci bound'])
-print('done!')
-'''
-'''
-sim.sim_process_naive(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)
-#robot_location_at_unit_time_interval
-robot_loc_time_unit = analyzer.robot_location_at_unit_time_interval(state_recorder)
+loc_algo = Centralized_EKF('algo')
+robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = True)
+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)
-# simulated data
-data_sim = state_recorder.get_recorded_data()
-le = state_recorder.get_loc_err_arr()
-ud = state_recorder.get_update_type_arr()
-t_S = state_recorder.get_trace_sigma_s_arr()
-ud_t = state_recorder.get_updata_type_in_time_order()
-data_sim_t = state_recorder.get_data_in_time_order()
-get_robot_time_func = state_recorder.get_time_arr
-# le, t_S is an array of 5 elements, each element has error, state variance
-# based on time stamp
-# print(len(t_S[1]), len(t_S[2]), len(t_S[3]), len(t_S[4]), len(t_S[5]))
-# print(data_sim)
-'''
+analyzer.algos_comparison([state_recorder, state_recorder_n])
-#animate_plot(dataset_labels, state_recorder, analyzer, get_robot_time_func, le, t_S)