diff --git a/CoLo-AT/dataset_manager/data_generator.py b/CoLo-AT/dataset_manager/data_generator.py
index cd585f1951a367ac254fdfa77547aeb16916df88..0dee24728f69f14b9884809a81920dc907d1ee31 100644
--- a/CoLo-AT/dataset_manager/data_generator.py
+++ b/CoLo-AT/dataset_manager/data_generator.py
@@ -72,45 +72,9 @@ class DataGenerator():
self.groundtruth_data[i].append({'time' : self.time_arr['groundtruth'][i][time_idx], 'x_pos': next_x, 'y_pos': next_y, 'orientation': next_orientation})
- # Generate Odometery Data
- for i, label in enumerate(self.robot_labels, 0):
- # Initial robot state
- self.odometry_data[i].append({'time': self.start_time, 'velocity' : 0, 'angular velocity' : 0, 'orientation' : self.groundtruth_data[i][0]['orientation'],
- 'delta_t' : 0})
- 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)
-
- 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)
+ self.generate_odometry_data()
+ self.generate_measurement_data()
- 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})
-
- # Generate Measurement Data
- for robot_idx, label in enumerate(self.robot_labels, 0):
- # NOTE - time_arr is used to emplace a one-to-one index-to-time correspondence
- # between measurement_data and time_arr['measurement'] to account for varing number of landmark observations
- time_arr = []
- # iterate over all possible times and (potentially) observable landmarks
- for time_idx in range(0, len(self.time_arr['measurement'][i])):
- for landmarkID, landmark_loc in self.landmark_map.items():
-
- time = self.time_arr['measurement'][robot_idx][time_idx]
- robot_loc_x = self.groundtruth_data[robot_idx][time_idx]['x_pos']
- robot_loc_y = self.groundtruth_data[robot_idx][time_idx]['y_pos']
- robot_loc = [robot_loc_x, robot_loc_y]
-
- if (self.within_vision(self.robot_fov, robot_loc, landmark_loc, 0)):
- measurement_range = self.calc_distance(robot_loc, landmark_loc) + np.random.normal(loc=0.0,scale=self.measurement_range_noise)
- bearing = atan2((landmark_loc[1]-robot_loc[1]), (landmark_loc[0]-robot_loc[0])) + np.random.normal(loc=0.0,scale=self.bearing_noise)
- if (bearing < 0):
- bearing += 2*pi
- self.measurement_data[robot_idx].append({'time': time, 'subject_ID': landmarkID, 'measurment_range':measurement_range, 'bearing':bearing})
- time_arr.append(time) # we expect repeats/skipped times in time array
- self.time_arr['measurement'][robot_idx] = time_arr
self.dataset_data = {'odometry': self.odometry_data, 'measurement': self.measurement_data, 'groundtruth': self.groundtruth_data}
if (test):
@@ -147,12 +111,24 @@ class DataGenerator():
next_y = curr_y + distance_step*sin(radian_step + curr_orientation)
next_orientation = curr_orientation + radian_step
- if (next_orientation > 2*pi): # later on we may have robots go in multiple loops
- next_orientation -= 2*pi
+ # Converts all orientations to be between pi and pi
+ if (next_orientation > pi or next_orientation < -pi):
+ next_orientation = self.converge_to_bearing_range(next_orientation)
+
self.groundtruth_data[i].append({'time' : self.time_arr['groundtruth'][i][time_idx], 'x_pos': next_x, 'y_pos': next_y, 'orientation': next_orientation})
- # Generate Odometery Data
+ self.generate_odometry_data()
+ self.generate_measurement_data()
+
+ self.dataset_data = {'odometry': self.odometry_data, 'measurement': self.measurement_data, 'groundtruth': self.groundtruth_data}
+
+ if (test):
+ self.verify_generated_data()
+
+ return self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data
+
+ def generate_odometry_data(self):
for i, label in enumerate(self.robot_labels, 0):
# Initial robot state
self.odometry_data[i].append({'time': self.start_time, 'velocity' : 0, 'angular velocity' : 0, 'orientation' : self.groundtruth_data[i][0]['orientation'],
@@ -169,13 +145,14 @@ class DataGenerator():
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})
- # Generate Measurement Data
+ def generate_measurement_data(self):
+ # Generate Measurement Data
for robot_idx, label in enumerate(self.robot_labels, 0):
# NOTE - time_arr is used to emplace a one-to-one index-to-time correspondence
# between measurement_data and time_arr['measurement'] to account for varing number of landmark observations
time_arr = []
# iterate over all possible times and (potentially) observable landmarks
- for time_idx in range(0, len(self.time_arr['measurement'][i])):
+ for time_idx in range(0, len(self.time_arr['measurement'][robot_idx])):
for landmarkID, landmark_loc in self.landmark_map.items():
time = self.time_arr['measurement'][robot_idx][time_idx]
@@ -186,20 +163,14 @@ class DataGenerator():
if (self.within_vision(self.robot_fov, robot_loc, landmark_loc, robot_orientation)):
measurement_range = self.calc_distance(robot_loc, landmark_loc) + np.random.normal(loc=0.0,scale=self.measurement_range_noise)
- bearing = atan2((landmark_loc[1]-robot_loc[1]), (landmark_loc[0]-robot_loc[0])) + np.random.normal(loc=0.0,scale=self.bearing_noise)
- if (bearing < 0):
- bearing += 2*pi
+ bearing = atan2((landmark_loc[1]-robot_loc[1]), (landmark_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_bearing_range(bearing)
self.measurement_data[robot_idx].append({'time': time, 'subject_ID': landmarkID, 'measurment_range':measurement_range, 'bearing':bearing})
time_arr.append(time) # we expect repeats/skipped times in time array
self.time_arr['measurement'][robot_idx] = time_arr
- self.dataset_data = {'odometry': self.odometry_data, 'measurement': self.measurement_data, 'groundtruth': self.groundtruth_data}
-
- if (test):
- self.verify_generated_data()
- return self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data
-
# Inputs: None
# Outputs: Verifies Generated Data
def verify_generated_data(self):
@@ -234,10 +205,10 @@ class DataGenerator():
generated_o = odometry_list[g_idx]['orientation']
generated_delta_t = odometry_list[g_idx]['delta_t']
- v_diff = actual_v - generated_v
- w_diff = actual_w - generated_w
- o_diff = actual_o - generated_o
- delta_t_diff = actual_delta_t - generated_delta_t
+ v_diff = actual_v - generated_v # should be randomly distributed between user noise
+ w_diff = actual_w - generated_w # should be randomly distributed between user noise
+ o_diff = actual_o - generated_o # should be 0
+ delta_t_diff = actual_delta_t - generated_delta_t # should be 0
odo_test_arr[i].append({'v_diff': v_diff, 'w_diff': w_diff, 'o_diff': o_diff, 'delta_t_diff': delta_t_diff, 'time': t2})
@@ -251,9 +222,7 @@ class DataGenerator():
actual_meas = self.calc_distance(curr_loc, landmark_loc)
generated_measurement = measurement['measurment_range']
- actual_bearing = atan2(landmark_loc[1]- y2, landmark_loc[0]-x2)
- if (actual_bearing < 0):
- actual_bearing += 2*pi
+ actual_bearing = self.converge_to_bearing_range(atan2(landmark_loc[1]- y2, landmark_loc[0]-x2) - actual_o) # global - orientation = local, -pi to pi
generated_bearing = measurement['bearing']
meas_diff = actual_meas - generated_measurement
@@ -261,7 +230,7 @@ class DataGenerator():
meas_test_arr[i].append({'meas_diff': meas_diff, 'bearing_diff': bearing_diff, 'time': curr_time})
- fig, ax_arr = plt.subplots(6) # TODO - change plotting code to accomodate measurement_range and bearing verification
+ fig, ax_arr = plt.subplots(6)
plt.xlabel('t [s]')
plt.title('Odometry & Measurement Data Verification', position=(0.5,7))
ax_arr[0].set_ylabel('v_diff [m/s]')
@@ -271,7 +240,7 @@ class DataGenerator():
ax_arr[4].set_ylabel('meas_diff [m]')
ax_arr[5].set_ylabel('bearing_diff [r]')
- labels = ['robot1', 'robot2', 'robot3']
+ labels = ['robot{}'.format(i) for i, lablel in enumerate(self.robot_labels,1,)]
for diff_list in odo_test_arr:
v_diff_arr = [d['v_diff'] for d in diff_list]
@@ -323,6 +292,7 @@ class DataGenerator():
return distance
# NOTE - add distance limitations to robot_vision?
+ # TODO - FIX THIS
# Input: robot_loc, landmark_loc, [x,y]
# Output: boolean, a given point is with the field of view (sector of a circle) of a robot
def within_vision(self, robot_fov, robot_loc, landmark_loc, robot_orientation=0):
@@ -330,7 +300,15 @@ class DataGenerator():
bearing = atan2((landmark_loc[1]-robot_loc[1]),(landmark_loc[0]-robot_loc[0]))
if bearing < 0:
bearing += 2*pi
- return bearing <= robot_fov/2 + robot_orientation and bearing >= robot_orientation - robot_fov/2
+ return True # bearing <= robot_fov/2 + robot_orientation and bearing >= robot_orientation - robot_fov/2
+
+ # get angle between -pi and pi
+ def converge_to_bearing_range(self, theta):
+ while (theta < -pi):
+ theta += 2*pi
+ while (theta > pi):
+ theta -= 2*pi
+ return theta
# NOTE - Data formatting
'''
diff --git a/CoLo-AT/localization_algos/simple_ekf.py b/CoLo-AT/localization_algos/simple_ekf.py
index f56a96c655fef852d8d2ea83daf897dcd4f11817..c50fabf5d43b2dd3d6412b037695e66f8b519bea 100644
--- a/CoLo-AT/localization_algos/simple_ekf.py
+++ b/CoLo-AT/localization_algos/simple_ekf.py
@@ -9,92 +9,110 @@ import sys
from localization_algo_framework import ekf_algo_framework
#from sympy import *
+import IPython
+
+
def rot_mtx(theta):
return np.matrix([[cos(theta), -sin(theta)], [sin(theta), cos(theta)]])
-
+# TODO - Multiple robots
class Simple_EKF(ekf_algo_framework):
+ # TODO - Change documentation: we don't need init function for orientation or for state anymore
+ # initialization of state & orientation is done in robot_centralized.py
+ # we only need init function for state_variance_init
def __init__(self, algo_name):
+ # TODO - We define our own sensor-covariance based upon user input for noise (IGNORE Passed covariance - this is based on real-world) this was the source of unkown noise
+ # Make this dependent on user input, rather than hard-coded
+ self.sigma_odo = 0.01
+ self.sigma_ob = 0.01*np.matrix(np.identity(2,float))
ekf_algo_framework.__init__(self, algo_name)
-
- def state_init(self):
- return 0.01*np.matrix(np.identity(2), dtype = float)
-
+
def state_variance_init(self, num_robots):
- return 0.01*np.matrix(np.identity(2), dtype = float)
+ return 0.01*np.matrix(np.identity(2*num_robots), dtype = float)
def calculate_trace_state_variance(self, robot_data):
- [s, orinetations, sigma_s, index] = robot_data
- trace_state_var = np.trace(sigma_s)
- return trace_state_var
+ sigma_s = robot_data[2]
+ return np.trace(sigma_s)
def propagation_update(self, robot_data, sensor_data):
- [s, orinetations, sigma_s, index] = robot_data
- [measurement_data, sensor_covariance] = sensor_data
- sigma_odo = sensor_covariance
- num_robots = int(len(s)/2)
+
+ # state : [x,y], orientations : [o1,o2, ...], state_variance, robot_idx (useful if multiple robots)
+ [s, orientations, sigma_s, index] = robot_data
+
+ # measurement data : [delta_t, v, orientation]
+ odometry_data = sensor_data[0]
+
+ delta_t = odometry_data[0]
+ v = odometry_data[1]
+ self_theta = odometry_data[2]
+ orientations[index] = self_theta
- delta_t = measurement_data[0]
if delta_t < 0:
sys.exit('delta_t: '+ str(delta_t))
- v = measurement_data[1]
- orinetations[index] = measurement_data[2]
- self_theta = orinetations[index]
- i = 2*index
- s[i,0] = s[i,0] + cos(self_theta)*v*delta_t #x
- s[i+1,0] = s[i+1,0] + sin(self_theta)*v*delta_t #y
+
+ # State update
+ s[0,0] += cos(self_theta)*v*delta_t #x
+ s[1,0] += sin(self_theta)*v*delta_t #y
- Q = sigma_odo
+ Q = self.sigma_odo
P = sigma_s
- W = delta_t*rot_mtx(self_theta)
- P = P + W*Q*W.getT() # A is identity matrix
+ W = delta_t*np.matrix([cos(self_theta), sin(self_theta)]).getT()
+ P = P + W*Q*W.getT() # A is identity matrix
+
+ # state variance update
sigma_s = P
- return [s, orinetations, sigma_s]
-
+ return [s, orientations, sigma_s]
def absolute_obser_update(self, robot_data, sensor_data):
- [s, orinetations, sigma_s, index] = robot_data
+
+ # state : [x,y], orientations : [o1,o2, ...], state_variance, robot_idx (useful if multiple robots
+ [s, orientations, sigma_s, index] = robot_data
- [measurement_data, sensor_covariance] = sensor_data
- sigma_ob = sensor_covariance
- num_robots = int(len(s)/2)
- self_theta = orinetations[index]
+ measurement_data = sensor_data[0]
landmark_loc = measurement_data[0]
meas_range = measurement_data[1]
bearing = measurement_data[2]
- i = 2*index
P = sigma_s
- R = sigma_ob
+ R = self.sigma_ob
z = np.matrix([meas_range,bearing]).getT()
lx = landmark_loc[0]
ly = landmark_loc[1]
- x = s[i,0]
- y = s[i+1,0]
+ x = s[0,0]
+ y = s[1,0]
+
+ self_theta = orientations[index][0,0]
+
+ z_hat_0 = sqrt((lx-x)**2+(ly-y)**2) # measurement distance
+ z_hat_1 = atan2((ly-y), (lx-x)) - self_theta
- z_hat_0 = sqrt((lx-x)*(lx-x)+(ly-y)*(ly-y))
- z_hat_1 = (atan2((ly-y), (lx-x))-self_theta)%pi
- if abs(z_hat_1-pi) < abs(z_hat_1):
- z_hat_1 = z_hat_1-pi
+ # -pi to pi
+ while (z_hat_1 < -pi):
+ z_hat_1 += 2*pi
+ while (z_hat_1 > pi):
+ z_hat_1 -= 2*pi
z_hat = np.matrix([z_hat_0, z_hat_1]).getT()
- h11 = (-lx + x)/sqrt((lx - x)**2 + (ly - y)**2)
- h12 = (-ly + y)/sqrt((lx - x)**2 + (ly - y)**2)
- h21 = -(-ly + y)/((lx - x)**2 + (ly - y)**2)
- h22 = -(lx - x)/((lx - x)**2 + (ly - y)**2)
+ h11 = -(lx - x)/sqrt((lx - x)**2 + (ly - y)**2)
+ h12 = -(ly - y)/sqrt((lx - x)**2 + (ly - y)**2)
+ h21 = (ly - y)/((lx - x)**2 + (ly - y)**2)
+ h22 = (x - lx)/((lx - x)**2 + (ly - y)**2)
-
H = np.matrix([[h11, h12],[h21, h22]])
- sigma_invention = H * P * H.getT() + R # V is a identity matrix
- K = P*H.getT()*sigma_invention.getI()
- s[i:i+2, 0] = s[i:i+2, 0] + K*(z - z_hat)
- P = P - K*H*P
- sigma_s = P
+ # Compute Kalman Gain
+ K = P*H.getT() * (H*P*H.getT() + R).getI() # V is the identity matrix
+ # State correction
+ s_update = K*(z - z_hat)
+ s = s + s_update
- return [s, orinetations, sigma_s]
\ No newline at end of file
+ # state variance correction
+ P = (np.matrix(np.identity(2,float)) - K*H)*P
+ sigma_s = P
+
+ return [s, orientations, sigma_s]
\ No newline at end of file
diff --git a/CoLo-AT/test_simulation_sagar.py b/CoLo-AT/test_simulation_sagar.py
index 9a250f47a61d15702eee709e37175882610fce82..4304428b114322400c6e7af6be275f36d121f6bd 100644
--- a/CoLo-AT/test_simulation_sagar.py
+++ b/CoLo-AT/test_simulation_sagar.py
@@ -40,27 +40,26 @@ compname = getpass.getuser()
# landmark IDs must be bigger than 5
landmarks = {10: [0,5], 11: [3,3], 12: [-3,0], 13: [-3, -2] }
-robot_labels = [1,2,3]
+robot_labels = [1]
boundary = 5 # radius of circular map centered at 0,0
-duration = 30 # (seconds)
+duration = 150 # (seconds)
-# TODO - debug velocity_noise
-testing_dataset = SimulatedDataSetManager('testing', boundary, landmarks, duration, robot_labels,
-velocity_noise=0.001, angular_velocity_noise=0.001, measurement_range_noise=0.001, bearing_noise=0.001, robot_fov=2*pi)
-start_time, starting_states, dataset_data, time_arr = testing_dataset.simulate_dataset('circle', test=True)
+# testing_dataset = SimulatedDataSetManager('testing', boundary, landmarks, duration, robot_labels,
+# velocity_noise=0.01, angular_velocity_noise=0.01, measurement_range_noise=0.01, bearing_noise=0.01, robot_fov=2*pi)
+# start_time, starting_states, dataset_data, time_arr = testing_dataset.simulate_dataset('circle', test=True)
# Real-World DataSet
-# dataset_path = "D:/LEMUR/CoLo/CoLo-D/CoLo-Datasets/official_dataset1/"
-# testing_dataset = RW_Dataset_Manager('testing')
-# start_time, starting_states, dataset_data, time_arr = testing_dataset.load_datasets(dataset_path, robot_labels, duration)
+dataset_path = "D:/LEMUR/CoLo/CoLo-D/CoLo-Datasets/official_dataset1/"
+testing_dataset = RW_Dataset_Manager('testing')
+start_time, starting_states, dataset_data, time_arr = testing_dataset.load_datasets(dataset_path, robot_labels, duration,delay_start=20)
analyzer = Analyzer('analyzer', robot_labels)
-loc_algo = Centralized_EKF('Centralized_EKF')
+loc_algo = Simple_EKF('Simple_EKF')
robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
sim = SimulationManager('sim')
-state_recorder = StatesRecorder('Centralized_EKF', robot_labels)
+state_recorder = StatesRecorder('Simple_EKF', robot_labels)
sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = False, comm=False, simulated_comm = False)