Skip to content
Snippets Groups Projects
Commit b7ac4bf0 authored by oceanpdoshi@g.ucla.edu's avatar oceanpdoshi@g.ucla.edu
Browse files

Some modifications to debug report

parent 2321b23d
Branches
No related merge requests found
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Apr 17 2019
@author: Sagar
"""
# Import necessary Libraries
import os, sys
import numpy as np
from math import pi, sqrt
# Import other Colo-AT Modules
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 Localization Algorithms
sys.path.append(os.path.join(os.path.dirname(__file__), "localization_algos"))
# LS -> centralized system.
from centralized_ekf import Centralized_EKF
from ekf_ls_bda import EKF_LS_BDA
from ekf_ls_ci import EKF_LS_CI
# GS -> distributed system
from ekf_gs_bound import EKF_GS_BOUND
from ekf_gs_ci2 import EKF_GS_CI2
from ekf_gs_sci2 import EKF_GS_SCI2
# Template code for running/debugging simulated data
# Set a random seed for replication
np.random.seed(1)
# Initialize landmark map, landmark IDs must be larger than 5
# Format {LandmarkID: [x,y]}
landmarks = {11: [1,0], 12: [0,1], 13: [-1,0], 14: [0,-1]}
# Initalize Robots, can be 1-5
robot_labels = [1,2,3]
# Specify simulation parameters:
# Units are [m], [s], and [rad]
duration = 120
delta_t = 0.2
v_noise = 0.05
w_noise = 0.0001
r_noise = 0.05
phi_noise = sqrt(2)*pi/180
fov = 2*pi
v = 0.1
sigma_v = 0.0
# Instantiate Simulated Dataset Manager
testing_dataset = SimulatedDataSetManager('name', landmarks, duration, robot_labels,
velocity_noise=v_noise, angular_velocity_noise=w_noise, measurement_range_noise=r_noise, bearing_noise=phi_noise,
robot_fov=fov, delta_t=delta_t)
# Generate the data
start_time, starting_states, dataset_data, time_arr = testing_dataset.simulate_dataset('random', test=True, velocity=v, velocity_spread=sqrt(sigma_v))
# Create Data Analyzer
analyzer = Analyzer('analyzer', robot_labels)
# Specify Loc Algo and create RobotSystem
loc_algo = Centralized_EKF('Centralized_EKF')
robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
# Create the simulation manager and state recorder
sim = SimulationManager('sim')
state_recorder = StatesRecorder('Centralized_EKF', robot_labels)
# Run the simulation
end_time = sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = True, comm=False, simulated_comm = False)
# Analyze the data
loc_err_per_run, state_err_per_run, trace_per_run, time_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = False)
# View an animated display of robot movement and loc algo performance
animate_plot(robot_labels, state_recorder, analyzer, testing_dataset.get_landmark_map())
'''
# Input: loc1=[x1,y1], loc2=[x2,y2]
# Output: Euclidean distance between two points
def calc_distance(loc1, loc2):
x1 = loc1[0]
y1 = loc1[1]
x2 = loc2[0]
y2 = loc2[1]
distance = sqrt((x1-x2)**2 + (y1-y2)**2)
return distance
np.random.seed(1)
# landmark IDs must be bigger than 5
# landmarks = {11: [120,2]}
landmarks = {11: [0,-1], 12: [1,0], 13: [1,1], 14: [0,1]}
# landmarks = {11: [1,0], 12: [0,1], 13: [-1,0], 14: [0,-1]}
robot_labels = [1]
duration = 120
delta_t = 0.2
# NOTE - for documentation: you need at least 3 robots to run communication
testing_dataset = SimulatedDataSetManager('testing', landmarks, duration, robot_labels,
velocity_noise=.05, angular_velocity_noise=0.00001, measurement_range_noise=.05, bearing_noise=sqrt(2) *pi/180
,robot_fov=2*pi, delta_t=delta_t)
start_time, starting_states, dataset_data, time_arr = testing_dataset.simulate_dataset('circle', test=False,
velocity=.1, velocity_spread=0.0)
analyzer = Analyzer('analyzer', robot_labels)
loc_algo = Centralized_EKF('Centralized_EKF')
robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
sim = SimulationManager('sim')
state_recorder = StatesRecorder('Centralized_EKF', robot_labels)
end_time = sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = True, comm=False, simulated_comm = False)
loc_err_per_run, state_err_per_run, trace_per_run, time_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = False)
# animate_plot(robot_labels, state_recorder, analyzer, testing_dataset.get_landmark_map())
'''
'''
# 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,delay_start=20)
# Investigation of RMS/trace corresponding to meas_range
x_diffs = diffs['x_diff']
y_diffs = diffs['y_diff']
times = np.arange(0, 120, delta_t)
x_diffs = list(filter(lambda a: a != -1, x_diffs))
x_diffs = [np.asscalar(x) for x in x_diffs]
y_diffs = list(filter(lambda a: a != -1, y_diffs))
y_diffs = [np.asscalar(y) for y in y_diffs]
robot_x = [gt['x_pos'] for gt in dataset_data['groundtruth'][0]]
robot_y = [gt['y_pos'] for gt in dataset_data['groundtruth'][0]]
robot_locs = [ [robot_x[i], robot_y[i] ] for i in range(0, len(robot_x))]
distances = [calc_distance(landmarks[11], robot_loc) for robot_loc in robot_locs]
fig = plt.figure(2)
plt.plot(times, x_diffs, times, y_diffs)
plt.xlabel('t [s]')
plt.ylabel('|z-z_hat| [m]')
plt.legend(['x', 'y'], loc='upper right')
fig = plt.figure(3)
plt.plot(times, distances)
plt.xlabel('t [s]')
plt.ylabel('robot distance to landmark [m]')
plt.show()
'''
\ No newline at end of file
......@@ -10,9 +10,10 @@ import os, sys
import getpass
import IPython
import numpy as np
from math import pi
from math import pi, sqrt
from matplotlib import pyplot as plt
from matplotlib import animation
import IPython
# sys.path.append(os.path.join(os.path.dirname(__file__), "."))
from dataset_manager.realworld_dataset_manager import RW_Dataset_Manager
......@@ -37,38 +38,155 @@ from ekf_ls_ci import EKF_LS_CI
from ekf_gs_bound import EKF_GS_BOUND
from ekf_gs_ci2 import EKF_GS_CI2
compname = getpass.getuser()
#dataset_path = "/home/"+ compname +"/CoLo/CoLo-D/UTIAS-Datasets/MRCLAM_Dataset3/"
# Template code for running/debugging simulated data
# Set a random seed for replication
np.random.seed(1)
# Initialize landmark map, landmark IDs must be larger than 5
# Format {LandmarkID: [x,y]}
landmarks = {11: [1,0], 12: [0,1], 13: [-1,0], 14: [0,-1]}
# Initalize Robots, can be 1-5
robot_labels = [1]
# Specify simulation parameters:
# Units are [m], [s], and [rad]
duration = 120
delta_t = 0.2
v_noise = 0.05
w_noise = 0.0001
r_noise = 0.05
phi_noise = sqrt(2)*pi/180
fov = 2*pi
v = 0.1
sigma_v = 0.1
# Instantiate Simulated Dataset Manager
testing_dataset = SimulatedDataSetManager('name', landmarks, duration, robot_labels,
velocity_noise=v_noise, angular_velocity_noise=w_noise, measurement_range_noise=r_noise, bearing_noise=phi_noise,
robot_fov=fov, delta_t=delta_t)
# Generate the data
start_time, starting_states, dataset_data, time_arr = testing_dataset.simulate_dataset('random', test=False, velocity=v, velocity_spread=sqrt(sigma_v))
# Create Data Analyzer
analyzer = Analyzer('analyzer', robot_labels)
# Specify Loc Algo and create RobotSystem
loc_algo = EKF_GS_CI2('EKF_GS_CI2')
robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
# Create the simulation manager and state recorder
sim = SimulationManager('sim')
state_recorder = StatesRecorder('EKF_GS_CI2', robot_labels)
# Run the simulation
end_time = sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = False, comm=False, simulated_comm = False)
# Analyze the data
loc_err_per_run, state_err_per_run, trace_per_run, time_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = False)
# View an animated display of robot movement and loc algo performance
# animate_plot(robot_labels, state_recorder, analyzer, testing_dataset.get_landmark_map())
'''
# Input: loc1=[x1,y1], loc2=[x2,y2]
# Output: Euclidean distance between two points
def calc_distance(loc1, loc2):
x1 = loc1[0]
y1 = loc1[1]
x2 = loc2[0]
y2 = loc2[1]
distance = sqrt((x1-x2)**2 + (y1-y2)**2)
return distance
np.random.seed(1)
# landmark IDs must be bigger than 5
landmarks = {10: [0,5] , 11: [3,3], 12: [-3,0], 13: [-3, -2] }
# landmarks = {11: [120,2]}
landmarks = {11: [0,-1], 12: [1,0], 13: [1,1], 14: [0,1]}
# landmarks = {11: [1,0], 12: [0,1], 13: [-1,0], 14: [0,-1]}
robot_labels = [1]
duration = 120
delta_t = 0.2
# NOTE - for documentation: you need at least 3 robots to run communication
robot_labels = [1,2]
boundary = 5 # radius of circular map centered at 0,0
duration = 45 # (seconds)
# NOTE - adjust noise if need to see drastic convergence/divergence
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('random', test=False)
testing_dataset = SimulatedDataSetManager('testing', landmarks, duration, robot_labels,
velocity_noise=.05, angular_velocity_noise=0.00001, measurement_range_noise=.05, bearing_noise=sqrt(2) *pi/180
,robot_fov=2*pi, delta_t=delta_t)
start_time, starting_states, dataset_data, time_arr = testing_dataset.simulate_dataset('circle', test=False,
velocity=.1, velocity_spread=0.0)
analyzer = Analyzer('analyzer', robot_labels)
loc_algo = Centralized_EKF('Centralized_EKF')
robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
sim = SimulationManager('sim')
state_recorder = StatesRecorder('Centralized_EKF', robot_labels)
end_time = sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = True, comm=False, simulated_comm = False)
loc_err_per_run, state_err_per_run, trace_per_run, time_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = False)
# animate_plot(robot_labels, state_recorder, analyzer, testing_dataset.get_landmark_map())
'''
'''
# 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,delay_start=20)
analyzer = Analyzer('analyzer', robot_labels)
# Investigation of RMS/trace corresponding to meas_range
x_diffs = diffs['x_diff']
y_diffs = diffs['y_diff']
loc_algo = EKF_GS_BOUND('EKF_GS_BOUND')
robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
times = np.arange(0, 120, delta_t)
sim = SimulationManager('sim')
state_recorder = StatesRecorder('EKF_GS_BOUND', robot_labels)
x_diffs = list(filter(lambda a: a != -1, x_diffs))
x_diffs = [np.asscalar(x) for x in x_diffs]
y_diffs = list(filter(lambda a: a != -1, y_diffs))
y_diffs = [np.asscalar(y) for y in y_diffs]
robot_x = [gt['x_pos'] for gt in dataset_data['groundtruth'][0]]
robot_y = [gt['y_pos'] for gt in dataset_data['groundtruth'][0]]
robot_locs = [ [robot_x[i], robot_y[i] ] for i in range(0, len(robot_x))]
distances = [calc_distance(landmarks[11], robot_loc) for robot_loc in robot_locs]
fig = plt.figure(2)
plt.plot(times, x_diffs, times, y_diffs)
# TODO - 3 robots, but one CANNOT take measurements and must rely on communication, see if convergent.
sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = False, comm=True, simulated_comm = True)
plt.xlabel('t [s]')
plt.ylabel('|z-z_hat| [m]')
loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = False)
plt.legend(['x', 'y'], loc='upper right')
animate_plot(robot_labels, state_recorder, analyzer, testing_dataset.get_landmark_map())
fig = plt.figure(3)
plt.plot(times, distances)
plt.xlabel('t [s]')
plt.ylabel('robot distance to landmark [m]')
plt.show()
'''
\ No newline at end of file
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment