diff --git a/CoLo-AT/Cooperative_Localization_Journal1.py b/CoLo-AT/Cooperative_Localization_Journal1.py
index d48eb2e4dfd0b8d56a5cf1509c215b3937bf08e2..445f809c2c53e4ba47c3faf0664abd28d61f274e 100644
--- a/CoLo-AT/Cooperative_Localization_Journal1.py
+++ b/CoLo-AT/Cooperative_Localization_Journal1.py
@@ -43,8 +43,6 @@ state_recorder = StatesRecorder('GS_CI', robot_labels)
sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = False)
loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = True)
-#animate_plot(robot_labels, state_recorder, analyzer, testing_dataset.get_landmark_map())
-
##############################################################################
testing_dataset.dataset_reset()
@@ -54,7 +52,7 @@ 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 = True)
+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)
##############################################################################
@@ -66,7 +64,7 @@ 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 = True)
+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)
##############################################################################
@@ -77,7 +75,7 @@ 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 = True)
+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)
##############################################################################
@@ -89,7 +87,7 @@ 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 = True)
+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)
##############################################################################
@@ -100,6 +98,6 @@ 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 = True)
+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)
analyzer.algos_comparison([state_recorder, state_recorder_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI, state_recorder_bound], only_trace = ['GS_CI_bound'])
diff --git a/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc b/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc
index dc6edb64e90bd2206b2c9cd8468823513f13048e..dffc9d42f386adb855d92b044eb516afee063aa8 100644
Binary files a/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc and b/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc differ
diff --git a/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-36.pyc b/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-36.pyc
index 3a8dd54cec228a7daaec9f21364b2a5091cbcb31..61c63f3de2a7898ac54f31c321a5c93a82e23461 100644
Binary files a/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-36.pyc and b/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-36.pyc differ
diff --git a/CoLo-AT/data_analysis/data_analyzer.py b/CoLo-AT/data_analysis/data_analyzer.py
index e1d352cc8738a5a72d9ea862f06b63c0e9ca7928..2de013b2ab69606998e3cbe93765adfdfa5a0e4d 100644
--- a/CoLo-AT/data_analysis/data_analyzer.py
+++ b/CoLo-AT/data_analysis/data_analyzer.py
@@ -146,7 +146,6 @@ class Analyzer():
time_arr.append((interval_start_time+unit_time_interval+interval_start_time)/2)
interval_start_time = interval_start_time+unit_time_interval
-
print(data_recorder.name,': ')
print('Avg location deviation errors per run: ', sum(loc_err_per_run)/len(loc_err_per_run))
print('Avg trace of state variances per run: ', sum(trace_per_run)/len(trace_per_run))
diff --git a/CoLo-AT/data_analysis/dataset_analyzer.py b/CoLo-AT/data_analysis/dataset_analyzer.py
deleted file mode 100644
index a9e77f89bba62c029ab475cd927806c253af18af..0000000000000000000000000000000000000000
--- a/CoLo-AT/data_analysis/dataset_analyzer.py
+++ /dev/null
@@ -1,35 +0,0 @@
-import numpy as np
-import os
-
-import initialization_datasets
-
-def dataset_analysis(dataset_name, duration, sampling_period):
- # this function is to find the characteristic of the dataset. Given name of the dataset and period of sampling (in second),
- # it will output v_max, r_max of for each robot during each period, also the mean and the STD of observation time interval
- dataset_labels = [1,2,3,4,5] #all robots
- [landmark_map, start_time, start_state_arr, measurement_data, odometry_data, groundtruth_data] = initialization_datasets.initialization_MRCLAMDatasets_multirobots(dataset_name, dataset_labels, duration)
-
- print 'robot\t\tv_max\t\tr_mean\t\tr_std\tnum_obser\ttime'
-
- for i, label in enumerate(dataset_labels):
- t = start_time
- while t <= start_time+duration-sampling_period:
- v_seq = [obs['velocity'] for obs in odometry_data[i] if t+sampling_period > obs['time'] >= t] # create a list of velocity data for each sampling period
- v_max = max(v_seq)
-
- r_seq = [obs['measurment_range'] for obs in measurement_data[i] if t+sampling_period > obs['time'] >= t] # create a list of measurement_range data for each sampling period
- try:
- r_max = max(r_seq)
- r_mean = np.mean(r_seq)
- r_std = np.std(r_seq)
-
- if r_std < 0.8:
- print str(label)+'\t'+ str(v_max) + '\t\t'+ str(r_max) +'\t' +str(r_std) + '\t'+ str(len(r_seq)) + '\t' + str(t-start_time) + 's~' + str(t+sampling_period-start_time) +'s'
-
- except ValueError:
- print '\tempty array'
-
- t = t + sampling_period
-
- return 1
-
diff --git a/CoLo-AT/dataset_analysis.py b/CoLo-AT/dataset_analysis.py
new file mode 100644
index 0000000000000000000000000000000000000000..2bd53386c4bc0438e07f05b9a6e7148b90283c15
--- /dev/null
+++ b/CoLo-AT/dataset_analysis.py
@@ -0,0 +1,24 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Thu May 16 16:26:29 2019
+
+@author: william
+"""
+
+import os, sys
+sys.path.append(os.path.join(os.path.dirname(__file__), "."))
+from dataset_manager.realworld_dataset_manager import RW_Dataset_Manager
+
+
+# load algorithms
+sys.path.append(os.path.join(os.path.dirname(__file__), "localization_algos"))
+
+# dataset_path = "/Users/shengkangchen/Documents/CoLo/CoLo-D/CoLo-Datasets/official_dataset3/"
+dataset_path = "/home/william/CoLo/CoLo-D/CoLo-Datasets/official_dataset3/" # for desktop Ubuntu
+
+robot_labels = [1,2,3]
+duration = 180 # duration for the simulation in sec
+testing_dataset = RW_Dataset_Manager('testing')
+start_time, starting_states, dataset_data, time_arr = testing_dataset.load_datasets(dataset_path, robot_labels, duration)
+testing_dataset.dataset_analysis(duration)
\ No newline at end of file
diff --git a/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc b/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc
index f73b2cf653e3579d4ded1a002c64063cda1cb593..de81e6ebe727461593d79fb78612610ee8c62a80 100644
Binary files a/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc and b/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc differ
diff --git a/CoLo-AT/dataset_manager/realworld_dataset_manager.py b/CoLo-AT/dataset_manager/realworld_dataset_manager.py
index b52738571bcbcfca8ba1f015367f78f2eda62de3..ca35dc4ff2570bc76adea152f79fa575a6921408 100755
--- a/CoLo-AT/dataset_manager/realworld_dataset_manager.py
+++ b/CoLo-AT/dataset_manager/realworld_dataset_manager.py
@@ -200,6 +200,36 @@ class RW_Dataset_Manager:
print("self.starting_states ", self.starting_states)
return self.start_time, self.starting_states, self.dataset_data, self.time_arr
+
+ def dataset_analysis(self, sampling_period):
+ # this function is to find the characteristic of the dataset. Given name of the dataset and period of sampling (in second),
+ analysis_result = {}
+
+ odo_varibles = ['velocity', 'angular velocity']
+ meas_varibles = ['measurment_range', 'bearing']
+
+ for i, label in enumerate(self.robot_lables):
+ t = self.start_time
+ while t <= self.start_time+self.duration-sampling_period:
+ for odo_variable in odo_varibles:
+ odo_seq = [robot_data[odo_variable] for robot_data in self.odometry_data[i] if t+sampling_period > robot_data['time'] >= t] # create a list of velocity data for each sampling period
+ odo_max = max(odo_seq)
+ odo_std = np.std(odo_seq)
+
+ print('time: ', t, 'robot ID:', label, odo_variable, '_max:', odo_max, odo_variable, '_std:', odo_std)
+
+ for meas_varible in meas_varibles:
+ meas_seq = [robot_data[meas_varible] for robot_data in self.measurement_data[i] if t+sampling_period > robot_data['time'] >= t] # create a list of velocity data for each sampling period
+ meas_max = max(meas_seq)
+ meas_std = np.std(meas_seq)
+
+ print('time: ', t, 'robot ID:', label, meas_varible, '_max:', meas_max, meas_varible, '_std:', meas_std)
+
+ t+=sampling_period
+
+
+ return 1
+
def dataset_reset(self):
self.data_trackers = {'odometry': np.ones((self.num_robots,), dtype=np.int),'measurement':np.ones((self.num_robots,), dtype=np.int), 'groundtruth':np.ones((self.num_robots,), dtype=np.int)}
@@ -460,3 +490,6 @@ class RW_Dataset_Manager:
return gt
+
+
+
diff --git a/CoLo-AT/localization_algos/ekf_gs_bound.py b/CoLo-AT/localization_algos/ekf_gs_bound.py
index cece257226299d8289559bb806900d69ed9dbd4d..fe91db4da8083b8dc1da422d1765a0e39196a3b5 100644
--- a/CoLo-AT/localization_algos/ekf_gs_bound.py
+++ b/CoLo-AT/localization_algos/ekf_gs_bound.py
@@ -10,6 +10,10 @@ def rot_mtx(theta):
class EKF_GS_BOUND(ekf_algo_framework):
def __init__(self, algo_name):
ekf_algo_framework.__init__(self, algo_name)
+ self.var_v = 0.04
+ self.var_range = 0.7
+ self.var_bearing = 0.3
+ self.max_range = 3.5
def state_variance_init(self, num_robots):
return 0.1*np.matrix(np.identity(2*num_robots), dtype = float)
@@ -25,7 +29,7 @@ class EKF_GS_BOUND(ekf_algo_framework):
[measurement_data, sensor_covariance] = sensor_data
sigma_odo = sensor_covariance
- var_v = 0.1 # variance of the velocity
+ var_v = self.var_v # variance of the velocity
i = 2*index
num_robots = int(len(s)/2)
@@ -66,11 +70,12 @@ class EKF_GS_BOUND(ekf_algo_framework):
H_i[1, i+1] = -1
H = rot_mtx(self_theta).getT()*H_i
- var_range = 0.1
- d_max = pow(0.05,2)
- var_bearing = pow(2.0 / 180, 2)
+ var_range = self.var_range
+ var_range = self.var_range
+ range_max = self.range_max
+ var_bearing = self.var_bearing
- R = np.matrix(max(var_range, d_max, var_bearing)*np.identity(2))
+ R = np.matrix(max(var_range, range_max, var_bearing)*np.identity(2))
sigma_s = (sigma_s.getI()+H.getT()*R.getI()*H).getI()
return [s, orinetations, sigma_s]
@@ -101,11 +106,12 @@ class EKF_GS_BOUND(ekf_algo_framework):
H_ij[1, j+1] = 1
H = rot_mtx(self_theta).getT()*H_ij
- var_range = 0.1
- d_max = pow(0.05,2)
- var_bearing = pow(2.0 / 180, 2)
-
- R = np.matrix(max(var_range, d_max, var_bearing)*np.identity(2))
+ var_range = self.var_range
+ var_range = self.var_range
+ range_max = self.range_max
+ var_bearing = self.var_bearing
+
+ R = np.matrix(max(var_range, range_max, var_bearing)*np.identity(2))
sigma_s = (sigma_s.getI()+H.getT()*R.getI()*H).getI()
return [s, orinetations, sigma_s]