diff --git a/CoLo-AT/Cooperative_Localization_Journal2_sim_data.py b/CoLo-AT/Cooperative_Localization_Journal2_sim_data.py
new file mode 100644
index 0000000000000000000000000000000000000000..23cdf34d3d0ad05ac6d13c4b8fefb675769fe30d
--- /dev/null
+++ b/CoLo-AT/Cooperative_Localization_Journal2_sim_data.py
@@ -0,0 +1,110 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Thu May 16 15:41:51 2019
+
+@author: william
+"""
+
+import os, sys
+import getpass
+from math import pi
+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
+from simulation_process.sim_manager import SimulationManager
+from robots.robot_system import RobotSystem
+from simulation_process.state_recorder import StatesRecorder
+from data_analysis.data_analyzer import Analyzer
+from data_analysis.realtime_plot import animate_plot
+
+import comparison_plot
+
+# load algorithms
+sys.path.append(os.path.join(os.path.dirname(__file__), "localization_algos"))
+from centralized_ekf import Centralized_EKF # works
+from ekf_ls_bda import EKF_LS_BDA
+from ekf_ls_ci import EKF_LS_CI
+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] }
+
+robot_labels = [1,2,3,4,5]
+boundary = 200
+duration = 30
+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)
+start_time, starting_states, dataset_data, time_arr = testing_dataset.simulate_dataset('random', test=False)
+
+analyzer = Analyzer('analyzer', robot_labels)
+
+#############################################################################
+
+loc_algo = EKF_GS_CI2('algo')
+robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
+sim = SimulationManager('sim gs_ci')
+state_recorder_gs_ci = StatesRecorder('GS_CI', robot_labels)
+
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_gs_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_gs_ci, plot_graphs = False)
+
+##############################################################################
+
+
+loc_algo = GS_CI_Bound('algo')
+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, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_bound, plot_graphs = False)
+
+
+##############################################################################
+
+
+loc_algo = Centralized_EKF('algo')
+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_algo = EKF_LS_BDA('algo')
+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_algo = EKF_LS_CI('algo')
+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_algo = EKF_GS_SCI2('algo')
+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)
+
+comparison_plot.robot1_algo_comparison_plot([state_recorder_gs_ci, state_recorder_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI, state_recorder_bound], only_trace = ['GS_CI_bound'])
\ No newline at end of file
diff --git a/CoLo-AT/comparison_plot.py b/CoLo-AT/comparison_plot.py
new file mode 100644
index 0000000000000000000000000000000000000000..6262f439137cf5d7185fc8ac34fd1f9ee79a44e7
--- /dev/null
+++ b/CoLo-AT/comparison_plot.py
@@ -0,0 +1,55 @@
+from matplotlib import pyplot as plt
+import IPython
+
+# [time_stamps, loc_err_per_run, data_recorder.name]
+# [time_stamps, trace_per_run, data_recorder.name]
+def generate_comparison_plot(arr_loc_err, arr_trace, only_trace):
+ fig = plt.figure()
+ fig1 = fig.add_subplot(211)
+ fig2 = fig.add_subplot(212)
+ for loc_err in arr_loc_err:
+ fig1.plot(loc_err[0], loc_err[1], label = loc_err[2])
+
+ for trace in arr_trace:
+ if (only_trace != None) and trace[2] in only_trace:
+ fig2.plot(trace[0], trace[1], '--' ,label = trace[2])
+ else:
+ fig2.plot(trace[0], trace[1], label = trace[2])
+
+ fig1.set_title('Location error', fontsize = 24)
+ fig1.set_xlabel('Time[s]', fontsize = 22)
+ fig1.set_ylabel('RMS[m]', fontsize = 22)
+ fig1.set_ylim(0, 1)
+ fig1.legend(loc=1, fontsize = 16)
+ fig1.tick_params(labelsize=18)
+
+ fig2.set_title('Trace of state covariance', fontsize = 24)
+ fig2.set_xlabel('Time [s]', fontsize = 22)
+ fig2.set_ylabel('Sigma_s [m^2]', fontsize = 22)
+ fig2.set_ylim(0, 0.1)
+ fig2.legend(loc=1, fontsize = 20)
+ fig2.tick_params(labelsize=18)
+
+ # plt.subplots_adjust(hspace=.6)
+ plt.show()
+ return
+
+def robot1_algo_comparison_plot(arr_data_recorder, only_trace=None):
+ print("************Algorithm Comparison***************")
+ arr_loc_err = []
+ arr_trace = []
+
+ for data_recorder in arr_data_recorder:
+ r1_label = data_recorder.get_dataset_labels()[0]
+
+ loc_err_per_run = data_recorder.loc_err_arr[r1_label]
+ trace_per_run = data_recorder.trace_sigma_s_arr[r1_label]
+ time_stamps = data_recorder.get_time_arr(r1_label)
+
+ 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] )
+ print('Plotting Comparison Graphs')
+ generate_comparison_plot(arr_loc_err, arr_trace, only_trace)
+ return arr_loc_err, arr_trace
+
diff --git a/CoLo-AT/dataset_manager/data_generator.py b/CoLo-AT/dataset_manager/data_generator.py
index 207210c6e165a38d987446288897bacdc155fc1e..545703b429cb283d60a9256047e6ea279b49c11e 100644
--- a/CoLo-AT/dataset_manager/data_generator.py
+++ b/CoLo-AT/dataset_manager/data_generator.py
@@ -1,7 +1,9 @@
import numpy as np
-from math import pi, sqrt, atan2, hypot, sin, cos
+from math import pi, sqrt, atan2, hypot, sin, cos, pow, trunc
import matplotlib.pyplot as plt
+import IPython
+
# TODO - add collision/boundary checking for circle/random data
class DataGenerator():
def __init__(self, boundary, landmarks, duration, robot_labels, starting_states, start_time, delta_t,
@@ -35,10 +37,12 @@ class DataGenerator():
if landmark_id < 5:
raise Exception("Invalid landmark ID: landmark IDs must be bigger than 5.")
+ # TODO - user specified sample rate
self.time_arr = {'odometry': [[] for robot in robot_labels], 'measurement': [[] for robot in robot_labels], 'groundtruth': [[] for robot in robot_labels]}
for data_type in self.time_arr:
for i in range(self.num_robots):
- self.time_arr[data_type][i] = np.arange(self.start_time, self.end_time)
+ arr = np.arange(self.start_time, self.end_time, self.delta_t)
+ self.time_arr[data_type][i] = arr
self.groundtruth_data = [ [] for robot in range(self.num_robots)]
self.odometry_data = [[] for robot in range(self.num_robots)]
@@ -92,7 +96,7 @@ class DataGenerator():
path_circumference = 2*pi*path_radius
radian_step = 2*pi/self.duration
- velocity_step = path_circumference/self.duration
+ velocity_step = path_circumference/self.duration # May want to randomize velocity w/normal distribution
distance_step = velocity_step*self.delta_t
# Generate ground truth data
@@ -141,15 +145,15 @@ class DataGenerator():
curr_y = self.groundtruth_data[i][time_idx-1]['y_pos']
curr_orientation = self.groundtruth_data[i][time_idx-1]['orientation']
- # rotate some random number of radians between pi/4 and -pi/4
+ # 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 = np.random.uniform(0, 0.5) # units of distance change are in
+ 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
next_x = curr_x + rand_dist*cos(next_orientation)
next_y = curr_y + rand_dist*sin(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})
-
+
self.generate_odometry_data()
self.generate_measurement_data()
@@ -278,6 +282,10 @@ class DataGenerator():
meas_diff = actual_meas - generated_measurement
bearing_diff = actual_bearing - 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):
+ print(actual_bearing, generated_bearing)
meas_test_arr[i].append({'meas_diff': meas_diff, 'bearing_diff': bearing_diff, 'time': curr_time})
else:
@@ -295,6 +303,10 @@ class DataGenerator():
meas_diff = actual_meas - generated_measurement
bearing_diff = actual_bearing - 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):
+ print(actual_bearing, generated_bearing)
+
meas_test_arr[i].append({'meas_diff': meas_diff, 'bearing_diff': bearing_diff, 'time': curr_time})
fig, ax_arr = plt.subplots(6)
@@ -330,10 +342,10 @@ class DataGenerator():
ax_arr[5].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])
+ # 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')
@@ -364,6 +376,8 @@ class DataGenerator():
def within_vision(self, robot_fov, robot_loc, landmark_loc, robot_orientation=0):
# between pi and -pi with respect to LOCAL FRAME
bearing = atan2((landmark_loc[1]-robot_loc[1]),(landmark_loc[0]-robot_loc[0])) - robot_orientation
+ if (robot_fov == 2*pi):
+ return True
return abs(bearing) <= robot_fov/2
# get angle between -pi and pi
diff --git a/CoLo-AT/dataset_manager/simulated_dataset_manager.py b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
index e51247bf50d73b72369d74740cd79dbed99528c4..6ac192579362de2e208d1a7fb83b59405f26cdcd 100644
--- a/CoLo-AT/dataset_manager/simulated_dataset_manager.py
+++ b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
@@ -13,6 +13,13 @@ sys.path.insert(0,parentdir)
from dataset_manager.data_generator import DataGenerator
+def find_nearest_time_idx(l, value):
+ if len(l) != 0:
+ array = np.asarray(l)
+ idx = (np.abs(array-value)).argmin()
+ return idx
+ else:
+ return None
# TODO
# communication
@@ -36,8 +43,8 @@ class SimulatedDataSetManager:
self.curr_request = 'odometry'
self.curr_robot_idx = 0
- # time step of the simulation (secs)
- self.delta_t = 1.0
+ # time step of the simulation (secs) TODO - user specified
+ self.delta_t = 1.0
# landmarks = {landmark ID: [x,y]} (m)
self.landmark_map = landmarks
@@ -47,7 +54,7 @@ class SimulatedDataSetManager:
# starting_states = {robot label: [time, x_pos, y_pos, orientation], ... }
# TODO - create a generate starting states function (potentially randomized to fit boundary)
- self.starting_states = {label : [self.start_time, -boundary + float(i+1)*boundary/self.num_robots, 0, 0.0] for i,label in enumerate(self.robot_labels)}
+ self.starting_states = {label : [self.start_time, i, 0, 0.0] for i,label in enumerate(self.robot_labels)}
# groundtruth & odometry are of fixed length=duration
# measurement_data is of variable length
@@ -71,7 +78,6 @@ class SimulatedDataSetManager:
return self.start_time, self.starting_states, self.dataset_data, self.time_arr
elif(path == 'random'):
self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data = self.generator.generate_random_data(test)
- self.measurement_data[0] = [] # TODO - to test simulated communication, trace of state variance should increase unless a communication update
return self.start_time, self.starting_states, self.dataset_data, self.time_arr
else:
raise Exception("Unsupported robot path specified.")
@@ -162,16 +168,17 @@ class SimulatedDataSetManager:
valid_dataline = current_time <= self.end_time
# Select message data
+ # NOTE - use difference rather than inequality because of float difficulties
if valid_dataline:
if req_type == 'odometry':
- odo_time_idx = np.where(self.time_arr[req_type][robot_idx] == current_time)[0][0]
+ odo_time_idx = find_nearest_time_idx(self.time_arr[req_type][robot_idx], current_time)
message['data'] = self.odometry_data[robot_idx][odo_time_idx]
# if no measurement data available (ex: no landmarks were in range) returns
# {'time': -1, 'subject_ID': None, 'measurment_range': None,'bearing': None}
elif req_type == 'measurement':
message['data'] = self.find_measurement(current_time, robot_idx)
- gt_time_idx = np.where(self.time_arr['groundtruth'][robot_idx] == current_time)[0][0]
+ gt_time_idx = find_nearest_time_idx(self.time_arr['groundtruth'][robot_idx], current_time)
message['groundtruth'] = self.groundtruth_data[robot_idx][gt_time_idx]
return valid_dataline, message, req_type, robot_idx
@@ -181,18 +188,19 @@ class SimulatedDataSetManager:
# {'time': current_time, 'subject_ID': None, 'measurment_range': None,'bearing': None} if no measurement available
def find_measurement(self, current_time, robot_idx):
- # {'idx': idx, 'distance': measurment_range}
available_measurements = []
+ measurement_indices = []
for data_idx, measurement in enumerate(self.measurement_data[robot_idx]):
- if (measurement['time'] == current_time):
- available_measurements.append({'idx': data_idx, 'distance': measurement['measurment_range']})
+ if (abs(measurement['time'] - current_time) < 0.001):
+ available_measurements.append(measurement['measurment_range'])
+ measurement_indices.append(data_idx)
# Select the "best"=shortest distance measurement
if (len(available_measurements) != 0):
- sorted_measurements = sorted(available_measurements, key=lambda k: k['distance'])
- min_distance_idx = sorted_measurements[0]['idx']
- return self.measurement_data[robot_idx][min_distance_idx]
+ least_index = np.argmin(available_measurements)
+ 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