diff --git a/real_robot/coordination_formation_control_pkg/README.md b/real_robot/coordination_formation_control_pkg/README.md
index 03ff56ea2b6f6ab7aa0bcb5c73289cfe8e3461fd..27c8d89b954db09c4752beeab9d0b54606504450 100644
--- a/real_robot/coordination_formation_control_pkg/README.md
+++ b/real_robot/coordination_formation_control_pkg/README.md
@@ -16,7 +16,7 @@ implement modules
 - [X] send command should include conversion to velocity
 - [X] test omniwheels node
 - [X] Implement swam controller
--
+- [X] Filter the values f the roll
 
 
 
diff --git a/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml b/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml
index ccb63c8d161409967bd8c20b9074d7bdda110517..d529233646db9da3c211c014156fa04e12114a3a 100644
--- a/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml
@@ -1,16 +1,16 @@
 crazyflies:
-- id: 1
-  channel: 90
-  initialPosition: [0.0, 0.0, 0.0]
-  type: default
-- id: 2
-  channel: 90
-  initialPosition: [0.0, 0.0, 0.0]
-  type: default
-# - id: 3
-#   channel: 80
+# - id: 1
+#   channel: 90
 #   initialPosition: [0.0, 0.0, 0.0]
 #   type: default
+# - id: 2
+#   channel: 90
+#   initialPosition: [0.0, 0.0, 0.0]
+#   type: default
+- id: 3
+  channel: 80
+  initialPosition: [0.0, 0.0, 0.0]
+  type: default
 # - id: 4
 #   channel: 80
 #   initialPosition: [0.0, 0.0, 0.0]
diff --git a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
index 3f680fc5c077e922031d90d4c5605555a169ea81..1eefca24cdcc0efc141519fd08090ce0b8c23bbe 100644
--- a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
@@ -18,7 +18,7 @@ uav_c1_delta: 0.09
 # uav_c1_delta: 0.01
 
 uav_d: 0.8
-uav_formation_t:  2 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+uav_formation_t:  3 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
 uav_k: 7
 uav_ratio:  0.8 # /* ratio between dist inter and obstacle*/
 uav_eps: 0.1
diff --git a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
index 9b0637d34543286b538d50305af4856c53e97c24..3a992a98f66d4a25d82a118cbc0d1c7e7e1308b4 100644
--- a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
@@ -1,4 +1,4 @@
-n_drones: 2
+n_drones: 3
 n_ugvs: 0
 hz_freq: 50 # controller frquency
 mass: 33 # grams
diff --git a/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch b/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch
index f5bfd85f40f78dcbf0b44013a5b6791dc0f18d30..98e4a1c51596b96687ec0a27c25f442414e175be 100644
--- a/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch
+++ b/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch
@@ -10,11 +10,11 @@
     <rosparam>
       # Logging configuration (Use enable_logging to actually enable logging)
       genericLogTopics: ["position", "velocity","acceleration", "angle"]
-      genericLogTopicFrequencies: [50,50,50]
+      genericLogTopicFrequencies: [50,50,50,50]
       genericLogTopic_position_Variables: ["stateEstimateZ.x", "stateEstimateZ.y","stateEstimateZ.z", "stateEstimate.yaw"]
       genericLogTopic_velocity_Variables: ["stateEstimateZ.vx", "stateEstimateZ.vy","stateEstimateZ.vz"]
       genericLogTopic_acceleration_Variables: ["stateEstimateZ.ax", "stateEstimateZ.ay","stateEstimateZ.az"]
-      genericLogTopic_orientation_Variables: [ "stateEstimate.roll", "stateEstimate.pitch", "stateEstimate.yaw"]
+      genericLogTopic_angle_Variables: [ "stateEstimate.roll", "stateEstimate.pitch", "stateEstimate.yaw"]
       # firmware parameters for all drones (use crazyflieTypes.yaml to set per type, or
       # allCrazyflies.yaml to set per drone)
       firmwareParams:
diff --git a/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch b/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
index b302d49cfaf497d06d278fd18c1285197e6fbc52..c72aeb0240cb2eb5695346f38195e2a19dc55a8d 100644
--- a/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
+++ b/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
@@ -8,7 +8,7 @@
     output="screen"/>
 
   <!-- Initialize all the drones first -->
-  <group ns="uav_1">
+  <!-- <group ns="uav_1">
        <node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="1" launch-prefix="xterm -e">
        </node>
    </group>
@@ -16,11 +16,11 @@
    <group ns="uav_2">
         <node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="2" launch-prefix="xterm -e">
         </node>
-    </group>
-    <!-- <group ns="uav_3">
-         <node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="3" launch-prefix="xterm -e">
+    </group> -->
+    <group ns="uav_3">
+         <node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="3" launch-prefix="xterm -e gdb --args">
          </node>
-     </group> -->
+     </group>
 
 
      <!-- <group ns="uav_4">
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_9_working/test_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_9_working/test_crazyflie_1_logdata.bag
deleted file mode 100644
index 11204819d3ce10bca124bed0d83222c58bbd6a6a..0000000000000000000000000000000000000000
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_9_working/test_crazyflie_1_logdata.bag and /dev/null differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_9_working/test_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_9_working/test_crazyflie_2_logdata.bag
deleted file mode 100644
index 0de3b9517f341b32bdf3876959c3ffd26ec9ac8e..0000000000000000000000000000000000000000
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_9_working/test_crazyflie_2_logdata.bag and /dev/null differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_9_working_filter_data/test_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_9_working_filter_data/test_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..ca595d66ac42f6aed57701fefa3da5d42ed49d7e
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_9_working_filter_data/test_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_9_working_filter_data/test_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_9_working_filter_data/test_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..ca595d66ac42f6aed57701fefa3da5d42ed49d7e
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_9_working_filter_data/test_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_9_working_filter_data/test_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_9_working_filter_data/test_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..a2fbfd1c5964343eefcb62c2253e991958042f4e
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_9_working_filter_data/test_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/scripts/plot_results.py b/real_robot/coordination_formation_control_pkg/scripts/plot_results.py
index 6f8aab6d82e502d8c9a880fd4f6ef8475e496f3b..1c483ef842d557084a76c9be11169f9a67db9e0d 100644
--- a/real_robot/coordination_formation_control_pkg/scripts/plot_results.py
+++ b/real_robot/coordination_formation_control_pkg/scripts/plot_results.py
@@ -289,24 +289,26 @@ axs6[0, 1].legend(robot_names)
 axs6[1, 0].set_ylabel("v(m/s)")
 axs6[1, 0].set_xlabel("time(s)")
 axs6[1, 0].legend(robot_names)
-plt.show()
 
 ####################### roll vs pitch ########################################
 # Start plotting
 ####################### command vs centroid ###################################
-fig1, axs1 = plt.subplots(nrows=2, ncols=1, figsize=(7, 7))
-st = fig1.suptitle("Roll vs pitch command sent to drone and current value", fontsize="x-large")
-
-# vx
-axs1[0, 0].plot(t_np, command_pose_np[:,0])
-axs1[0, 0].plot(t_np, angle[:,0])
-axs1[0, 0].set_ylabel("roll(m)")
-axs1[0, 0].set_xlabel("time(s)")
-axs1[0, 0].legend(["command", "state"])
-
-
-axs1[0, 1].plot(t_np, command_pose_np[:,1])
-axs1[0, 1].plot(t_np, angle[:,1])
-axs1[0, 1].set_ylabel("pitch(m)")
-axs1[0, 1].set_xlabel("time(s)")
-axs1[0, 1].legend(["command", "state"])
+fig7, axs7 = plt.subplots(nrows=2, ncols=2, figsize=(7, 7))
+st = fig7.suptitle("Roll vs pitch command sent to drone and current value", fontsize="x-large")
+# row = t_np.shape
+# print(str(row) + " " + str(row))
+#
+# # vx
+axs7[0, 0].plot(t_np, command_pose_np[:,0])
+axs7[0, 0].plot(t_np, ang_data_np[:,0])
+axs7[0, 0].set_ylabel("roll(m)")
+axs7[0, 0].set_xlabel("time(s)")
+axs7[0, 0].legend(["command", "state"])
+
+
+axs7[0, 1].plot(t_np, command_pose_np[:,1])
+axs7[0, 1].plot(t_np, ang_data_np[:,1])
+axs7[0, 1].set_ylabel("pitch(m)")
+axs7[0, 1].set_xlabel("time(s)")
+axs7[0, 1].legend(["command", "state"])
+plt.show()
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 3b9ab7a4869b251753b4504269371aa40778a648..48c26155526f2d450704026795d8f6a18d48431b 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -254,9 +254,9 @@ int main(int argc, char **argv) {
       uav_state_p(1, i-1) = current_state.p(1);
       uav_state_a(0, i-1) = current_state.a(0);
       uav_state_a(1, i-1) = current_state.a(1);
-      uav_state_a(0, i-1) = current_state.o(0);
-      uav_state_a(1, i-1) = current_state.o(1);
-      uav_state_a(2, i-1) = current_state.o(2);
+      uav_state_o(0, i-1) = current_state.o(0);
+      uav_state_o(1, i-1) = current_state.o(1);
+      uav_state_o(2, i-1) = current_state.o(2);
 
     }
     // std::cout << uav_state_q << std::endl;
@@ -350,8 +350,8 @@ int main(int argc, char **argv) {
           // we are saturating the acceleration
           range << -2, 2;
           row_vector_2d_t out = saturate(in, range);
-          input_to_system_x = -out(1)*180.0/(mass*M_PI);
-          input_to_system_y =  out(0)*180.0/(mass*M_PI);
+          input_to_system_x = current_command.roll; //-out(1)*180.0/(mass*M_PI);
+          input_to_system_y =  current_command.pitch; //out(0)*180.0/(mass*M_PI);
           // current_command.roll = input_to_system_x; // conver it to the desired angle
           // current_command.pitch =input_to_system_y;
 
diff --git a/real_robot/coordination_formation_control_pkg/src/robot_state.cpp b/real_robot/coordination_formation_control_pkg/src/robot_state.cpp
index 936d9d23bbadeebe670b8babfd5d46b0b30fd3a4..8bab8871238a7ba8cb006140492e655792e9bd6e 100644
--- a/real_robot/coordination_formation_control_pkg/src/robot_state.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/robot_state.cpp
@@ -116,6 +116,7 @@ void robotState::stateCallbackUavPos(const crazyswarm::GenericLogDataConstPtr &p
 void robotState::stateCallbackUavAng(const crazyswarm::GenericLogDataConstPtr &ang_msg)
 {
   // all in degrees
+  // ROS_INFO("Here");
   this->state.o(0) = ang_msg->values[0];
   this->state.o(1) = ang_msg->values[1];
   this->state.o(2) = ang_msg->values[2];