diff --git a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
index a3de0fa087b6e55495ef56af49b11b043cd7ee98..84b30b6fa371573d3ecac84d67a995ae082a5314 100644
--- a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
@@ -35,14 +35,14 @@ uav_int_max:  0.1 #/* maximum integral*/
 # ugv
 
 # Uav
-ugv_c1_alpha: 0.6
+ugv_c1_alpha: 1
 ugv_c2_alpha: 1 # this is recomputed
 ugv_c1_beta: 0.35
 ugv_c2_beta: 1 # this is recomputed
 ugv_c1_gamma: 0.25
 ugv_c2_gamma: 0.2
-ugv_c1_theta: 0.0
-ugv_c1_delta: 0.0
+ugv_c1_theta: 0.1
+ugv_c1_delta: 0.07
 
 
 ugv_d: 0.3
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 74e20c01a3248768a2ecd0511146b41c9ea5bf53..fbedf3f0005851b2060deb57fa7094645592d76b 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 9ea81e6d87d8792f9f0fd5bbb5816c54559ddffb..ee77381d7aeb8dbff3f9a5c7f2cf4fca872a8a8e 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 9514750b3d8a9eb85167f5475afc56d268d2658a..36a5c940ae4b846b5a1866d9178d545057b75ec3 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 35def27d1c69aa081d999250c8590d2032f5752f..296740ee93ec7843cbb38291ac85e0399e8017a6 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 b66f35f94372af493b07a90d8014b3b2c3b5ef6f..5b1556170df7ab1f92b20bf6e9efa5e46b22733d 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 6eb011322452d18d8f9ec49c9c89faed97ee67d5..4bb156b1803e31ae132cccba0c7592a30fe62f7f 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
@@ -177,8 +177,8 @@ 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")
 
 
 
@@ -191,9 +191,9 @@ if N_ugvs > 0:
 #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];
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 b44e84203514b03b4e73e9a8d7ac28a1c853bf0a..19e3a292448f8ab79d09b5e3523807a37aabaef9 100644
--- a/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/mission_controller.cpp
@@ -446,7 +446,7 @@ Eigen::MatrixXf missionController::compute_cluster(Eigen::MatrixXf uav_q, Eigen:
   else
   {
 
-    std::vector<Point2f> centers;;
+    std::vector<Point2f> centers;
     // for (int i = 0; i <num_ugvs; i++)
     // {
     //   /* make intial ugv be the centroid */
@@ -496,12 +496,14 @@ Eigen::MatrixXf missionController::compute_cluster(Eigen::MatrixXf uav_q, Eigen:
 
     /* this is intended for two ugvs, it computed the energy to mantain leader cluster,
     and change, if energy in both ugv to change is higher, the switch is perfomred*/
+
+
     if(num_ugvs == 2)
     {
-      float man1 = (centroids.col(0) - previous_centroids.col(0)).norm();
-      float man2 = (centroids.col(1) - previous_centroids.col(0)).norm();
-      float cha1 = (centroids.col(1) - previous_centroids.col(1)).norm();
-      float cha2 = (centroids.col(0) - previous_centroids.col(1)).norm();
+      float man1 = (centroids.col(0) - uav_q.col(0)).norm();
+      float man2 = (centroids.col(1) - uav_q.col(1)).norm();
+      float cha1 = (centroids.col(1) - uav_q.col(0)).norm();
+      float cha2 = (centroids.col(0) - uav_q.col(1)).norm();
 
       if( cha1 < man1 && cha2 <man2)
       {
@@ -520,7 +522,7 @@ Eigen::MatrixXf missionController::compute_cluster(Eigen::MatrixXf uav_q, Eigen:
         // bad ondding
         if(this->cluster_initialized)
         {
-          centroids = previous_centroids;
+          centroids = centroids;
 
         }
       }