updated animate plot

functions/data_analysis/realtime_plot.py

@@ -191,12 +191,15 @@ def animate_plot(dataset_labels, data_recorder, analyzer, lm = None):
 	ig = initialize_graphs(num_robots, ax_err, ax_var, loc_err, trace_sigma, time_func, colors)
 	arr_err = ig[0]
 	arr_var = ig[1]
-	lm_x = []
-	lm_y = []
-	for landmark_id, [x, y] in lm.items():
-		lm_x.append(x)
-		lm_y.append(y)
-	ax.plot(lm_x, lm_y)
+	
+	if lm != None:
+		lm_x = []
+		lm_y = []
+		for landmark_id, [x, y] in lm.items():
+			lm_x.append(x)
+			lm_y.append(y)
+		ax.scatter(lm_x, lm_y, s=120, marker=(5,0), label='Landmark')
+
 	# obtain robot timestamps
 	times = get_robot_times(num_robots, robot_loc_time_unit)
 	min_time_arr = min_robot_times(times)

functions/robots/robot_system.py

@@ -75,7 +75,8 @@ class RobotSystem:
 		if rsp_type == 'odometry': #propogation update 
 			update_type = 'propagation'
 			v = message_data['velocity']
-			sigma_odo = np.matrix([[5.075*v, 0], [0, 0.1]]) #with respect to velocity and orientation
+			#sigma_odo = np.matrix([[5.075*v, 0], [0, 0.1]]) #with respect to velocity and orientation for utias datasets
+			sigma_odo = np.matrix([[0.01, 0], [0, 0.5]]) #with respect to velocity and orientation
 			sensor_covariance = sigma_odo
 			#delta_t = message_data['time'] - self.prev_prop_times[robot_index]
 			delta_t = message_data['delta_t']
@@ -89,7 +90,8 @@ class RobotSystem:
 			self.prev_prop_times[robot_index] = message_data['time']
 
 		elif rsp_type == 'measurement':
-			sigma_obser = np.matrix([[0.15, 0], [0,0.1]]) #with respect to range and bearing
+
+			sigma_obser = np.matrix([[0.015, 0], [0,0.01]]) #with respect to range and bearing
 			sensor_covariance = sigma_obser
 			obj_id = message_data['subject_ID']
 			meas_range = message_data['measurment_range'] 

functions/ros_compatibility.py

@@ -31,10 +31,10 @@ from ekf_gs_ci2 import EKF_GS_CI2
 #dataset_path = '/home/william/UTIAS-dataset/MRCLAM_Dataset3/'
 #dataset_path = '/home/william/full_tests/full_test9/'
 compname = getpass.getuser()
-dataset_path = "/home/"+ compname +"/full_tests/full_test_v2_4/"
+dataset_path = "/home/"+ compname +"/full_tests/full_test_v2_5/"
 
 dataset_labels = [1,2]
-duration = 200 # duration for the simulation in sec
+duration = 240 # duration for the simulation in sec
 testing_dataset = Dataset('testing')
 start_time, starting_states, dataset_data, time_arr = testing_dataset.load_MRCLAMDatasets(dataset_path, dataset_labels, duration, delay_start = 0)
 loc_algo = Simple_EKF('algo')

functions/ros_demo.py

@@ -28,10 +28,10 @@ from ekf_gs_ci2 import EKF_GS_CI2
 #dataset_path = '/home/william/catkin_ws/ros_colo_dataset/'
 #dataset_path = '/home/william/UTIAS-dataset/MRCLAM_Dataset3/'
 compname = getpass.getuser()
-dataset_path = "/home/"+ compname +"/full_tests/full_test_v2_4/"
+dataset_path = "/home/"+ compname +"/full_tests/full_test_v2_5/"
 
 dataset_labels = [1, 2]
-duration = 200 # duration for the simulation in sec
+duration = 240 # duration for the simulation in sec
 testing_dataset = Dataset('testing')
 start_time, starting_states, dataset_data, time_arr = testing_dataset.load_MRCLAMDatasets(dataset_path, dataset_labels, duration, delay_start = 0)
 loc_algo = Centralized_EKF('algo')