diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_1_logdata.bag
index eccb7117fd4d31ba6a693f8bc6ecc6e5489a7e24..74e20c01a3248768a2ecd0511146b41c9ea5bf53 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_1_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_2_logdata.bag
index 8ee0186eb0b0d41eec31c80d66f04d9585362a95..9ea81e6d87d8792f9f0fd5bbb5816c54559ddffb 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_2_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_3_logdata.bag
index 027965c29d555ad7d39d362b32a0d27e04de7e8c..9514750b3d8a9eb85167f5475afc56d268d2658a 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_3_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_1_logdata.bag
index b90896bb766ee03d414658cfe71987d50b268d7d..35def27d1c69aa081d999250c8590d2032f5752f 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_1_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_2_logdata.bag
index 66fd6291d7740aa0fef43c5784d5ee3f3b773131..b66f35f94372af493b07a90d8014b3b2c3b5ef6f 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_2_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_6_formation_3_uav_2_ugv/result_omniwheel_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/scripts/plot_real_experiment_pgf_plot.py b/real_robot/coordination_formation_control_pkg/scripts/plot_real_experiment_pgf_plot.py
index 862f361ae8e1131cc47a8ce620080993b7b0e7e6..6eb011322452d18d8f9ec49c9c89faed97ee67d5 100644
--- a/real_robot/coordination_formation_control_pkg/scripts/plot_real_experiment_pgf_plot.py
+++ b/real_robot/coordination_formation_control_pkg/scripts/plot_real_experiment_pgf_plot.py
@@ -24,8 +24,8 @@ from matplotlib import pyplot as plt
 ### obtain information from topic
 # basic configurations
 result_folder_name = "/home/spot/masters_thesis_stevedan/formation_coordination_uav_ugv/real_robot/coordination_formation_control_pkg/results"
-file_name = "/experiment_6_formation_3_uav_2_ugv/result_crazyflie_1_logdata.bag"
-topic_name = "crazyflie_1" + "result"
+file_name = "/experiment_6_formation_3_uav_2_ugv/result_crazyflie_2_logdata.bag"
+topic_name = "crazyflie_2" + "result"
 bag = rosbag.Bag(result_folder_name + file_name)
 
 N_uavs = 3
@@ -177,28 +177,29 @@ axis_traj.plot(position_x[-1,:], position_y[-1,:], ".",marker="*",markersize=10,
 if N_ugvs > 0:
     axis_traj.plot(ugv_position_x[0,:], ugv_position_y[0,:], ".",marker="s",markersize=12, markeredgecolor="black", markerfacecolor="black")
     axis_traj.plot(ugv_position_x[-1,:], ugv_position_y[-1,:], ".",marker="p",markersize=12, markeredgecolor="black", markerfacecolor="black")
-    axis_traj.plot(ugv_cluster_x[0,:], ugv_cluster_y[0,:], ".",marker="h",markersize=12, markeredgecolor="black", markerfacecolor="black")
-    axis_traj.plot(ugv_cluster_x[-1,:], ugv_cluster_y[-1,:], ".",marker="+",markersize=12, markeredgecolor="black", markerfacecolor="black")
+    # axis_traj.plot(ugv_cluster_x[0,:], ugv_cluster_y[0,:], ".",marker="h",markersize=12, markeredgecolor="black", markerfacecolor="black")
+    # axis_traj.plot(ugv_cluster_x[-1,:], ugv_cluster_y[-1,:], ".",marker="+",markersize=12, markeredgecolor="black", markerfacecolor="black")
 
 
 
 #plot final configuration
-axis_traj.plot(np.append(position_x[-1,:],position_x[-1,0]) , np.append(position_y[-1,:],position_y[-1,0]),"r")
-# if N_ugvs > 0:
-#     axis_traj.plot(np.append(ugv_position_x[-1,:],ugv_cluster_x[-1,:]) , np.append(ugv_position_y[-1,:],ugv_cluster_y[-1,:]),"k")
-#     axis_traj.plot(np.append(ugv_position_x[-1,:],ugv_cluster_x[-1,:]) , np.append(ugv_position_y[-1,:],ugv_cluster_y[-1,:]),"k")
+# axis_traj.plot(np.append(position_x[-1,:],position_x[-1,0]) , np.append(position_y[-1,:],position_y[-1,0]),"r")
+if N_ugvs > 0:
+    for i in range(N_ugvs):
+        axis_traj.plot(np.append(ugv_position_x[-1,i],ugv_cluster_x[-1,i]) , np.append(ugv_position_y[-1,i],ugv_cluster_y[-1,i]),"k")
+        axis_traj.plot(np.append(ugv_position_x[-1,i],ugv_cluster_x[-1,i]) , np.append(ugv_position_y[-1,i],ugv_cluster_y[-1,i]),"k")
 #plot trajectories
 color_array = ["c", "g","b", "y", "m", "C1", "C2"]
 # plot the uavs
-for i in range(N_uavs):
-    c = color_array[i];
-    axis_traj.plot(position_x[:,i], position_y[:,i], c,label="UAV_" + str(i+1))
+# for i in range(N_uavs):
+#     c = color_array[i];
+#     axis_traj.plot(position_x[:,i], position_y[:,i], c,label="UAV_" + str(i+1))
 
 for t in range(N_ugvs):
     c = color_array[t+N_uavs];
     c1 = color_array[t+N_uavs+2];
-    axis_traj.plot(ugv_position_x[:,t], ugv_position_y[:,t],c+"--",label="UGV_" + str(t+1))
-    axis_traj.plot(ugv_cluster_x[:,t], ugv_cluster_y[:,t],c1+"--",label="Cluster_" + str(t+1))
+    # axis_traj.plot(ugv_position_x[:,t], ugv_position_y[:,t],c+"--",label="UGV_" + str(t+1))
+    axis_traj.plot(ugv_cluster_x[:,t], ugv_cluster_y[:,t],c1+"--",label="Cluster_" + str(t+1),marker="*",markersize=10)
 
 # plot the obstacle
 if n_obstacles >0:
diff --git a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
index 89b1afbf7967840a07f9668428c61a58f5b4a4d6..169941d6df118e3a2278d0595d025bd085ecc95f 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -410,7 +410,7 @@ int main(int argc, char **argv) {
       // std::cout << "vy: transformed: " << vxl << "vy transformed: " << vyl << std::endl;
 
 
-      vel_cmd_publisher.publish(current_command);
+      // vel_cmd_publisher.publish(current_command);
 
     }
 
@@ -465,7 +465,7 @@ int main(int argc, char **argv) {
         logdata->ugv_current_pose[i].position.x = current_ugv_state.q(0);
         logdata->ugv_current_pose[i].position.y = current_ugv_state.q(1);
         logdata->ugv_cluster[i].position.x = next_waypoint->cluster_pos[i*2];
-        // logdata->ugv_cluster[i].position.x = next_waypoint->cluster_pos[i*2+1];
+        logdata->ugv_cluster[i].position.y = next_waypoint->cluster_pos[i*2+1];
       }
       logdata->command_pose.x = current_waypoint.q(0);
       logdata->command_pose.y = current_waypoint.q(1);
diff --git a/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp b/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
index 3b0c4a7dc26ac683c9086cf0f1d7d6b9235ff51c..b44e84203514b03b4e73e9a8d7ac28a1c853bf0a 100644
--- a/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
@@ -340,7 +340,7 @@ for (int i = 0; i ++; i < n_ugvs)
           /* send goals*/
           if (dist < this->eps) // waypoint reached TODO, this is hard coded, not a good idea
           {
-            std::cout << "here" << std::endl;
+            // std::cout << "here" << std::endl;
             if(this->mantain_position == false)
             {
               this->current_waypoint.stop = true;
@@ -528,7 +528,7 @@ Eigen::MatrixXf missionController::compute_cluster(Eigen::MatrixXf uav_q, Eigen:
     }
     previous_centroids = centroids;
     // std::cout << "final centroids" << centroids << std::endl;
-this->cluster_initialized = true;
+    this->cluster_initialized = true;
     return centroids;
 
   }