diff --git a/CoLo-AT/colo_demo.gif b/CoLo-AT/colo_demo.gif
index 11814815779625121911f8e0665deb5244182b7f..510b42ad54e08dacc55ab4fa5ab76470af32d3cb 100644
Binary files a/CoLo-AT/colo_demo.gif and b/CoLo-AT/colo_demo.gif differ
diff --git a/CoLo-AT/data_analysis/__pycache__/__init__.cpython-36.pyc b/CoLo-AT/data_analysis/__pycache__/__init__.cpython-36.pyc
index a6c36afd728c74bcb0dfce9bb6db5c63562fdade..7951a34af129990fbec3808b660de5d9b931859d 100644
Binary files a/CoLo-AT/data_analysis/__pycache__/__init__.cpython-36.pyc and b/CoLo-AT/data_analysis/__pycache__/__init__.cpython-36.pyc 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 9099e8d5a14e68fe056753339e4b41849ba094a8..71e848bdc21f0e4085fd1f8b06af55246286a44c 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 6b1a42fb26002c6c84ef44c593f5cfb6ffc15f86..3a8dd54cec228a7daaec9f21364b2a5091cbcb31 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/dataset_manager/__pycache__/__init__.cpython-36.pyc b/CoLo-AT/dataset_manager/__pycache__/__init__.cpython-36.pyc
index d15e459c547ee60f84d0fd9af17f5941c3593eb0..cb88c3505e2b2c5e6c1ee380f0a77cd9c0a344d3 100644
Binary files a/CoLo-AT/dataset_manager/__pycache__/__init__.cpython-36.pyc and b/CoLo-AT/dataset_manager/__pycache__/__init__.cpython-36.pyc differ
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 5ed681eb5c3773249ce5512ba5e08b9c1e174c34..f73b2cf653e3579d4ded1a002c64063cda1cb593 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/gs_ci_journal.py b/CoLo-AT/gs_ci_journal.py
new file mode 100644
index 0000000000000000000000000000000000000000..91ee0212f07fdd62f0520436112a5473b39e36be
--- /dev/null
+++ b/CoLo-AT/gs_ci_journal.py
@@ -0,0 +1,54 @@
+#!/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.realworld_dataset_manager import RW_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
+
+# 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)
+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)
+
+#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'])
diff --git a/CoLo-AT/icra2019_demo.py b/CoLo-AT/icra2019_demo.py
index a7a21cb03ae8245b7ab3cf744df75538113671d0..69956e1540fec673f4c1c484ac9307c57b320484 100644
--- a/CoLo-AT/icra2019_demo.py
+++ b/CoLo-AT/icra2019_demo.py
@@ -21,7 +21,9 @@ sys.path.append(os.path.join(os.path.dirname(__file__), "localization_algos"))
from centralized_ekf import Centralized_EKF
from ekf_gs_ci2 import EKF_GS_CI2
-dataset_path = "/Users/shengkangchen/Documents/CoLo/CoLo-D/CoLo-Datasets/official_dataset3/"
+# 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 = 120 # duration for the simulation in sec
testing_dataset = RW_Dataset_Manager('testing')
diff --git a/CoLo-AT/localization_algos/__pycache__/centralized_ekf.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/centralized_ekf.cpython-36.pyc
index 841baa9ca29af8beb5f008ea2df788530bc20823..b622a79aa5e6e569d0346267045d4f76a5f9e92b 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/centralized_ekf.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/centralized_ekf.cpython-36.pyc differ
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 8fcbaef524ac506c9b9676683c55297759c8b32a..87b4e002316795055a7b4197a473a3c2f164b637 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/__pycache__/gs_ci_bound.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-36.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..83bf5d28309a25e87b1bc794f5adbd1a8d27d0d4
Binary files /dev/null and b/CoLo-AT/localization_algos/__pycache__/gs_ci_bound.cpython-36.pyc differ
diff --git a/CoLo-AT/localization_algos/__pycache__/localization_algo_framework.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/localization_algo_framework.cpython-36.pyc
index eec902bc7e08e4b527973ba0b2a34ecf567d64a3..0bca44984a733081df2ca92b4174ffa27ef07b52 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/localization_algo_framework.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/localization_algo_framework.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 5743b3fd332172fcf35da58acb01d5f7a8effba7..19d7cadd0b6d571de4a6b9e63ae6950ced29b2d1 100644
--- a/CoLo-AT/localization_algos/ekf_gs_ci2.py
+++ b/CoLo-AT/localization_algos/ekf_gs_ci2.py
@@ -53,8 +53,8 @@ class EKF_GS_CI2(ekf_algo_framework):
def absolute_obser_update(self, robot_data, sensor_data):
[s, orinetations, 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]
diff --git a/CoLo-AT/localization_algos/gs_ci_bound.py b/CoLo-AT/localization_algos/gs_ci_bound.py
index 74e482c0b35a179d6fda78b7fb1512de9f406183..21f007c61988f990f1272db1bae82861295e9249 100644
--- a/CoLo-AT/localization_algos/gs_ci_bound.py
+++ b/CoLo-AT/localization_algos/gs_ci_bound.py
@@ -1,67 +1,60 @@
import numpy as np
from numpy import matrix
from math import cos, sin, atan2, sqrt
+from localization_algo_framework import ekf_algo_framework
def rot_mtx(theta):
return matrix([[cos(theta), -sin(theta)], [sin(theta), cos(theta)]])
-class gs_ci_bound():
+class GS_CI_Bound(ekf_algo_framework):
def __init__(self, algo_name):
self.algo_name = algo_name
- self.d_max = 4.5 # max measurement distance
+ self.d_max = 2 # max measurement distance
self.d_var = 0.2
self.bearing_var = 0.2
- self.var_v = 0.5
+ self.var_v = 0.2
- def state_veriance_init(self, num_robots):
+ def state_variance_init(self, num_robots):
return 0.01*np.matrix(np.identity(2*num_robots), dtype = float)
- def algo_update(self, robot_data, update_type, sensor_data):
- print 'in th algo update'
- #return [s, orinetations, sigma_s]
- [s, odo_freq, th_sigma_s, index] = robot_data
- [measurement_data, sensor_covariance] = sensor_data
-
- if update_type == 'propagation update':
- print 'th bound propagation'
- return self.propagation_update(s, th_sigma_s, sensor_covariance, index, odo_freq)
- elif update_type == 'landmark observation update':
- return self.absolute_obser_update(s, th_sigma_s, index)
- elif update_type == 'relative observation update':
- obser_index = measurement_data[0]
- return self.relative_obser_update(s, th_sigma_s, index, obser_index)
- elif update_type == 'communication':
- return self.communication(s, th_sigma_s, index, measurement_data)
- else:
- print "invalid update"
-
+ def calculate_trace_state_variance(self, robot_data):
+ [s, orinetations, sigma_s, index] = robot_data
+ i = 2*index
+ trace_state_var = np.trace(sigma_s[i:i+2, i:i+2])
+ return trace_state_var
- def calculate_trace_state_variance(self, state_variance, robot_index):
- #j = robot_index*2
- #return np.trace(state_variance[j:j+1, j:j+1])
- return np.trace(state_variance)
+ #def propagation_update(self, s, th_sigma_s, sigma_odo, index, odo_freq):
+ 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
- def propagation_update(self, s, th_sigma_s, sigma_odo, index, odo_freq):
- num_robots = len(s)/2
+ num_robots = int(len(s)/2)
i = 2*index
- delta_t = 1/odo_freq
+ #delta_t = 1/odo_freq
+ delta_t = measurement_data[0]
for j in range(num_robots):
jj = 2*j
if j==index:
- th_sigma_s[jj:jj+2, jj:jj+2] += sigma_odo[0,0]*np.identity(2)*delta_t*delta_t
+ sigma_s[jj:jj+2, jj:jj+2] += sigma_odo[0,0]*np.identity(2)*delta_t*delta_t
else:
#var_v = pow(2*self.d_max, 2)/12
var_v = self.var_v
- th_sigma_s[jj:jj+2, jj:jj+2] += var_v*np.identity(2)*delta_t*delta_t
- print 'bound'
- print np.trace(state_variance)
- return th_sigma_s
+ sigma_s[jj:jj+2, jj:jj+2] += var_v*np.identity(2)*delta_t*delta_t
+
+ #return th_sigma_s
+ return [s, orinetations, sigma_s]
+
+ #def absolute_obser_update(self, s, th_sigma_s, index):
+ def absolute_obser_update(self, robot_data, sensor_data):
+ [s, orinetations, sigma_s, index] = robot_data
+ [measurement_data, sensor_covariance] = sensor_data
+ sigma_ob = sensor_covariance
- def absolute_obser_update(self, s, th_sigma_s, index):
- num_robots = len(s)/2
+ num_robots = int(len(s)/2)
i = 2*index
H_i = np.matrix(np.zeros((2,2*num_robots)))
H_i[0, i] = -1
@@ -71,22 +64,25 @@ class gs_ci_bound():
var_dis = self.d_var
var_phi = self.bearing_var
- #sigma_th_z = np.matrix(max(sigma_d, d_max*d_max*sigma_bearing)*np.identity(2)) #sigme_Ri
- #th_sigma_s = (th_sigma_s.getI()+H_i.getT()*sigma_th_z.getI()*H_i).getI()
-
sigma_th_z = np.matrix(max(var_dis, d_max*d_max*var_phi)*np.identity(2))
- th_sigma_s = (th_sigma_s.getI() + H_i.getT() * sigma_th_z.getI() * H_i).getI()
+ sigma_s = (sigma_s.getI() + H_i.getT() * sigma_th_z.getI() * H_i).getI()
- return th_sigma_s
+ return [s, orinetations, sigma_s]
- def relative_obser_update(self, s, th_sigma_s, index, obser_index):
+ #def relative_obser_update(self, s, th_sigma_s, index, obser_index):
#i obser j
+ def relative_obser_update(self, robot_data, sensor_data):
+ #when robot i observes robot j
+ [s, orinetations, sigma_s, index] = robot_data
+ [measurement_data, sensor_covariance] = sensor_data
+ sigma_ob = sensor_covariance
- i = 2*index
- j = obser_index * 2
+ num_robots = int(len(s)/2)
- num_robots = len(s)/2
+ i = 2*index
+ obser_index = measurement_data[0]
+ j = obser_index * 2
H_ij = np.matrix(np.zeros((2,2*num_robots)))
H_ij[0, i] = -1
@@ -99,43 +95,19 @@ class gs_ci_bound():
var_phi = self.bearing_var
sigma_th_z = np.matrix(max(var_dis, d_max*d_max*var_phi)*np.identity(2))
- th_sigma_s = (th_sigma_s.getI() + H_ij.getT() * sigma_th_z.getI() * H_ij).getI()
- return th_sigma_s
-
-
- def communication(self, s, th_sigma_s, index, measurement_data):
-
- e = 0.5
- '''
- sender_th_sigma_s = measurement_data
- rec_th_sigma_s = th_sigma_s
- sig_inv = e*rec_th_sigma_s.getI() + (1-e)*sender_th_sigma_s.getI()
- th_sigma_s = sig_inv.getI()
-
- '''
- '''
- i = 1*2
- j = 2*2
+ sigma_s = (sigma_s.getI() + H_ij.getT() * sigma_th_z.getI() * H_ij).getI()
+ return [s, orinetations, sigma_s]
- num_robots = len(s)/2
- H_ij = np.matrix(np.zeros((2,2*num_robots)))
- H_ij[0, i] = -1
- H_ij[1, i+1] = -1
- H_ij[0, j] = 1
- H_ij[1, j+1] = 1
-
+ #def communication(self, s, th_sigma_s, index, measurement_data):
+ def communication(self, robot_data, sensor_data):
+ [s, orinetations, sigma_s, index] = robot_data
+ [comm_data, comm_variance] = sensor_data
+ [sender_idx, comm_robot_s, comm_robot_sigma_s]=comm_data
- d_max = self.d_max
- sigma_d = self.d_var
- sigma_bearing = self.bearing_var
- sigma_th_z = np.matrix(max(sigma_d, d_max*d_max*sigma_bearing)*np.identity(2)) #sigme_Ri
- sig_inv = e*th_sigma_s.getI() + (1-e)*(th_sigma_s.getI()+H_ij.getT()*sigma_th_z.getI()*H_ij)
- th_sigma_s = sig_inv.getI()
- '''
+ e = 0.8
- comm_robot_th_sigma = measurement_data
- th_sigma_s = ( e*th_sigma_s.getI() + (1-e)*comm_robot_th_sigma.getI()).getI() # from TK's gs-ci
+ sigma_s = ( e*sigma_s.getI() + (1-e)*comm_robot_sigma_s.getI()).getI() # from TK's gs-ci
- return th_sigma_s
\ No newline at end of file
+ return [s, orinetations, sigma_s]
diff --git a/CoLo-AT/requests/__pycache__/request_response.cpython-36.pyc b/CoLo-AT/requests/__pycache__/request_response.cpython-36.pyc
index 02de2f22087bd952cd1f47b84adff5254474fbc2..02f0b15864eb7b70ac13122aa853f571e3175e7e 100644
Binary files a/CoLo-AT/requests/__pycache__/request_response.cpython-36.pyc and b/CoLo-AT/requests/__pycache__/request_response.cpython-36.pyc differ
diff --git a/CoLo-AT/robots/__pycache__/__init__.cpython-36.pyc b/CoLo-AT/robots/__pycache__/__init__.cpython-36.pyc
index 9fbfa1b359d1539e86fc883a14c1eca9b609c242..3e4279038eff40af456ec8a9a6ad13d41851a75e 100644
Binary files a/CoLo-AT/robots/__pycache__/__init__.cpython-36.pyc and b/CoLo-AT/robots/__pycache__/__init__.cpython-36.pyc differ
diff --git a/CoLo-AT/robots/__pycache__/robot_centralized.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_centralized.cpython-36.pyc
index f398e784201849b2ada4d3786ab8e9f91f15f080..4fb83e19970af4b54c068338068033b446c1d1c9 100644
Binary files a/CoLo-AT/robots/__pycache__/robot_centralized.cpython-36.pyc and b/CoLo-AT/robots/__pycache__/robot_centralized.cpython-36.pyc differ
diff --git a/CoLo-AT/robots/__pycache__/robot_distributive.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_distributive.cpython-36.pyc
index da90b600c29a7f15a8efa73b8285c7ec01ba6880..5bb557ca470f7cead2cc371006431c89563455b5 100644
Binary files a/CoLo-AT/robots/__pycache__/robot_distributive.cpython-36.pyc and b/CoLo-AT/robots/__pycache__/robot_distributive.cpython-36.pyc differ
diff --git a/CoLo-AT/robots/__pycache__/robot_parameters.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_parameters.cpython-36.pyc
index 5c0a10a0b85351858e365c063ea53d687f40cbba..f3b63698f4f2e7abe941a8abd88bd6650ccf3129 100644
Binary files a/CoLo-AT/robots/__pycache__/robot_parameters.cpython-36.pyc and b/CoLo-AT/robots/__pycache__/robot_parameters.cpython-36.pyc differ
diff --git a/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc
index 0a4ab392e06f5b4faa2b3b161afb380bbc100399..a3749f65089c47a4d5adccc0f77e5a142a6440b3 100644
Binary files a/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc and b/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc differ
diff --git a/CoLo-AT/simulation_process/__pycache__/__init__.cpython-36.pyc b/CoLo-AT/simulation_process/__pycache__/__init__.cpython-36.pyc
index 9911a68bb6d5909124222a8960179c2e0a77f33f..69554d904b9048111013c5e978a58d4a14d3ee19 100644
Binary files a/CoLo-AT/simulation_process/__pycache__/__init__.cpython-36.pyc and b/CoLo-AT/simulation_process/__pycache__/__init__.cpython-36.pyc differ
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 2abcfecd1515e8497a8c7f92566c428ba0bfdf5e..16eb67c30e1e3d7f4b2e61a5c101b65f43765efa 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 31aec99f52c7efbe1e48f8d132f91e5c27c28b48..85c0e77429d59df4555496f0556e72434261e905 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/state_recorder.py b/CoLo-AT/simulation_process/state_recorder.py
index cf310934ebdf925c904a6e101323fdd174bc2a24..9a40f1473d162d7d86538578fb72de231307a2c4 100644
--- a/CoLo-AT/simulation_process/state_recorder.py
+++ b/CoLo-AT/simulation_process/state_recorder.py
@@ -10,9 +10,10 @@ import matplotlib.pyplot as plt
class StatesRecorder():
"""docstring for ClassName"""
- def __init__(self, name, dataset_labels):
+ def __init__(self, name, dataset_labels, state_var_only = False ):
self.name = name
self.dataset_labels = dataset_labels
+ self.state_var_only = state_var_only
self.recorded_data = {}
self.data_in_time_order = []
self.updata_type_in_time_order = []
@@ -62,9 +63,14 @@ class StatesRecorder():
gt_x_pos = gt['x_pos']
gt_y_pos = gt['y_pos']
robot_label = self.dataset_labels[robot_idx]
- 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]
+
+ if self.state_var_only:
+ loc_err = 0
+ recorded_dataline = [time, robot_label, gt_x_pos, gt_y_pos, trace_state_var, gt_x_pos, gt_y_pos, loc_err]
+ else:
+ 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]
if(trace_state_var<0):
print('TIME: ', time+self.start_time)
@@ -76,7 +82,7 @@ class StatesRecorder():
print(updata_type)
print('>1 m loc err: ',recorded_dataline)
print(req.get_message())
-
+
#print(recorded_dataline)
self.data_in_time_order.append(recorded_dataline)
self.updata_type_in_time_order.append(updata_type)