diff --git a/CoLo-AT/Algorithm Comparison.pdf b/CoLo-AT/Algorithm Comparison.pdf index ecf54c22f8094da499d2356a5eb9472b84c1e26d..be8592ab63d020456462f6a207fa78bf16221817 100644 Binary files a/CoLo-AT/Algorithm Comparison.pdf and b/CoLo-AT/Algorithm Comparison.pdf 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 00026763e21aa8bcb83c2edf9f8367b7c06a0654..7c702c1fe6559289fc6ad2a6503301457bd71125 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 a68e60d46dae216d9bf2ed275fbb33b57ad6028d..d1b233b6d10c97d17fc37a6a3cf43cbac14e2116 100644 --- a/CoLo-AT/data_analysis/data_analyzer.py +++ b/CoLo-AT/data_analysis/data_analyzer.py @@ -91,14 +91,19 @@ class Analyzer(): finish_flag = False loc_err_per_run = [] + state_err_per_run = [] + trace_per_run = [] + + lm_measurement_num = [] relative_measurment_num = [] comm_num = [] - trace_per_run = [] + time_arr = [] while True: loc_err_per_time_iterval = 0 + state_err_per_time_interval = 0 trace_per_time_iterval = 0 num_dataline_per_time_iterval = 0 lm_measurement_count = 0 @@ -123,7 +128,8 @@ class Analyzer(): if selected_labels == None or data_in_time_order[time_index][1] in selected_labels: - loc_err_per_time_iterval+= data_in_time_order[time_index][7] + loc_err_per_time_iterval += data_in_time_order[time_index][7] + state_err_per_time_interval += np.linalg.norm(data_in_time_order[time_index][8] - data_in_time_order[time_index][9]) trace_per_time_iterval += data_in_time_order[time_index][4] num_dataline_per_time_iterval+=1 @@ -134,10 +140,12 @@ class Analyzer(): if num_dataline_per_time_iterval != 0: loc_err_per_run.append(loc_err_per_time_iterval/num_dataline_per_time_iterval) + state_err_per_run.append(state_err_per_time_interval/num_dataline_per_time_iterval) trace_per_run.append(trace_per_time_iterval/num_dataline_per_time_iterval) else: loc_err_per_run.append(0) + state_err_per_run.append(0) trace_per_run.append(0) lm_measurement_num.append(lm_measurement_count) @@ -155,7 +163,7 @@ class Analyzer(): if plot_graphs: self.plot_loc_err_and_trace(loc_err_per_run, trace_per_run, time_arr, operations_distr = [lm_measurement_num, relative_measurment_num, comm_num], recorder_name = recorder_name) - return loc_err_per_run, trace_per_run, time_arr + return loc_err_per_run, state_err_per_run, trace_per_run, time_arr def robot_location_at_unit_time_interval(self, data_recorder, unit_time_interval = 0.2): #recorded_dataline = [time, robot_label, est_x_pos, est_y_pos, trace_state_var, gt_x_pos, gt_y_pos, loc_err] @@ -262,9 +270,9 @@ 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 = False, selected_labels = selected_labels) + loc_err_per_run, state_err_per_run, trace_per_run, time_stamps = self.calculate_loc_err_and_trace_state_variance_per_run(data_recorder, plot_graphs = False, selected_labels = selected_labels) 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_loc_err.append([time_stamps, state_err_per_run, data_recorder.name]) arr_trace.append([time_stamps, trace_per_run, data_recorder.name] ) print('Plotting Comparison Graphs') diff --git a/CoLo-AT/dataset_manager/__pycache__/simulated_dataset_manager_w.cpython-36.pyc b/CoLo-AT/dataset_manager/__pycache__/simulated_dataset_manager_w.cpython-36.pyc index 381de36ccc1f4eb7c7dd8ef1ec3fc445ea46b36d..35c5604c197dbd43ba397d71f598ccbad8207c35 100644 Binary files a/CoLo-AT/dataset_manager/__pycache__/simulated_dataset_manager_w.cpython-36.pyc and b/CoLo-AT/dataset_manager/__pycache__/simulated_dataset_manager_w.cpython-36.pyc differ diff --git a/CoLo-AT/gs_ci_journal_sim.py b/CoLo-AT/gs_ci_journal_sim.py index 9769bc128971ac02919e36ca4e946e28d93ff1b3..9d4b5c4d9341091d1b8594dc99f8c602ca94197e 100644 --- a/CoLo-AT/gs_ci_journal_sim.py +++ b/CoLo-AT/gs_ci_journal_sim.py @@ -27,7 +27,7 @@ from ekf_gs_sci2 import EKF_GS_SCI2 ############################################################################## -duration = 200 +duration = 100 robot_labels = [1,2,3,4] testing_dataset = Sim_Dataset_Manager('testing') start_time, starting_states, dataset_data, time_arr = testing_dataset.circular_path_4_robots(duration) @@ -40,7 +40,7 @@ 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 = False, selected_labels = [1]) +loc_err_per_run, state_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = False, selected_labels = [1]) #analyzer.trajectory_plot(state_recorder) @@ -55,10 +55,9 @@ 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_b, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_bound, plot_graphs = False, selected_labels = [1]) +loc_err_per_run, state_err_per_run, trace_per_run_b, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_bound, plot_graphs = False, selected_labels = [1]) ############################################################################## - testing_dataset.dataset_reset() loc_algo = Centralized_EKF('algo') robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False) @@ -66,7 +65,7 @@ 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 = False) +loc_err_per_run, state_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_cen, plot_graphs = False) ############################################################################## @@ -77,7 +76,7 @@ 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 = False) +loc_err_per_run, state_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_BDA, plot_graphs = False) ############################################################################## @@ -89,7 +88,7 @@ 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 = False) +loc_err_per_run, state_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_CI, plot_graphs = False) ############################################################################## @@ -100,6 +99,8 @@ 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 = False) +loc_err_per_run, state_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_GS_SCI, plot_graphs = False) + +analyzer.algos_comparison([state_recorder, state_recorder_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI, state_recorder_bound], only_trace = ['gs_ci_bound'], selected_labels = [1]) + -analyzer.algos_comparison([state_recorder, state_recorder_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI, state_recorder_bound], only_trace = ['gs_ci_bound'], selected_labels = [1]) \ No newline at end of file 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 bac96246273de72566a102ee4ecc5854305bbdd9..a02f049cf7ac54d49143049b5fe2c2769f6fa360 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 df5af6c196922ce05dc1069d82fc707493feac86..ba0a8a86ae49dfa3867677edda712eb20893b78f 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__/gs_ci_bound.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-36.pyc index ae10aa7792e6ae1c7580c28e4258839aa5d64c59..607a82df754796bfe76937e000767780fb5e9098 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_gs_bound.py b/CoLo-AT/localization_algos/ekf_gs_bound.py index fe91db4da8083b8dc1da422d1765a0e39196a3b5..320691a48391d1d1b93fe89963aa49d7a8f4aaa1 100644 --- a/CoLo-AT/localization_algos/ekf_gs_bound.py +++ b/CoLo-AT/localization_algos/ekf_gs_bound.py @@ -10,10 +10,10 @@ def rot_mtx(theta): class EKF_GS_BOUND(ekf_algo_framework): def __init__(self, algo_name): ekf_algo_framework.__init__(self, algo_name) - self.var_v = 0.04 - self.var_range = 0.7 - self.var_bearing = 0.3 - self.max_range = 3.5 + self.var_v = 0.1**2 + self.var_range = 0.1**2 + self.var_bearing = 0.3**2 + self.max_range = 0.001 def state_variance_init(self, num_robots): return 0.1*np.matrix(np.identity(2*num_robots), dtype = float) @@ -22,7 +22,7 @@ class EKF_GS_BOUND(ekf_algo_framework): [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 + return np.trace(sigma_s) def propagation_update(self, robot_data, sensor_data): [s, orinetations, sigma_s, index] = robot_data @@ -49,6 +49,7 @@ class EKF_GS_BOUND(ekf_algo_framework): jj = 2*j sigma_s[jj:jj+2, jj:jj+2] += delta_t*delta_t*var_v*np.identity(2) + print(np.trace(sigma_s)) return [s, orinetations, sigma_s] def absolute_obser_update(self, robot_data, sensor_data): @@ -71,13 +72,13 @@ class EKF_GS_BOUND(ekf_algo_framework): H = rot_mtx(self_theta).getT()*H_i var_range = self.var_range - var_range = self.var_range - range_max = self.range_max + max_range = self.max_range var_bearing = self.var_bearing - R = np.matrix(max(var_range, range_max, var_bearing)*np.identity(2)) + R = np.matrix(max(var_range, max_range**2*var_bearing)*np.identity(2)) sigma_s = (sigma_s.getI()+H.getT()*R.getI()*H).getI() + return [s, orinetations, sigma_s] diff --git a/CoLo-AT/localization_algos/ekf_gs_ci2.py b/CoLo-AT/localization_algos/ekf_gs_ci2.py index 2adbad7ba45ad7ce4e03bc59ace54ee46803113b..fd399d232e4f791a519343d5c95e3e4bdaaef87e 100644 --- a/CoLo-AT/localization_algos/ekf_gs_ci2.py +++ b/CoLo-AT/localization_algos/ekf_gs_ci2.py @@ -38,7 +38,7 @@ class EKF_GS_CI2(ekf_algo_framework): self_theta = orinetations[index] Q = sigma_odo - Q[1,1] = Q[1,1]*v*v + #Q[1,1] = Q[1,1]*v*v# W = delta_t*rot_mtx(self_theta) s[i,0] = s[i,0] + cos(self_theta)*v*delta_t #x diff --git a/CoLo-AT/localization_algos/gs_ci_bound.py b/CoLo-AT/localization_algos/gs_ci_bound.py index 44f08048b41efe65509bdaaef92c4fdef21f60b2..d3d7fbf1835d7b9d0000d93aa3490dadc11ab28c 100644 --- a/CoLo-AT/localization_algos/gs_ci_bound.py +++ b/CoLo-AT/localization_algos/gs_ci_bound.py @@ -9,10 +9,11 @@ def rot_mtx(theta): class GS_CI_Bound(ekf_algo_framework): def __init__(self, algo_name): self.algo_name = algo_name - self.d_max = 2 # max measurement distance - self.d_var = 0.2 - self.bearing_var = 0.2 - self.var_v = 0.25 + + self.var_v = 0.25**2 + self.var_range = 0.3**2 + self.var_bearing = 0.3**2 + self.max_range = 1.5 def state_variance_init(self, num_robots): return 0.04*np.matrix(np.identity(2*num_robots), dtype = float) @@ -37,10 +38,6 @@ class GS_CI_Bound(ekf_algo_framework): for j in range(num_robots): jj = 2*j - if j==index: - 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 sigma_s[jj:jj+2, jj:jj+2] += var_v*np.identity(2)*delta_t*delta_t @@ -60,11 +57,11 @@ class GS_CI_Bound(ekf_algo_framework): H_i[0, i] = -1 H_i[1, i+1] = -1 - d_max = self.d_max - var_dis = self.d_var - var_phi = self.bearing_var + max_range = self.max_range + var_dis = self.var_range + var_phi = self.var_bearing - sigma_th_z = np.matrix(max(var_dis, d_max*d_max*var_phi)*np.identity(2)) + sigma_th_z = np.matrix(max(var_dis, max_range*max_range*var_phi)*np.identity(2)) sigma_s = (sigma_s.getI() + H_i.getT() * sigma_th_z.getI() * H_i).getI() return [s, orinetations, sigma_s] @@ -90,11 +87,11 @@ class GS_CI_Bound(ekf_algo_framework): H_ij[0, j] = 1 H_ij[1, j+1] = 1 - d_max = self.d_max - var_dis = self.d_var - var_phi = self.bearing_var + max_range = self.max_range + var_dis = self.var_range + var_phi = self.var_bearing - sigma_th_z = np.matrix(max(var_dis, d_max*d_max*var_phi)*np.identity(2)) + sigma_th_z = np.matrix(max(var_dis, max_range*max_range*var_phi)*np.identity(2)) sigma_s = (sigma_s.getI() + H_ij.getT() * sigma_th_z.getI() * H_ij).getI() return [s, orinetations, sigma_s] diff --git a/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc index e6f893b9bb34363e909e83cc8442b5731bb39e34..1fd2769d85d4c09a6c7592f51c8ff5792061162a 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/robots/robot_system.py b/CoLo-AT/robots/robot_system.py index 075be13efdc5fd156693047456735fc42c3ce749..bea373646609bb56e5ae55314ec53fae447a4576 100644 --- a/CoLo-AT/robots/robot_system.py +++ b/CoLo-AT/robots/robot_system.py @@ -31,7 +31,7 @@ class RobotSystem: else: self.robot_sys = CentralizedRobotSystem(name, robot_labels, loc_algo) print('created a centralized system for ', self.num_robots, ' robots ') - self.gt_orientations = np.zeros(self.num_robots) + def set_starting_state(self, start_state_arr): if self.distr_sys: @@ -40,6 +40,11 @@ class RobotSystem: else: self.robot_sys.set_starting_state(start_state_arr) + + self.gt_orientations = np.zeros(self.num_robots) + for i, label in enumerate(self.robot_labels): + self.gt_orientations[i] = start_state_arr[label][3] + def request_data(self, time): pass ''' @@ -82,7 +87,7 @@ class RobotSystem: a_v = message_data ['angular velocity'] #sigma_odo = np.matrix([[5.075*v, 0], [0, 0.1]]) #with respect to velocity and orientation for utias datasets - sigma_odo = np.matrix([[0.01, 0], [0, 0.1]]) #with respect to velocity and orientation + sigma_odo = np.matrix([[0.01, 0], [0, 0.01]]) #with respect to velocity and orientation sensor_covariance = sigma_odo delta_t = message_data['delta_t'] @@ -98,7 +103,7 @@ class RobotSystem: elif rsp_type == 'measurement': - sigma_obser = np.matrix([[0.015, 0], [0,0.01]]) #with respect to range and bearing + sigma_obser = np.matrix([[0.1**2, 0], [0, 0.035**2]]) #with respect to range and bearing sensor_covariance = sigma_obser obj_id = message_data['subject_ID'] meas_range = message_data['measurment_range'] @@ -155,5 +160,5 @@ class RobotSystem: [est_states, est_orientaions, est_state_variance]= self.robot_sys.get_status() trace_sigma_s = self.robot_sys.get_trace_state_variance(robot_index) - robot_state = {'x_pos':est_states[2*robot_index], 'y_pos': est_states[2*robot_index+1], 'trace of state variance': trace_sigma_s, 'state variance': est_state_variance, 'update_type': update_type } + robot_state = {'x_pos':est_states[2*robot_index], 'y_pos': est_states[2*robot_index+1], 'trace of state variance': trace_sigma_s, 'state variance': est_state_variance, 'update_type': update_type, 'est_states': est_states.copy() } return robot_state 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 8406071719767482050a4be41f2168eaa7139445..1450bcf5f5e286f03e7a2161ce1cbf2616342420 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-36.pyc b/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-36.pyc index 7d0b96af6d2bd113c042393330c80a3848183572..fecf62bb365b60850ecd554432651b7f9690698c 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 97a6696c01f40786d2c831038387942ced76e1a9..ad6a540a861e827f64668cd5e392325ef8ea9a22 100644 --- a/CoLo-AT/simulation_process/state_recorder.py +++ b/CoLo-AT/simulation_process/state_recorder.py @@ -21,6 +21,7 @@ class StatesRecorder(): self.loc_err_arr = {} self.trace_sigma_s_arr = {} self.updata_type_arr = {} + self.gt_states = np.matrix(np.zeros((2*self.num_robots,1))) for i, label in enumerate(self.dataset_labels): self.recorded_data[label]=[] self.loc_err_arr[label]=[] @@ -28,6 +29,10 @@ class StatesRecorder(): self.updata_type_arr[label]=[] def set_starting_state(self, stating_states): + for i, label in enumerate(self.dataset_labels): + self.gt_states[2*i,0] = stating_states[label][1] + self.gt_states[2*i+1,0] = stating_states[label][2] + for i, label in enumerate(self.dataset_labels): self.start_time = stating_states[label][0] time = 0 @@ -35,7 +40,7 @@ class StatesRecorder(): y_pos = stating_states[label][2] initial_trace_state_var = 0.0001 loc_err = 0 - recorded_dataline = [time, label, x_pos, y_pos, initial_trace_state_var, x_pos, y_pos, loc_err] + recorded_dataline = [time, label, x_pos, y_pos, initial_trace_state_var, x_pos, y_pos, loc_err, self.gt_states.copy(), self.gt_states.copy()] self.data_in_time_order.append(recorded_dataline) self.recorded_data[label].append(recorded_dataline) self.loc_err_arr[label].append(0) @@ -60,17 +65,20 @@ class StatesRecorder(): est_y_pos = float(robot_state['y_pos']) trace_state_var = robot_state['trace of state variance'] updata_type = robot_state['update_type'] + est_states = robot_state['est_states'].copy() gt_x_pos = gt['x_pos'] gt_y_pos = gt['y_pos'] robot_label = self.dataset_labels[robot_idx] + self.gt_states[2*robot_idx,0] = gt_x_pos + self.gt_states[2*robot_idx+1,0] = gt_y_pos 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] + recorded_dataline = [time, robot_label, gt_x_pos, gt_y_pos, trace_state_var, gt_x_pos, gt_y_pos, loc_err, self.gt_states.copy(), self.gt_states.copy()] 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] + recorded_dataline = [time, robot_label, est_x_pos, est_y_pos, trace_state_var, gt_x_pos, gt_y_pos, loc_err, est_states, self.gt_states.copy()] #warning (optional) ''' @@ -94,6 +102,7 @@ class StatesRecorder(): self.loc_err_arr[robot_label].append(loc_err) self.trace_sigma_s_arr[robot_label].append(trace_state_var) self.updata_type_arr[robot_label].append(updata_type) + def get_data_in_time_order(self):