diff --git a/CoLo-AT/UTIAS_d1.pdf b/CoLo-AT/UTIAS_d1.pdf
index dc31ab18dfd28dca78b61bde9585002a34a01abf..bd9396c77c85c294222b8cd011c7d1a1d364fafd 100644
Binary files a/CoLo-AT/UTIAS_d1.pdf and b/CoLo-AT/UTIAS_d1.pdf differ
diff --git a/CoLo-AT/UTIAS_d2.pdf b/CoLo-AT/UTIAS_d2.pdf
index 666b58288f7c5c1919d5a00d087a34fa80a5fad8..5685917113339f5da05582b2eeea11fae7ff6d4e 100644
Binary files a/CoLo-AT/UTIAS_d2.pdf and b/CoLo-AT/UTIAS_d2.pdf differ
diff --git a/CoLo-AT/UTIAS_d3.pdf b/CoLo-AT/UTIAS_d3.pdf
index d52cbfe7d3e9597a7d08bf378cefa3b4f903fb5f..b3a201c4721d78c69dc5dad045027e739814faa3 100644
Binary files a/CoLo-AT/UTIAS_d3.pdf and b/CoLo-AT/UTIAS_d3.pdf differ
diff --git a/CoLo-AT/UTIAS_d4.pdf b/CoLo-AT/UTIAS_d4.pdf
index 3bef22e44e8da8fbaa43ffa5e50eea00f59fd1ea..ec7bfc655cbdb39f07af45190447c99813e3bea2 100644
Binary files a/CoLo-AT/UTIAS_d4.pdf and b/CoLo-AT/UTIAS_d4.pdf differ
diff --git a/CoLo-AT/UTIAS_d5.pdf b/CoLo-AT/UTIAS_d5.pdf
index edb1dcfba097570dd11f3c1c812f4bfe3e5195b2..ad937d82cb1317123dc70bdef681912c89dec928 100644
Binary files a/CoLo-AT/UTIAS_d5.pdf and b/CoLo-AT/UTIAS_d5.pdf differ
diff --git a/CoLo-AT/UTIAS_d6.pdf b/CoLo-AT/UTIAS_d6.pdf
index e743fd2d1e2c28a2c2d2831113bdf1159f9e9958..7c61a9ffe45bb01418f73480d4d5c783b1392b11 100644
Binary files a/CoLo-AT/UTIAS_d6.pdf and b/CoLo-AT/UTIAS_d6.pdf differ
diff --git a/CoLo-AT/UTIAS_d7.pdf b/CoLo-AT/UTIAS_d7.pdf
index b13c61a8b9804007e12a4bc8e36a8d85db510232..a48aee2635ac573e6b74540dbf821b759b40bc6a 100644
Binary files a/CoLo-AT/UTIAS_d7.pdf and b/CoLo-AT/UTIAS_d7.pdf differ
diff --git a/CoLo-AT/UTIAS_d8.pdf b/CoLo-AT/UTIAS_d8.pdf
index f61e5badf3b7670a5a70a8a571d10067ba21b758..2fbc6e7cc5bb3a67888bb0cd5f5559c3d1d20224 100644
Binary files a/CoLo-AT/UTIAS_d8.pdf and b/CoLo-AT/UTIAS_d8.pdf differ
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 6ecd8a96d9e87cca6ae9fdbe0b0a55b9757dd3b1..ae53ffcb4e2b5018885bded3a62c28e30f410d13 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/data_analyzer.py b/CoLo-AT/data_analysis/data_analyzer.py
index b9707e71c65f925d66055f460326ff9b990b2222..30302e3884aa22ef4885c441a9e17d20627b0196 100644
--- a/CoLo-AT/data_analysis/data_analyzer.py
+++ b/CoLo-AT/data_analysis/data_analyzer.py
@@ -285,8 +285,8 @@ class Analyzer():
plt.xlabel('x-axis[m]', fontsize = 22)
plt.ylabel('y-axis[m]', fontsize = 22)
- plt.xlim(-1.5, 1.5)
- plt.ylim(-1.5, 1.5)
+ #plt.xlim(-10, 10)
+ #plt.ylim(-10, 10)
plt.legend(loc='center left', bbox_to_anchor=(1, 0.5))
plt.show()
diff --git a/CoLo-AT/dataset_manager/__pycache__/data_generator.cpython-36.pyc b/CoLo-AT/dataset_manager/__pycache__/data_generator.cpython-36.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..df8ff028a45a82f9c0483bceccbc439794173e95
Binary files /dev/null and b/CoLo-AT/dataset_manager/__pycache__/data_generator.cpython-36.pyc differ
diff --git a/CoLo-AT/dataset_manager/__pycache__/simulated_dataset_manager.cpython-36.pyc b/CoLo-AT/dataset_manager/__pycache__/simulated_dataset_manager.cpython-36.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..02f82922df836837aaa96b9917d3d3959b81c86d
Binary files /dev/null and b/CoLo-AT/dataset_manager/__pycache__/simulated_dataset_manager.cpython-36.pyc differ
diff --git a/CoLo-AT/dataset_manager/__pycache__/simulated_dataset_manager_w.cpython-36.pyc b/CoLo-AT/dataset_manager/__pycache__/simulated_dataset_manager_w.cpython-36.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..995425e8175bc1ff852dc294c58f967c09f771fb
Binary files /dev/null and b/CoLo-AT/dataset_manager/__pycache__/simulated_dataset_manager_w.cpython-36.pyc differ
diff --git a/CoLo-AT/dataset_manager/simulated_dataset_manager_w.py b/CoLo-AT/dataset_manager/simulated_dataset_manager_w.py
new file mode 100755
index 0000000000000000000000000000000000000000..e7a30a532d90fb1e1af7856de4ce4393226eee72
--- /dev/null
+++ b/CoLo-AT/dataset_manager/simulated_dataset_manager_w.py
@@ -0,0 +1,213 @@
+# -*- coding: utf-8 -*-
+
+import numpy as np
+from numpy import random
+import sys
+from math import atan2, sqrt, pi
+import random
+from math import cos
+from math import sin
+import math
+import os.path
+'''
+sys.path.insert(0, '/Users/william/Documents/SLAM_Simulation/Simulation-Environment-for-Cooperative-Localization/functions/requests')
+import request_response
+'''
+
+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
+
+
+def find_next_time_idx(array, start_idx, value):
+ i = start_idx
+ try:
+ array[i]
+ except IndexError:
+ i = -1
+ return i
+ while array[i] < value:
+ i = i+1
+ try:
+ array[i]
+ except IndexError:
+ i = -1
+ break
+ return i
+
+def linear_interpolation(end0, end1, x):
+ [x0, y0] = end0
+ [x1, y1] = end1
+ if x1 == x0:
+ y = y0
+ else:
+ y = y0+(x-x0)*(y1-y0)/(x1-x0)
+ return y
+
+class Sim_Dataset_Manager:
+
+ def __init__(self, dataset_name):
+ self.name = dataset_name
+
+ def circular_path_4_robots(self, duration, op_freqs = [[10, 2, 2], [10, 2, 2], [10, 2, 2], [10, 2, 2]], lm_dict = {6:[0,0]}):
+ self.robot_lables = [1,2,3,4]
+ self.duration = duration
+ self.landmark_map = lm_dict
+ self.op_freqs = np.array(op_freqs) #['prop', 'lm_obser', 'rel_obser', 'comm'] for each robot
+ self.num_op = 3
+ self.op_time_interval = 1/ self.op_freqs
+
+ self.start_time = 0
+ self.starting_states = {}
+ self.starting_states[1] = [self.start_time, 1, 0, pi/2]
+ self.starting_states[2] = [self.start_time, 0, 1, pi]
+ self.starting_states[3] = [self.start_time, -1, 0, 3*pi/2]
+ self.starting_states[4] = [self.start_time, 0, -1, 2*pi]
+
+ self.num_robots = len(self.robot_lables)
+ self.measurement_data = [[] for i in range(self.num_robots)]
+ self.odometry_data = [[] for i in range(self.num_robots)]
+ self.groundtruth_data = [[] for i in range(self.num_robots)]
+ #noise
+ self.noise_v = 0.0125
+ self.noise_range = 0.1
+ self.noise_bearing = 0.035
+
+
+ print ('******** Initialization Completed ********')
+ print("self.start_time: ", self.start_time)
+ print("self.starting_states ", self.starting_states)
+
+ self.current_time = self.start_time
+
+ self.current_states = {}
+ for label in self.robot_lables:
+ self.current_states[label] = {'x': self.starting_states[label][1], 'y': self.starting_states[label][2], 'orientation': self.starting_states[label][3]}
+
+ self.time_arr = np.zeros((self.num_robots, self.num_op))
+ self.dataset_data = {'odometry': self.odometry_data, 'measurement': self.measurement_data, 'groundtruth': self.groundtruth_data}
+ return self.start_time, self.starting_states, self.dataset_data, self.time_arr
+
+ def get_start_time(self):
+ return self.start_time
+
+ def get_starting_states(self):
+ return self.starting_states
+
+ def get_duration(self):
+ return self.duration
+
+ def get_landmark_map(self):
+ return self.landmark_map
+
+ def get_start_moving_times(self):
+ return np.full(self.num_robots, self.start_time)
+
+
+ def respond(self, req, current_time, need_specific_time = False):
+ message = req.get_message()
+ if need_specific_time:
+ valid_respond = False
+ else:
+
+ rbt_idx, op_idx = np.unravel_index(self.time_arr.argmin(), self.time_arr.shape)
+ req_time = self.time_arr[rbt_idx, op_idx]
+ message['robot_index'] = rbt_idx
+ message['time'] = req_time
+
+ robot_actual_state = self.current_states[self.robot_lables[rbt_idx]]
+ gt_x = robot_actual_state['x']
+ gt_y = robot_actual_state['y']
+ gt_orientation = robot_actual_state['orientation']
+
+
+ if op_idx == 0: #propagation
+ req_type = 'odometry'
+ actual_v = np.random.uniform(0.5) # between 0 to 0.5 m/s
+ input_v = actual_v + np.random.normal(0, self.noise_v)
+ actual_av = 0.1
+ delta_t = self.op_time_interval[rbt_idx, op_idx]
+ gt_x = gt_x + actual_v*delta_t*cos(gt_orientation)
+ gt_y = gt_y + actual_v*delta_t*sin(gt_orientation)
+ gt_orientation = gt_orientation + actual_av*delta_t
+
+ message['data'] = {'time':req_time, 'velocity': input_v, 'angular velocity': actual_av, 'orientation': gt_orientation, 'delta_t': delta_t}
+ valid_respond = True
+
+ elif op_idx == 1: # landmark observation
+ req_type = 'measurement'
+ [lm_x, lm_y]= self.landmark_map[6]
+ delta_x = lm_x - gt_x
+ delta_y = lm_y - gt_y
+ meas_range = sqrt(delta_y**2+delta_x**2) + np.random.normal(0, self.noise_range)
+ bearing = math.atan2(delta_y, delta_x) - gt_orientation + np.random.normal(0, self.noise_bearing)
+ message['data'] = {'time':req_time, 'subject_ID':6, 'measurment_range': meas_range, 'bearing':bearing}
+ valid_respond = True
+
+
+ elif op_idx == 2: # relative observation
+ req_type = 'measurement'
+ observed_label = self.robot_lables[(rbt_idx+1)%self.num_robots]
+ obser_x = self.current_states[observed_label]['x']
+ obser_y = self.current_states[observed_label]['y']
+ delta_x = obser_x - gt_x
+ delta_y = obser_y - gt_y
+ meas_range = sqrt(delta_y**2+delta_x**2) + np.random.normal(0, self.noise_range)
+ bearing = math.atan2(delta_y, delta_x) - gt_orientation + np.random.normal(0, self.noise_bearing)
+ message['data'] = {'time':req_time, 'subject_ID': observed_label, 'measurment_range': meas_range, 'bearing':bearing}
+ valid_respond = True
+
+
+ message['groundtruth'] = {'time':req_time, 'x_pos':gt_x, 'y_pos':gt_y, 'orientation':gt_orientation}
+ self.current_states[self.robot_lables[rbt_idx]] = {'x': gt_x, 'y': gt_y, 'orientation': gt_orientation}
+
+ req.set_message(message)
+ req.set_type(req_type)
+
+
+ #time update
+ self.current_time = self.time_arr.min()
+ self.time_arr[rbt_idx, op_idx] += self.op_time_interval[rbt_idx, op_idx]
+
+
+ return valid_respond, self.current_time, req
+
+
+ def get_robot_groundtruth(self, gt_time, robot_index):
+ robot_actual_state = self.current_states[self.robot_lables[robot_index]]
+ gt_x = robot_actual_state['x']
+ gt_y = robot_actual_state['y']
+ gt_orientation = robot_actual_state['orientation']
+
+ return {'time':self.current_time, 'x_pos':gt_x, 'y_pos':gt_y, 'orientation':gt_orientation}
+
+
+ def dataset_reset(self):
+ self.start_time = 0
+ self.starting_states = {}
+ self.starting_states[1] = [self.start_time, 0, 5, pi/2]
+ self.starting_states[2] = [self.start_time, 5, 0, pi]
+ self.starting_states[3] = [self.start_time, 0, -5, 3*pi/2]
+ self.starting_states[4] = [self.start_time, 0, 5, 2*pi]
+
+ self.num_robots = len(self.robot_lables)
+ self.measurement_data = [[] for i in range(self.num_robots)]
+ self.odometry_data = [[] for i in range(self.num_robots)]
+ self.groundtruth_data = [[] for i in range(self.num_robots)]
+
+ print ('******** Initialization Completed ********')
+ print("self.start_time: ", self.start_time)
+ print("self.starting_states ", self.starting_states)
+
+ self.current_time = self.start_time
+
+ self.current_states = {}
+ for label in self.robot_lables:
+ self.current_states[label] = {'x': self.starting_states[label][1], 'y': self.starting_states[label][2], 'orientation': self.starting_states[label][3]}
+
+ self.time_arr = np.zeros(self.num_robots, self.num_op)
+ return self.start_time, self.starting_states, self.dataset_data, self.time_arr
\ No newline at end of file
diff --git a/CoLo-AT/gs_ci_journal.py b/CoLo-AT/gs_ci_journal.py
index 3a0707893832ff70df5ff1fee3a3fdcbab1b3567..f855f966bbe6f789eb8206aa9327b7a39573732f 100644
--- a/CoLo-AT/gs_ci_journal.py
+++ b/CoLo-AT/gs_ci_journal.py
@@ -10,6 +10,7 @@ import os, sys
import getpass
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 Sim_Dataset_Manager
from simulation_process.sim_manager import SimulationManager
from robots.robot_system import RobotSystem
from simulation_process.state_recorder import StatesRecorder
@@ -21,6 +22,14 @@ sys.path.append(os.path.join(os.path.dirname(__file__), "localization_algos"))
from ekf_gs_ci2 import EKF_GS_CI2
from gs_ci_bound import GS_CI_Bound
+
+
+
+
+##############################################################################
+
+
+
# dataset_path = "/Users/shengkangchen/Documents/CoLo/CoLo-D/CoLo-Datasets/official_dataset3/"
dataset_path = "/home/william/CoLo/CoLo-D/CoLo-Datasets/official_dataset1/" # for desktop Ubuntu
diff --git a/CoLo-AT/gs_ci_journal_sim.py b/CoLo-AT/gs_ci_journal_sim.py
new file mode 100644
index 0000000000000000000000000000000000000000..32d081d0fa98d3659ffe72273992605c5d4de7cb
--- /dev/null
+++ b/CoLo-AT/gs_ci_journal_sim.py
@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Sun Apr 8 17:55:06 2018
+
+@author: william
+"""
+
+import os, sys
+import getpass
+sys.path.append(os.path.join(os.path.dirname(__file__), "."))
+from dataset_manager.simulated_dataset_manager_w import Sim_Dataset_Manager
+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
+
+# load algorithms
+sys.path.append(os.path.join(os.path.dirname(__file__), "localization_algos"))
+from ekf_gs_ci2 import EKF_GS_CI2
+from gs_ci_bound import GS_CI_Bound
+
+
+##############################################################################
+duration = 60
+robot_labels = [1,2,3,4]
+testing_dataset = Sim_Dataset_Manager('testing')
+start_time, starting_states, dataset_data, time_arr = testing_dataset.circular_path_4_robots(duration)
+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 = 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)
+
+analyzer.trajectory_plot(state_recorder)
+
+
+#animate_plot(robot_labels, state_recorder, analyzer, testing_dataset.get_landmark_map())
+'''
+##############################################################################
+
+testing_dataset.dataset_reset()
+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 = True)
+
+analyzer.algos_comparison([state_recorder, state_recorder_bound], only_trace = ['gs_ci_bound'])
+'''
\ No newline at end of file
diff --git a/CoLo-AT/j1_rw.py b/CoLo-AT/j1_rw.py
index 220bc51f0ddc1042d6e0ac5b90248161fdc0acfd..4cdbb13f460461d2decd10bdb3e5bbb6fa0de63e 100644
--- a/CoLo-AT/j1_rw.py
+++ b/CoLo-AT/j1_rw.py
@@ -21,9 +21,9 @@ for i in [1, 2, 3, 4]:
datasets_path = "/home/william/CoLo/CoLo-D/UTIAS-dataset/MRCLAM_Dataset" # for desktop Ubuntu
robot_labels = [1,2,3,4,5]
-duration = 100 # duration for the simulation in sec
+duration = 600 # duration for the simulation in sec
-for i in range(1, 2):
+for i in range(1, 9):
dataset_label = str(i)
dataset_path = datasets_path + dataset_label + "/"
graph_name = 'UTIAS_d'+dataset_label
diff --git a/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-36.pyc
index 87b4e002316795055a7b4197a473a3c2f164b637..e0fb6da36a8122d0230cf0396f055b6f7bee79b8 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-36.pyc differ
diff --git a/CoLo-AT/localization_algos/ekf_gs_ci2.py b/CoLo-AT/localization_algos/ekf_gs_ci2.py
index 19d7cadd0b6d571de4a6b9e63ae6950ced29b2d1..9fe960ffb51d3ad49882a0b7e2e9a975e1688c7b 100644
--- a/CoLo-AT/localization_algos/ekf_gs_ci2.py
+++ b/CoLo-AT/localization_algos/ekf_gs_ci2.py
@@ -25,7 +25,9 @@ class EKF_GS_CI2(ekf_algo_framework):
[measurement_data, sensor_covariance] = sensor_data
sigma_odo = sensor_covariance
- var_v = 0.1 # variance of the velocity
+ var_v = 0.5# variance of the velocity
+ exp_v = 0, # expected vecolcity for other robots
+
i = 2*index
num_robots = int(len(s)/2)
delta_t = measurement_data[0]
@@ -40,12 +42,15 @@ class EKF_GS_CI2(ekf_algo_framework):
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
+
for j in range(num_robots):
jj = 2*j
if j==index:
#print(np.matmul(sigma_odo,rot_mtx(self_theta).getT()))
sigma_s[jj:jj+2, jj:jj+2] += delta_t*delta_t*rot_mtx(self_theta)*Q*rot_mtx(self_theta).getT()
else:
+ s[jj,0] = s[jj,0] + cos(orinetations[j])*v*delta_t #x
+ s[jj+1,0] = s[jj+1,0] + sin(orinetations[j])*v*delta_t #y
sigma_s[jj:jj+2, jj:jj+2] += delta_t*delta_t*var_v*np.identity(2)
return [s, orinetations, sigma_s]
@@ -103,7 +108,6 @@ class EKF_GS_CI2(ekf_algo_framework):
phi = measurement_data[2]
j = obser_index * 2
- z = matrix([dis*cos(phi), dis*sin(phi)]).getT()
H_ij = np.zeros((2,2*num_robots))
H_ij[0, i] = -1
@@ -118,6 +122,7 @@ class EKF_GS_CI2(ekf_algo_framework):
delta_x = s.item(j,0) - s.item(i,0)
delta_y = s.item(j+1,0) - s.item(i+1,0)
z_hat = rot_mtx(self_theta).getT()*(np.matrix([delta_x, delta_y]).getT())
+ z = matrix([dis*cos(phi), dis*sin(phi)]).getT()
sigma_ob[1,1] = sigma_ob[1,1]*dis*dis
sigma_z = rot_mtx(phi)*sigma_ob*rot_mtx(phi).getT()
diff --git a/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc b/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc
index 38a616d58716ef5379fc7ebcaac974579cb9ff5b..9269b36a4870d9616b209d71121adb18806b365c 100644
Binary files a/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc and b/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc differ
diff --git a/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-36.pyc b/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-36.pyc
index 2585faca9cf6926677e2db77c4b5894887f9ad0f..7d0b96af6d2bd113c042393330c80a3848183572 100644
Binary files a/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-36.pyc and b/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-36.pyc differ
diff --git a/CoLo-AT/simulation_process/sim_manager.py b/CoLo-AT/simulation_process/sim_manager.py
index a7a2a49bac9907b4773ca5986b37d5e93e710633..494515398848b0bb6c677b3161e970d86e3831ab 100644
--- a/CoLo-AT/simulation_process/sim_manager.py
+++ b/CoLo-AT/simulation_process/sim_manager.py
@@ -25,7 +25,7 @@ class SimulationManager():
def simulated_communication(self, receiver_status, sender_status):
max_range = 5 # in meters
- threshold = 0.2
+ threshold = 0.02
[receiver_idx, receiver_gt] = receiver_status
[sender_idx, sender_gt] = sender_status
@@ -36,12 +36,17 @@ class SimulationManager():
mean = math.exp(-1.5*dis)
std = math.exp(-0.5*dis)
-
+ '''
if numpy.random.normal(mean, std) > threshold:
return True
else:
return False
+ '''
+ if np.random.randint(0,20) <=1:
+ return True
+ else:
+ return False
def allow_operation(self, rsp):
if rsp.get_type()=='measurement':
rbt_idx = rsp.get_robot_index()
@@ -83,10 +88,11 @@ class SimulationManager():
if self.time < prev_time:
print('Time inconsistant!')
break
+ '''
if valid == False :
break
-
- if self.allow_operation(rsp):
+ '''
+ if self.allow_operation(rsp) and valid:
robot_state = robot_system.localization_update(rsp)
state_var.append(robot_state['state variance'])
state_recorder.record_state(rsp, robot_state)
diff --git a/CoLo-AT/simulation_process/state_recorder.py b/CoLo-AT/simulation_process/state_recorder.py
index 2e1d66a94b0627afddbc705f2058ce59c961d4cf..97a6696c01f40786d2c831038387942ced76e1a9 100644
--- a/CoLo-AT/simulation_process/state_recorder.py
+++ b/CoLo-AT/simulation_process/state_recorder.py
@@ -72,7 +72,7 @@ class StatesRecorder():
loc_err = sqrt((est_x_pos-gt_x_pos)*(est_x_pos-gt_x_pos)+(est_y_pos-gt_y_pos)*(est_y_pos-gt_y_pos))
recorded_dataline = [time, robot_label, est_x_pos, est_y_pos, trace_state_var, gt_x_pos, gt_y_pos, loc_err]
-
+ #warning (optional)
'''
if(trace_state_var<0):
print('TIME: ', time+self.start_time)
@@ -87,6 +87,7 @@ class StatesRecorder():
'''
#print(recorded_dataline)
+
self.data_in_time_order.append(recorded_dataline)
self.updata_type_in_time_order.append(updata_type)
self.recorded_data[robot_label].append(recorded_dataline)
diff --git a/CoLo-AT/utias_datastes.pdf b/CoLo-AT/utias_datastes.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..29c36ac9d1bd40bc147c3fd3b7fb805433a341a0
Binary files /dev/null and b/CoLo-AT/utias_datastes.pdf differ