diff --git a/real_robot/coordination_formation_control_pkg/include/robot_state.h b/real_robot/coordination_formation_control_pkg/include/robot_state.h
index 1e3ce92c63244f99d36ffd7a68002858eed1f506..1ebaf40383a5454fe27b9580a05a9eeddc2c3165 100644
--- a/real_robot/coordination_formation_control_pkg/include/robot_state.h
+++ b/real_robot/coordination_formation_control_pkg/include/robot_state.h
@@ -23,6 +23,7 @@ private:
   ros::Subscriber state_sub_pose;
   ros::Subscriber state_sub_vel;
   ros::Subscriber state_sub_acc;
+  ros::Subscriber state_sub_ang;
 
   ros::NodeHandle nh;
   /* not an effective way*/
@@ -40,6 +41,7 @@ private:
   void stateCallbackUavVel(const crazyswarm::GenericLogDataConstPtr &vel_msg);
   void stateCallbackUavPos(const crazyswarm::GenericLogDataConstPtr &pose_msg);
   void stateCallbackUavAcc(const crazyswarm::GenericLogDataConstPtr &Acc_msg);
+  void stateCallbackUavAng(const crazyswarm::GenericLogDataConstPtr &Ang_msg);
 
 
 public:
diff --git a/real_robot/coordination_formation_control_pkg/include/shared_definitions.h b/real_robot/coordination_formation_control_pkg/include/shared_definitions.h
index f5d7f3b94025911ce47fc8e194d6107a4c340edd..bf3ae2ade49c36ac4d3fb34974769be8ba4bb146 100644
--- a/real_robot/coordination_formation_control_pkg/include/shared_definitions.h
+++ b/real_robot/coordination_formation_control_pkg/include/shared_definitions.h
@@ -28,6 +28,7 @@ typedef enum formation_matrix
 typedef  struct robot_state
 {
   Vector4d q; /*position 3 position 1 orientation yaw*/
+  Vector3d o; /* orientation */
   Vector3d p; /* velocity*/
   Vector3d a; /* acceleration*/
 }robot_state_t;
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 279a0cb28bc17f0d2be76804b7179efa6baf8de2..f5bfd85f40f78dcbf0b44013a5b6791dc0f18d30 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
@@ -9,11 +9,12 @@
   <node pkg="crazyswarm" type="crazyswarm_server" name="crazyswarm_server" output="screen" >
     <rosparam>
       # Logging configuration (Use enable_logging to actually enable logging)
-      genericLogTopics: ["position", "velocity","acceleration"]
+      genericLogTopics: ["position", "velocity","acceleration", "angle"]
       genericLogTopicFrequencies: [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"]
       # 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/msg/logData.msg b/real_robot/coordination_formation_control_pkg/msg/logData.msg
index 9816f0758f69bcdece5b74d3229f16f1438ea873..d579484745da76df1edf7cc2a5a54f4940e2b3d1 100644
--- a/real_robot/coordination_formation_control_pkg/msg/logData.msg
+++ b/real_robot/coordination_formation_control_pkg/msg/logData.msg
@@ -11,3 +11,4 @@ geometry_msgs/Vector3 input_navigation
 geometry_msgs/Vector3 input_obstacle
 geometry_msgs/Vector3 input_integration
 geometry_msgs/Vector3 input_to_system
+geometry_msgs/Vector3 angle
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 65f3a9c7a69b18bfdd83aa1063e6e05f6f91b35c..6f8aab6d82e502d8c9a880fd4f6ef8475e496f3b 100644
--- a/real_robot/coordination_formation_control_pkg/scripts/plot_results.py
+++ b/real_robot/coordination_formation_control_pkg/scripts/plot_results.py
@@ -21,6 +21,7 @@ t_ = []
 pose_data = []
 vel_data = []
 acc_data = []
+ang_data = []
 command_pose_data = []
 command_twist_data = []
 input_data = []
@@ -34,6 +35,7 @@ input_to_system_data = []
 pose_row_ = [0] * n_robots_logged*2;
 twist_row_ = [0] * n_robots_logged*2;
 acc_row_ = [0] * n_robots_logged*2;
+# ang_row_ = [0] * n_robots_logged*3;
 
 
 # converting data to numpy for easy plotting
@@ -49,6 +51,10 @@ for topics, msg, t in bag.read_messages(topics =[topic_name]):
         twist_row_[ind*2+1] = msg.current_twist[ind].linear.y
         acc_row_[ind*2] = msg.current_acc[ind].linear.x
         acc_row_[ind*2+1] = msg.current_acc[ind].linear.y
+        # ang_row_[ind*2] = msg.angle[ind].x
+        # ang_row_[ind*2+1] = msg.angle[ind].y
+        # ang_row_[ind*2+2] = msg.angle[ind].z
+
     # print(pose_row_)
     # print(pose_row_)
     t_.append(msg.header.stamp.secs + msg.header.stamp.nsecs*1e-9);
@@ -58,6 +64,7 @@ for topics, msg, t in bag.read_messages(topics =[topic_name]):
     # time.sleep(1);
     vel_data.append(twist_row_[:]);
     acc_data.append(acc_row_[:]);
+    ang_data.append([msg.angle.x, msg.angle.y]);
     command_pose_data.append([msg.command_pose.x, msg.command_pose.y, msg.command_pose.theta]);
     command_twist_data.append([msg.command_twist.linear.x, msg.command_twist.linear.y]);
     input_data.append([msg.input.x, msg.input.y])
@@ -79,6 +86,8 @@ pose_data_np = np.array(pose_data);
 # print(pose_data_np)
 vel_data_np = np.array(vel_data);
 acc_data_np = np.array(acc_data);
+ang_data_np = np.array(ang_data);
+
 x_list = range(0,n_robots_logged*2,2);
 y_list = range(1,n_robots_logged*2,2);
 x_list_np = np.array(pose_data_np[:,x_list]);
@@ -281,3 +290,23 @@ 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"])
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 d651e99bb6f4d20a008041de92d673269a9e91a6..3b9ab7a4869b251753b4504269371aa40778a648 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -91,6 +91,7 @@ int main(int argc, char **argv) {
   logdata->current_pose.resize(n_drones);
   logdata->current_twist.resize(n_drones);
   logdata->current_acc.resize(n_drones);
+  // logdata->angle.resize(n_drones);
 
   /* current command*/
   crazyswarm::ZDistance current_command;
@@ -104,6 +105,7 @@ int main(int argc, char **argv) {
   Eigen::MatrixXf uav_state_q(2,n_drones);
   Eigen::MatrixXf uav_state_p(2,n_drones);
   Eigen::MatrixXf uav_state_a(2,n_drones);
+  Eigen::MatrixXf uav_state_o(3,n_drones);
 
   // obstacle_t *current_obs = new obstacle_t[n_obs]();
   std::vector<obstacle_t> current_obs;
@@ -233,6 +235,7 @@ int main(int argc, char **argv) {
 
 
   ros::Rate loop_rate(hz_freq);
+  float input_to_system_x = 0.0, input_to_system_y = 0.0;
 
 
   /* main loop*/
@@ -251,6 +254,10 @@ 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);
+
     }
     // std::cout << uav_state_q << std::endl;
     if(coord_config->initialize_robot && coord_config->init_formation ==false && coord_config->stop_demo == false)
@@ -343,8 +350,11 @@ int main(int argc, char **argv) {
           // we are saturating the acceleration
           range << -2, 2;
           row_vector_2d_t out = saturate(in, range);
-          current_command.roll = -out(1)*180.0/(mass*M_PI); // conver it to the desired angle
-          current_command.pitch = out(0)*180.0/(mass*M_PI);
+          input_to_system_x = -out(1)*180.0/(mass*M_PI);
+          input_to_system_y =  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;
+
           // vxx = vxl;
           // vyy = vyl;
         }
@@ -409,12 +419,17 @@ int main(int argc, char **argv) {
       {
         logdata->current_pose[i].position.x = uav_state_q(0,i);
         logdata->current_pose[i].position.y = uav_state_q(1,i);
+
+
         // std::cout <<(uav_state_q(1,i)) << std::endl;
         logdata->current_twist[i].linear.x = uav_state_p(0,i);
         logdata->current_twist[i].linear.y = uav_state_p(1,i);
         logdata->current_acc[i].linear.x = uav_state_a(0,i);
         logdata->current_acc[i].linear.y = uav_state_a(1,i);
       }
+      logdata->angle.x =  uav_state_o(0,robotid-1);
+      logdata->angle.y =  uav_state_o(1,robotid-1);
+      logdata->angle.z =  uav_state_o(2,robotid-1);
       logdata->command_pose.x = current_waypoint.q(0);
       logdata->command_pose.y = current_waypoint.q(1);
       logdata->command_pose.theta = current_waypoint.orientation;
@@ -432,8 +447,8 @@ int main(int argc, char **argv) {
       logdata->input_orientation.y = input(1,4);
       logdata->input_integration.x = input(0,5);
       logdata->input_integration.y = input(1,5);
-      logdata->input_to_system.x = current_command.roll;
-      logdata->input_to_system.y = current_command.pitch;
+      logdata->input_to_system.x = input_to_system_x;
+      logdata->input_to_system.y =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 2b8d78300e2c6b6146dbf8e38aaf6496270e5508..936d9d23bbadeebe670b8babfd5d46b0b30fd3a4 100644
--- a/real_robot/coordination_formation_control_pkg/src/robot_state.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/robot_state.cpp
@@ -12,22 +12,29 @@ t(robot_ind.t)
   std::stringstream ss_pose;
   std::stringstream ss_vel;
   std::stringstream ss_acc;
+  std::stringstream ss_ang;
 
   if (this->t == CRAZYFLIE)
   {
     ss_pose << "/" << robot_name << std::to_string(this->id) << "/position";
     ss_vel << "/" << robot_name << std::to_string(this->id) << "/velocity";
     ss_acc << "/" << robot_name << std::to_string(this->id) << "/acceleration";
+    ss_ang << "/" << robot_name << std::to_string(this->id) << "/angle";
 
     // uav_sync.registerCallback( boost::bind( &robotState::stateCallbackUav, this, _1, _2 ) );
     this->state_sub_pose = nh.subscribe(ss_pose.str(), 1,&robotState::stateCallbackUavPos, this);
     this->state_sub_vel = nh.subscribe(ss_vel.str(), 1,&robotState::stateCallbackUavVel, this);
     this->state_sub_acc = nh.subscribe(ss_acc.str(), 1,&robotState::stateCallbackUavAcc, this);
+    this->state_sub_ang = nh.subscribe(ss_ang.str(), 1,&robotState::stateCallbackUavAng, this);
 
     ROS_INFO("Crazyflie %d state subcribed", this->id );
     ros::topic::waitForMessage< crazyswarm::GenericLogData >(ss_vel.str(), ros::Duration(5));
     ros::topic::waitForMessage< crazyswarm::GenericLogData>(ss_pose.str(), ros::Duration(5));
     ros::topic::waitForMessage< crazyswarm::GenericLogData>(ss_acc.str(), ros::Duration(5));
+    ros::topic::waitForMessage< crazyswarm::GenericLogData>(ss_ang.str(), ros::Duration(5));
+
+
+
 
     sucess_initialization = true;
 
@@ -86,6 +93,10 @@ void robotState::stateCallbackUgvPose(const geometry_msgs::PoseConstPtr &pose_ms
 
   tf::Matrix3x3(quat).getRPY(roll, pitch,yaw);
   this->state.q(3) = yaw*180.0/M_PI;  // degres to mantain consistency
+  this->state.o(0) = 0;//yaw*180.0/M_PI;  // degres to mantain consistency
+  this->state.o(1) = 0; // yaw*180.0/M_PI;  // degres to mantain consistency
+  this->state.o(2) = yaw*180.0/M_PI; // yaw*180.0/M_PI;  // degres to mantain consistency
+
 }
 void robotState::stateCallbackUavVel(const crazyswarm::GenericLogDataConstPtr &vel_msg)
 {
@@ -102,6 +113,15 @@ void robotState::stateCallbackUavPos(const crazyswarm::GenericLogDataConstPtr &p
 
 }
 
+void robotState::stateCallbackUavAng(const crazyswarm::GenericLogDataConstPtr &ang_msg)
+{
+  // all in degrees
+  this->state.o(0) = ang_msg->values[0];
+  this->state.o(1) = ang_msg->values[1];
+  this->state.o(2) = ang_msg->values[2];
+
+}
+
 void robotState::stateCallbackUgvAcc(const geometry_msgs::Vector3ConstPtr &acc_msg)
 {