diff --git a/CoLo-AT/dataset_manager/__pycache__/existing_dataset.cpython-36.pyc b/CoLo-AT/dataset_manager/__pycache__/existing_dataset.cpython-36.pyc
index 3c4964b7c5fa18e5c4e54efd6d80b0a0f47841d3..67e140af40210d6d0afaeb1ef2c0b4393be92044 100644
Binary files a/CoLo-AT/dataset_manager/__pycache__/existing_dataset.cpython-36.pyc and b/CoLo-AT/dataset_manager/__pycache__/existing_dataset.cpython-36.pyc differ
diff --git a/CoLo-AT/dataset_manager/existing_dataset.py b/CoLo-AT/dataset_manager/existing_dataset.py
index ec3fc68c7b5fcb9569e077ba48ede319c77a666c..966391e0bba272748ed330c4dbca0e4aaf25ba56 100755
--- a/CoLo-AT/dataset_manager/existing_dataset.py
+++ b/CoLo-AT/dataset_manager/existing_dataset.py
@@ -351,10 +351,14 @@ class Dataset:
v0 = self.dataset_data[req_type][robot_idx][prev_time_idx]['velocity']
v1 = self.dataset_data[req_type][robot_idx][post_time_idx]['velocity']
velocity = linear_interpolation([t0, v0], [t1, v1], req_time)
+ a_v0 = self.dataset_data[req_type][robot_idx][prev_time_idx]['angular velocity']
+ a_v1 = self.dataset_data[req_type][robot_idx][post_time_idx]['angular velocity']
+ a_v = linear_interpolation([t0, a_v0], [t1, a_v1], req_time)
o0 = self.dataset_data[req_type][robot_idx][prev_time_idx]['orientation']
o1 = self.dataset_data[req_type][robot_idx][post_time_idx]['orientation']
orientation = linear_interpolation([t0, o0], [t1, o1], req_time)
- message['data'] = {'time':req_time, 'velocity':velocity, 'orientation':orientation}
+
+ message['data'] = {'time':req_time, 'velocity':velocity, 'angular velocity': a_v, 'orientation':orientation, 'delta_t': 0}
else: #measurement: meas_info = {'time':time, 'subject_ID':subject_ID, 'measurment_range': measurment_range, 'bearing':bearing}
pass
@@ -375,8 +379,8 @@ class Dataset:
lx = matched_gt_data['x_pos']
ly = matched_gt_data['y_pos']
+ np.random.seed(6)
measurment_range = sqrt((lx-gt_x)*(lx-gt_x)+(ly-gt_y)*(ly-gt_y)) + float(np.random.normal(0, sqrt(var_dis), 1))
- # bearing not matching with closest dataline....
bearing = (atan2((ly-gt_y), (lx-gt_x))-gt_orientation)%pi + float(np.random.normal(0, sqrt(var_phi), 1))
if abs(bearing-pi) < abs(bearing):
bearing = bearing-pi
@@ -394,13 +398,8 @@ class Dataset:
message['data'] = self.dataset_data[req_type][robot_idx][time_idx]
message['groundtruth'] = self.dataset_matched_gt_data[req_type][robot_idx][time_idx]
return message
-
- def create_additional_dataline(self, req, time_idx, meas_input_var):
- message = self.create_synthetic_dataline(req, time_idx, meas_input_var)
- return message
-
- def get_dataline_at_specific_time(self, req, time_diff_limit = 0.1):
+ def get_dataline_at_specific_time(self, req, time_diff_limit = 0.2):
message = req.get_message()
req_type = req.get_type()
meas_input_var = [0.01, 0.01]
@@ -414,16 +413,9 @@ class Dataset:
gt_orientation = self.groundtruth_data[robot_idx][g_idx]['orientation']
message['groundtruth'] = {'time':req_time, 'x_pos':gt_x, 'y_pos':gt_y, 'orientation':gt_orientation}
if req_type == 'odometry':
- message['data'] = {'time':req_time, 'velocity':0 , 'orientation':gt_orientation + float(np.random.normal(0, sqrt(odo_input_var[1]), 1) )}
- else:
- subject_ID = 6
- [lx, ly] = self.landmark_map[subject_ID]
- measurment_range = sqrt((lx-gt_x)*(lx-gt_x)+(ly-gt_y)*(ly-gt_y)) + float(np.random.normal(0, sqrt(meas_input_var[0]), 1))
- bearing = (atan2((ly-gt_y), (lx-gt_x))-gt_orientation)%pi + float(np.random.normal(0, sqrt(meas_input_var[1]), 1))
- if abs(bearing-pi) < abs(bearing):
- bearing = bearing-pi
-
- message['data'] = {'time':req_time, 'subject_ID':subject_ID, 'measurment_range': measurment_range, 'bearing':bearing}
+ message['data'] = {'time':req_time, 'velocity':0 , 'angular velocity': 0, 'orientation':gt_orientation, 'delta_t': 0}
+ else:
+ message['data'] = {'time':req_time, 'subject_ID':None, 'measurment_range': 0, 'bearing':0}
valid_dataline = True
time_idx = 0
return valid_dataline, message, req_type, robot_idx, time_idx
@@ -436,9 +428,9 @@ class Dataset:
message['data'] = self.dataset_data[req_type][robot_idx][time_idx]
message['groundtruth'] = self.dataset_matched_gt_data[req_type][robot_idx][time_idx]
+
if abs(respond_time-req_time) > time_diff_limit and self.adding_actifical_dataline:
- message = self.create_additional_dataline(req, time_idx, meas_input_var)
-
+ message = self.create_synthetic_dataline(req, time_idx, meas_input_var)
if req_time > self.end_time:
valid_dataline = False
else:
diff --git a/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc
index 764c3acefb5a5748df2049ab1d5b37a629ec6b64..f804a9e8365eebc5c47729bdfb6824eaa49232ac 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 ae0849fd5c92ce9fd440fe7d14de067765d2b6a8..d1c9daf6b4dba9726b07a2ce830730088dcd7c8e 100644
--- a/CoLo-AT/robots/robot_system.py
+++ b/CoLo-AT/robots/robot_system.py
@@ -96,7 +96,7 @@ class RobotSystem:
obj_id = message_data['subject_ID']
meas_range = message_data['measurment_range']
bearing = message_data['bearing']
- if message_data['subject_ID'] > 5: #landmark observation
+ if message_data['subject_ID'] != None and message_data['subject_ID'] > 5: #landmark observation
update_type = 'landmark observation'
landmark_loc = self.landmark_map.get(obj_id)
if landmark_loc != None:
@@ -105,12 +105,13 @@ class RobotSystem:
else:
valid_update = False
else:
- update_type = 'relative observation'
if obj_id in self.robot_labels:
+ update_type = 'relative observation'
obser_index = self.robot_labels.index(obj_id)
valid_update = True
sensor_input = [obser_index, meas_range, bearing, message_data['time'], obj_id]
else:
+ update_type = 'invalid observation'
valid_update = False
sensor_input = None
@@ -125,6 +126,7 @@ class RobotSystem:
sensor_input = [sender_idx, comm_rbt_state, comm_rbt_state_variance]
sensor_covariance = np.matrix([[0.01, 0], [0, 0.01]])
else:
+
print('undefined message type!')
if valid_update:
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 6cfd0e6ece49a18042367124c5337d035c92bd4c..1f52de8ccdebd6de988d3d8bcff8fa717cd86d3d 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 8bd85f9ab45ec39fe2de3ed7f2669cf30395c845..2afa61a8e0fddd99c65d8befd44562410b9cfcd7 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/sim_manager.py b/CoLo-AT/simulation_process/sim_manager.py
index 4db03761e6bbbce42f9a891f8c542d9cfab33024..d6bd79daa035e79255ab2ae8becbc770f9f1268d 100644
--- a/CoLo-AT/simulation_process/sim_manager.py
+++ b/CoLo-AT/simulation_process/sim_manager.py
@@ -113,6 +113,12 @@ class SimulationManager():
if valid == False:
print('invalid responese')
break
+ message = rsp.get_message()
+ if rsp.get_type() == "odometry":
+ delta_t = 1/freqs[0][robot_idx]
+ message['data']['delta_t'] = delta_t
+ rsp.set_message(message)
+
robot_state = robot_system.localization_update(rsp)
state_recorder.record_state(rsp, robot_state)
@@ -137,7 +143,6 @@ class SimulationManager():
sys.quit('Time inconsistant!')
next_possible_time_array[req_type_idx][robot_idx] += 1/freqs[req_type_idx][robot_idx]
-
if simple_plot:
state_recorder.simple_plot(plot_title=self.name)
diff --git a/CoLo-AT/simulation_process/state_recorder.py b/CoLo-AT/simulation_process/state_recorder.py
index 559fb1d6a403a64103580c4e57a34e563588c2b1..b9ae1f5c6e3540019e45e4bb772921d9af1fbe84 100644
--- a/CoLo-AT/simulation_process/state_recorder.py
+++ b/CoLo-AT/simulation_process/state_recorder.py
@@ -67,7 +67,8 @@ class StatesRecorder():
print(updata_type)
print('neg trace: ', recorded_dataline)
- if(loc_err >= 1):
+
+ if(loc_err >= 0.5):
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 8080a85d4bcfd4c4bdfbce18fd284715bb0d2223..a990e7d84b25084961e612d7ebc9c0fb8ac2a97a 100644
--- a/CoLo-AT/test_simulation.py
+++ b/CoLo-AT/test_simulation.py
@@ -26,17 +26,11 @@ from ekf_ls_ci import EKF_LS_CI
from ekf_gs_bound import EKF_GS_BOUND
from ekf_gs_ci import EKF_GS_CI
-#need to verify these algo
-from ekf_gs_sci2 import EKF_GS_SCI2
-from ekf_ls_ci2 import EKF_LS_CI2
-from ekf_gs_ci2 import EKF_GS_CI2
-from centralized_ekf2 import Centralized_EKF2
-
dataset_path = '/home/william/UTIAS-dataset/MRCLAM_Dataset3/'
dataset_labels = [1,2,3]
-duration = 100 # duration for the simulation in sec
+duration = 150 # 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)
@@ -48,8 +42,8 @@ robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = False)
sim = SimulationManager('sim Centralized_EKF')
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)
+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)