diff --git a/CoLo-AT/Cooperative_Localization_Journal2_sim_data.py b/CoLo-AT/Cooperative_Localization_Journal2_sim_data.py
index 23cdf34d3d0ad05ac6d13c4b8fefb675769fe30d..f792db76d69d4a07aa18cbd2014d32d59af12294 100644
--- a/CoLo-AT/Cooperative_Localization_Journal2_sim_data.py
+++ b/CoLo-AT/Cooperative_Localization_Journal2_sim_data.py
@@ -8,7 +8,7 @@ Created on Thu May 16 15:41:51 2019
import os, sys
import getpass
-from math import pi
+from math import pi, sqrt
sys.path.append(os.path.join(os.path.dirname(__file__), "."))
from dataset_manager.realworld_dataset_manager import RW_Dataset_Manager
from dataset_manager.simulated_dataset_manager import SimulatedDataSetManager
@@ -29,13 +29,14 @@ from ekf_gs_ci2 import EKF_GS_CI2
from gs_ci_bound import GS_CI_Bound
from ekf_gs_sci2 import EKF_GS_SCI2
-landmarks = {10: [0,0] }
+landmarks = {10: [0,5] , 11: [20,3], 12: [-30,0], 13: [-13, -12] }
robot_labels = [1,2,3,4,5]
-boundary = 200
-duration = 30
+boundary = 60
+duration = 180
testing_dataset = SimulatedDataSetManager('testing', boundary, landmarks, duration, robot_labels,
-velocity_noise=0.0125, angular_velocity_noise=0.0, measurement_range_noise=0.1, bearing_noise=0.0349, robot_fov=2*pi)
+velocity_noise=sqrt(0.0125), angular_velocity_noise=0.001, measurement_range_noise=sqrt(0.1), bearing_noise=sqrt(2)*pi/180,
+robot_fov=2*pi, delta_t=0.2)
start_time, starting_states, dataset_data, time_arr = testing_dataset.simulate_dataset('random', test=False)
analyzer = Analyzer('analyzer', robot_labels)
diff --git a/CoLo-AT/dataset_manager/data_generator.py b/CoLo-AT/dataset_manager/data_generator.py
index 545703b429cb283d60a9256047e6ea279b49c11e..97985478045b95e08bfcbb7217857050cfc7c525 100644
--- a/CoLo-AT/dataset_manager/data_generator.py
+++ b/CoLo-AT/dataset_manager/data_generator.py
@@ -148,7 +148,7 @@ class DataGenerator():
# rotate some random number of radians between pi/3 and -pi/3
next_orientation = curr_orientation + np.random.uniform(-pi/3, pi/3)
next_orientation = self.converge_to_angle_range(next_orientation)
- rand_dist = 1 + np.random.normal(0, sqrt(0.25)) # units of distance change are in meters, if delta t is one second... v = m/s
+ rand_dist = (1 + np.random.normal(0, sqrt(0.25)))*self.delta_t # units of distance change are in meters, if delta t is one second... v = m/s
next_x = curr_x + rand_dist*cos(next_orientation)
next_y = curr_y + rand_dist*sin(next_orientation)
@@ -175,14 +175,17 @@ class DataGenerator():
for time_idx in range(1, len(self.time_arr['odometry'][i])):
loc1 = [self.groundtruth_data[i][time_idx-1]['x_pos'], self.groundtruth_data[i][time_idx-1]['y_pos']]
loc2 = [self.groundtruth_data[i][time_idx]['x_pos'], self.groundtruth_data[i][time_idx]['y_pos']]
- velocity = self.calc_distance(loc1, loc2)/self.delta_t + np.random.normal(loc=0.0,scale=self.velocity_noise)
+ t1 = self.groundtruth_data[i][time_idx-1]['time']
+ t2 = self.groundtruth_data[i][time_idx]['time']
+
+ velocity = self.calc_distance(loc1, loc2)/(t2-t1) + np.random.normal(loc=0.0,scale=self.velocity_noise)
theta_1 = self.groundtruth_data[i][time_idx-1]['orientation']
theta_2 = self.groundtruth_data[i][time_idx]['orientation']
- angular_velocity = (theta_2-theta_1)/self.delta_t + np.random.normal(loc=0.0,scale=self.angular_velocity_noise)
+ angular_velocity = (theta_2-theta_1)/(t2-t1) + np.random.normal(loc=0.0,scale=self.angular_velocity_noise)
self.odometry_data[i].append({'time': self.time_arr['odometry'][i][time_idx], 'velocity' : velocity, 'angular velocity' : angular_velocity,
- 'orientation' : self.groundtruth_data[i][time_idx]['orientation'], 'delta_t' : self.delta_t})
+ 'orientation' : self.groundtruth_data[i][time_idx]['orientation'], 'delta_t' : (t2-t1)})
def generate_measurement_data(self):
@@ -199,16 +202,16 @@ class DataGenerator():
robot_orientation = self.groundtruth_data[robot_idx][time_idx]['orientation']
robot_loc = [robot_loc_x, robot_loc_y]
- # Relative (other robots) observations
+ # Relative (other robots) observations
other_robot_locs = {label : [self.groundtruth_data[i][time_idx]['x_pos'], self.groundtruth_data[i][time_idx]['y_pos']] for i, label in enumerate(self.robot_labels, 0) if i != robot_idx }
- for robotID, other_robot_loc in other_robot_locs.items():
+ for robotidx, other_robot_loc in other_robot_locs.items():
if (self.within_vision(self.robot_fov, robot_loc, other_robot_loc, robot_orientation)):
measurement_range = self.calc_distance(robot_loc, other_robot_loc) + np.random.normal(loc=0.0,scale=self.measurement_range_noise)
bearing = atan2((other_robot_loc[1]-robot_loc[1]), (other_robot_loc[0]-robot_loc[0])) + np.random.normal(loc=0.0,scale=self.bearing_noise) # global coordinates
bearing = bearing - robot_orientation # NOTE - local = global - orientation, all between 0 and 2*pi
bearing = self.converge_to_angle_range(bearing)
- self.measurement_data[robot_idx].append({'time': time, 'subject_ID': robotID, 'measurment_range':measurement_range, 'bearing':bearing})
+ self.measurement_data[robot_idx].append({'time': time, 'subject_ID': self.robot_labels[robotidx], 'measurment_range':measurement_range, 'bearing':bearing})
time_arr.append(time) # we expect repeats/skipped times in time array
# Landmark (absolute) observations
@@ -281,7 +284,7 @@ class DataGenerator():
generated_bearing = measurement['bearing']
meas_diff = actual_meas - generated_measurement
- bearing_diff = actual_bearing - generated_bearing
+ bearing_diff = abs(actual_bearing) - abs(generated_bearing)
# double check for edge cases where both are approximately pi (one may be (-), the other may be positive
if (abs(bearing_diff) > 1):
@@ -301,7 +304,7 @@ class DataGenerator():
generated_bearing = measurement['bearing']
meas_diff = actual_meas - generated_measurement
- bearing_diff = actual_bearing - generated_bearing
+ bearing_diff = abs(actual_bearing) - abs(generated_bearing)
# double check for edge cases where both are approximately pi (one may be (-), the other may be positive
if (abs(bearing_diff) > 1):
@@ -309,15 +312,21 @@ class DataGenerator():
meas_test_arr[i].append({'meas_diff': meas_diff, 'bearing_diff': bearing_diff, 'time': curr_time})
- fig, ax_arr = plt.subplots(6)
+ fig, ax_arr = plt.subplots(8)
plt.xlabel('t [s]')
- plt.title('Odometry & Measurement Data Verification', position=(0.5,7))
- ax_arr[0].set_ylabel('v_diff [m/s]')
- ax_arr[1].set_ylabel('w_diff [r/s]')
- ax_arr[2].set_ylabel('o_diff [r]')
- ax_arr[3].set_ylabel('delta_t_diff [s]')
- ax_arr[4].set_ylabel('meas_diff [m]')
- ax_arr[5].set_ylabel('bearing_diff [r]')
+ # plt.title('Odometry & Measurement Data Verification', position=(0.5,7))
+ ax_arr[0].set_xlabel('v_diff [m/s]')
+ ax_arr[1].set_xlabel('w_diff [r/s]')
+ ax_arr[2].set_xlabel('o_diff [r]')
+ ax_arr[3].set_xlabel('delta_t_diff [s]')
+ ax_arr[4].set_xlabel('meas_diff [m]')
+ ax_arr[5].set_xlabel('bearing_diff [r]')
+
+ ax_arr[6].set_xlabel('t')
+ ax_arr[7].set_xlabel('t')
+
+ ax_arr[6].set_ylabel('meas_diff [m]')
+ ax_arr[7].set_ylabel('bearing_diff [r]')
labels = ['robot{}'.format(i) for i, lablel in enumerate(self.robot_labels,1,)]
@@ -326,29 +335,30 @@ class DataGenerator():
w_diff_arr = [d['w_diff'] for d in diff_list]
o_diff_arr = [d['o_diff'] for d in diff_list]
delta_t_diff_arr = [d['delta_t_diff'] for d in diff_list]
- time_arr = [d['time'] for d in diff_list]
+ # time_arr = [d['time'] for d in diff_list]
+
+ n_bins = 60
- ax_arr[0].plot(time_arr, v_diff_arr)
- ax_arr[1].plot(time_arr, w_diff_arr)
- ax_arr[2].plot(time_arr, o_diff_arr)
- ax_arr[3].plot(time_arr, delta_t_diff_arr)
+ ax_arr[0].hist(v_diff_arr, n_bins)
+ ax_arr[1].hist(w_diff_arr, n_bins)
+ ax_arr[2].hist(o_diff_arr, n_bins)
+ ax_arr[3].hist(delta_t_diff_arr, n_bins)
for diff_list in meas_test_arr:
meas_diff_arr = [d['meas_diff'] for d in diff_list]
bearing_diff_arr = [d['bearing_diff'] for d in diff_list]
meas_time_arr = [d['time'] for d in diff_list]
- ax_arr[4].scatter(meas_time_arr, meas_diff_arr)
- ax_arr[5].scatter(meas_time_arr, bearing_diff_arr)
+ ax_arr[4].hist(meas_diff_arr, n_bins)
+ ax_arr[5].hist(bearing_diff_arr, n_bins)
+
+ ax_arr[6].scatter(meas_time_arr, meas_diff_arr)
+ ax_arr[7].scatter(meas_time_arr, bearing_diff_arr)
fig.legend(labels, prop={'size': 12})
- # ax_arr[0].set_ylim([-10*self.velocity_noise, 10*self.velocity_noise])
- # ax_arr[1].set_ylim([-10*self.angular_velocity_noise, 10*self.angular_velocity_noise])
- # ax_arr[4].set_ylim([-10*self.measurement_range_noise, 10*self.measurement_range_noise])
- # ax_arr[5].set_ylim([-10*self.bearing_noise, 10*self.bearing_noise])
-
- plt.show()
- plt.close('all')
+ plt.subplots_adjust(left=.07, bottom=.08, right=.94, top=.89, wspace=.2, hspace=.75)
+ # plt.show()
+ # plt.close('all')
print('******Verification Complete******')
diff --git a/CoLo-AT/dataset_manager/simulated_dataset_manager.py b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
index 6ac192579362de2e208d1a7fb83b59405f26cdcd..ec659335d8804b4e38a6c62a24ef4ca9fe304659 100644
--- a/CoLo-AT/dataset_manager/simulated_dataset_manager.py
+++ b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
@@ -28,7 +28,7 @@ class SimulatedDataSetManager:
def __init__(self, dataset_name, boundary, landmarks, duration, robot_labels,
velocity_noise = 0, angular_velocity_noise = 0, measurement_range_noise = 0, bearing_noise = 0, communication_noise = 0,
- robot_fov = 2*pi/3):
+ robot_fov = 2*pi/3, delta_t=1.0):
self.name = dataset_name
self.boundary = float(boundary)
@@ -44,7 +44,7 @@ class SimulatedDataSetManager:
self.curr_robot_idx = 0
# time step of the simulation (secs) TODO - user specified
- self.delta_t = 1.0
+ self.delta_t = delta_t
# landmarks = {landmark ID: [x,y]} (m)
self.landmark_map = landmarks
@@ -69,7 +69,7 @@ class SimulatedDataSetManager:
# Input: robot_path (line, circle, etc.)
# Output: Generates simulated data (gt, odo, meas)
- def simulate_dataset(self, path='line', test=False):
+ def simulate_dataset(self, path='random', test=False):
if (path == 'line'):
self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data = self.generator.generate_straight_line_data(test)
return self.start_time, self.starting_states, self.dataset_data, self.time_arr
@@ -103,12 +103,9 @@ class SimulatedDataSetManager:
def get_time_arr(self, data_category):
return self.time_arr[data_category]
- # TODO - issues with simulation process communication module:
- # issue appears to be time index is out of boutds
def get_robot_groundtruth(self, gt_time, robot_index):
- while (gt_time >= self.end_time):
- gt_time -= self.delta_t
- gt_index = np.where(gt_time == self.time_arr['groundtruth'][robot_index])[0][0]
+ gt_time_arr = [ groundtruth['time'] for groundtruth in self.groundtruth_data[robot_index] ]
+ gt_index = find_nearest_time_idx(gt_time_arr, gt_time)
gt = self.groundtruth_data[robot_index][gt_index]
return gt
@@ -133,9 +130,10 @@ class SimulatedDataSetManager:
req.set_robot_idx(robot_idx)
req.set_type(req_type)
+
+ # TODO - perhaps this is the sourceo f the issue?
if (req_type == 'measurement' and self.reset_robot_ctr()):
current_time += self.delta_t
- self.reset_robot_idx = False
return valid_respond, current_time, req
@@ -165,10 +163,9 @@ class SimulatedDataSetManager:
message['robot_index'] = robot_idx
# Past the total time of simulation or no further measurements
- valid_dataline = current_time <= self.end_time
+ valid_dataline = current_time < self.end_time or (abs(current_time - self.end_time) < 0.001) # <= w/floats
# Select message data
- # NOTE - use difference rather than inequality because of float difficulties
if valid_dataline:
if req_type == 'odometry':
odo_time_idx = find_nearest_time_idx(self.time_arr[req_type][robot_idx], current_time)
@@ -202,5 +199,6 @@ class SimulatedDataSetManager:
idx = measurement_indices[least_index]
return self.measurement_data[robot_idx][idx]
+
# No measurement data available (ex: no landmarks in vision at given time)
return {'time': current_time, 'subject_ID': None, 'measurment_range': None,'bearing': None}
\ No newline at end of file