diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllertabs.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllertabs.h
index ec3e39c1cfe27b9458786587a06707a4a1bee88b..c8bbfba70b79396c3b88c5c9854ebffc48a18d13 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllertabs.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllertabs.h
@@ -157,7 +157,9 @@ private:
 
     // SUBSRIBER
     // > For the pose data from a motion capture system
-    ros::Subscriber m_poseDataSubscriber;
+    ros::Subscriber m_poseDataMocapSubscriber;
+    // > For the pose data from a onboard state estimate
+    ros::Subscriber m_poseDataOnboardSubscriber;
     // > For the controller that is currently operating
     ros::Subscriber controllerUsedSubscriber;
 
@@ -171,10 +173,16 @@ private:
     // --------------------------------------------------- //
     // PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
 
+    // > For the data from the motion capture system, received on
+    //   "m_poseDataMocapSubscriber"
+    void poseDataMocapReceivedCallback(const dfall_pkg::ViconData& viconData);
+
+    // > For the data from onboard the flying agent, received on
+    //   "m_poseDataOnboardSubscriber"
+    void poseDataOnboardReceivedCallback(const dfall_pkg::FlyingVehicleState& vehicleState);
+
     // > For the controller currently operating, received on
     //   "controllerUsedSubscriber"
-    void poseDataReceivedCallback(const dfall_pkg::ViconData& viconData);
-
     void controllerUsedChangedCallback(const std_msgs::Int32& msg);
 
     // Get the paramters that specify the type and ID
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
index fe2cf599acef1c228844213ae0aaed313aff42b8..10fa29b6ea17cf095c7c88fc56da3e037ed36790 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
@@ -209,12 +209,15 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
     ros::NodeHandle nodeHandle_dfall_root("/dfall");
 
     // CREATE THE SUBSCRIBER TO THE MOTION CAPTURE DATA
-    m_poseDataSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 100, &ControllerTabs::poseDataReceivedCallback, this);
+    m_poseDataMocapSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 100, &ControllerTabs::poseDataMocapReceivedCallback, this);
 
-    // CREATE THE SUBSCRIBER TO THE CONTROLLER THAT IS CURRENTLY OPERATING
-    // Only if this is an agent GUI
+    // CREATE THE SUBSCRIBER TO AGENT SPECIFIC TOPICS:
+    // i.e., only if this is an agent GUI
     if (m_type == TYPE_AGENT)
     {
+        // THE STATE ESTIMATE FROM ONBOARD THE FLYING AGENT
+        m_poseDataOnboardSubscriber = nodeHandle_for_this_gui.subscribe("CrazyRadio/CFStateEstimate", 20, &ControllerTabs::poseDataOnboardReceivedCallback, this);
+        // THE CONTROLLER THAT IS CURRENTLY OPERATING
         controllerUsedSubscriber = nodeHandle_for_this_gui.subscribe("FlyingAgentClient/ControllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
     }
 
@@ -361,9 +364,9 @@ void ControllerTabs::setObjectNameForDisplayingPoseData( QString object_name )
 
 
 #ifdef CATKIN_MAKE
-// > For the controller currently operating, received on
-//   "controllerUsedSubscriber"
-void ControllerTabs::poseDataReceivedCallback(const dfall_pkg::ViconData& viconData)
+// > For the data from the motion capture system, received on
+//   "m_poseDataMocapSubscriber"
+void ControllerTabs::poseDataMocapReceivedCallback(const dfall_pkg::ViconData& viconData)
 {
     m_should_search_pose_data_for_object_name_mutex.lock();
     if (m_should_search_pose_data_for_object_name)
@@ -407,6 +410,29 @@ void ControllerTabs::poseDataReceivedCallback(const dfall_pkg::ViconData& viconD
 
 
 
+#ifdef CATKIN_MAKE
+// > For the data from the motion capture system, received on
+//   "m_poseDataOnboardSubscriber"
+void ControllerTabs::poseDataOnboardReceivedCallback(const dfall_pkg::FlyingVehicleState& pose_in_local_frame)
+{
+    //if(pose_in_local_frame.vehicleName == m_object_name_for_emitting_pose_data)
+    //{
+        emit measuredPoseValueChanged(
+                pose_in_local_frame.x,
+                pose_in_local_frame.y,
+                pose_in_local_frame.z,
+                pose_in_local_frame.roll,
+                pose_in_local_frame.pitch,
+                pose_in_local_frame.yaw,
+                pose_in_local_frame.isValid
+            );
+    //}
+}
+#endif
+
+
+
+
 
 //    ----------------------------------------------------------------------------------
 //     CCCC   OOO   N   N  TTTTT  RRRR    OOO   L      L      EEEEE  RRRR
@@ -646,8 +672,11 @@ void ControllerTabs::setAgentIDsToCoordinate(QVector<int> agentIDs , bool should
         }
 
         // SUBSCRIBERS
-        // > For receiving message that the instant controller was changed
+
         m_change_highlighted_controller_mutex.lock();
+        // > For receiving the state estimate from onboard the fying agent
+        m_poseDataOnboardSubscriber = agent_base_nodeHandle.subscribe("CrazyRadio/CFStateEstimate", 20, &ControllerTabs::poseDataOnboardReceivedCallback, this);
+        // > For receiving messages that the instant controller was changed
         controllerUsedSubscriber = agent_base_nodeHandle.subscribe("FlyingAgentClient/ControllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
         m_change_highlighted_controller_mutex.unlock();
     }
@@ -655,6 +684,7 @@ void ControllerTabs::setAgentIDsToCoordinate(QVector<int> agentIDs , bool should
     {
         // Unsubscribe
         m_change_highlighted_controller_mutex.lock();
+        m_poseDataOnboardSubscriber.shutdown();
         controllerUsedSubscriber.shutdown();
         m_change_highlighted_controller_mutex.unlock();
 
diff --git a/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
index a58cbc78e9962a097ff3390a0a0bd8b95edbef8c..4974bcb79d905f6271732b5174e9547d8dd93f4b 100755
--- a/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
+++ b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
@@ -265,9 +265,9 @@ class CrazyRadioClient:
         cfStateEstimate.vy = data["stateEstimateZ.vy"] / 1000.0
         cfStateEstimate.vz = data["stateEstimateZ.vz"] / 1000.0
         # > (x,y,z) accelerations
-        cfStateEstimate.ax = data["stateEstimateZ.ax"] / 1000.0
-        cfStateEstimate.ay = data["stateEstimateZ.ay"] / 1000.0
-        cfStateEstimate.az = data["stateEstimateZ.az"] / 1000.0
+        #cfStateEstimate.ax = data["stateEstimateZ.ax"] / 1000.0
+        #cfStateEstimate.ay = data["stateEstimateZ.ay"] / 1000.0
+        #cfStateEstimate.az = data["stateEstimateZ.az"] / 1000.0
         # > (roll,pitch,yaw) angles
         cfStateEstimate.roll  =  data["stateEstimateZ.roll"]  / 1000.0
         cfStateEstimate.pitch = -data["stateEstimateZ.pitch"] / 1000.0
@@ -354,9 +354,9 @@ class CrazyRadioClient:
         self.logconf_stateEstimate.add_variable("stateEstimateZ.vy",   "int16_t");
         self.logconf_stateEstimate.add_variable("stateEstimateZ.vz",   "int16_t");
         # > (x,y,z) accelerations
-        self.logconf_stateEstimate.add_variable("stateEstimateZ.ax",   "int16_t");
-        self.logconf_stateEstimate.add_variable("stateEstimateZ.ay",   "int16_t");
-        self.logconf_stateEstimate.add_variable("stateEstimateZ.az",   "int16_t");
+        #self.logconf_stateEstimate.add_variable("stateEstimateZ.ax",   "int16_t");
+        #self.logconf_stateEstimate.add_variable("stateEstimateZ.ay",   "int16_t");
+        #self.logconf_stateEstimate.add_variable("stateEstimateZ.az",   "int16_t");
         # > (roll,pitch,yaw) angles
         self.logconf_stateEstimate.add_variable("stateEstimateZ.roll",        "int16_t");
         self.logconf_stateEstimate.add_variable("stateEstimateZ.pitch",       "int16_t");
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h b/dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h
index 21b594e2725722a4b98bb210331dc73cb68c7bae..50bcbf213d7b339bc8254a75b92ed143b48c55cc 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h
@@ -161,16 +161,25 @@ float yaml_battery_polling_period_in_seconds = 0.2f;
 // // > Note that the delay in seconds therefore depends on the polling period
 // int yaml_battery_delay_threshold_to_change_state = 5;
 
-// Frequency of requesting the onboard state estimate, in [seconds]
-float yaml_cfStateEstimate_polling_period_in_seconds = 0.2f;
 
 
 
 
+// VARIABLES FOR SIMULATING A QUAROTOR
 
 // A QUADROTORS SIMULATOR
 QuadrotorSimulator m_quadrotor_sim;
 
+// FREQUENCY OF REQUESTION ON BOARD STATE ESTIMATE, IN [seconds]
+float yaml_cfStateEstimate_polling_period_in_seconds = 0.02f;
+
+// THE COEFFICIENT OF THE 16-BIT COMMAND TO THRUST CONVERSION
+std::vector<float> yaml_motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};
+
+// THE MIN AND MAX FOR SATURATING THE 16-BIT THRUST COMMANDS
+float yaml_command_sixteenbit_min = 1000.0f;
+float yaml_command_sixteenbit_max = 65000.0f;
+
 
 
 
diff --git a/dfall_ws/src/dfall_pkg/param/CrazyRadioConfig.yaml b/dfall_ws/src/dfall_pkg/param/CrazyRadioConfig.yaml
index ff103cac0a90634567a977bf3ce92dbe7fb26747..73270b35ec1bce084b168963f90ba62792d09241 100755
--- a/dfall_ws/src/dfall_pkg/param/CrazyRadioConfig.yaml
+++ b/dfall_ws/src/dfall_pkg/param/CrazyRadioConfig.yaml
@@ -1,2 +1,15 @@
 # Frequency of requesting the onboard state estimate, in [milliseconds]
-cfStateEstimate_polling_period: 20
\ No newline at end of file
+cfStateEstimate_polling_period: 20
+
+
+
+# ----------------------------------------------- #
+# THE FOLLOWING ARE USED FOR EMULATING A CRAZYFLIE:
+
+# QUADRATIC MOTOR REGRESSION EQUATION (a0, a1, a2)
+# > [Newtons] = a2 [cmd]^2 + a1 [cmd] + a0
+motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
+
+# THE MIN AND MAX FOR SATURATING THE 16-BIT THRUST COMMANDS
+command_sixteenbit_min : 1000
+command_sixteenbit_max : 65000
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp b/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp
index 6349dedc830090073ffd9904bc142c7905b8c30c..b36f14648625c46374f8e9ee68fe48287412f18f 100644
--- a/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp
+++ b/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp
@@ -250,6 +250,10 @@ void QuadrotorSimulator::simulate_for_one_time_step(float deltaT)
 		this->m_euler_angles[0] = new_roll;
 		this->m_euler_angles[1] = new_pitch;
 		this->m_euler_angles[2] = new_yaw;
+		// > For the Euler angular velocities
+		this->m_euler_velocities[0] = command_body_rate_x;
+		this->m_euler_velocities[1] = command_body_rate_y;
+		this->m_euler_velocities[2] = command_body_rate_z;
 
 
 
@@ -259,6 +263,8 @@ void QuadrotorSimulator::simulate_for_one_time_step(float deltaT)
 		{
 			// Reset the state
 			this->reset();
+			// Inform the user
+			ROS_INFO_STREAM("[QUADROTOR SIMULATOR] Quadrotor \"" << this->m_id_string << "\" reset due to roll or pitch exceeding 80 degrees." );
 		}
 
 	} // END OF: "if (this->m_isFlying)"
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp
index 9bad1acfb9b9bc610732b27aad616ef122b3de41..2d9556930e577a2e9882b06f1b34649621e5ee2f 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp
@@ -296,9 +296,40 @@ void timerCallback_update_battery_voltage(const ros::TimerEvent&)
 
 void timerCallback_update_cfStateEstimate(const ros::TimerEvent&)
 {
-	// Declare static paramters for the voltage
-	static float voltage_current = 4.2;
-	
+	// Simulate the quadrotor for one time step
+	m_quadrotor_sim.simulate_for_one_time_step( yaml_cfStateEstimate_polling_period_in_seconds );
+
+	// Local variable for the data of this quadrotor
+	FlyingVehicleState quadrotor_data;
+
+	// Fill in the details
+	// > For the type
+	quadrotor_data.type = FLYING_VEHICLE_STATE_TYPE_CRAZYFLIE_STATE_ESTIMATE;
+	// > For the name
+	quadrotor_data.vehicleName = m_quadrotor_sim.get_id_string();
+	// > For the occulsion flag
+	quadrotor_data.isValid = true;
+	// > For position
+	quadrotor_data.x = m_quadrotor_sim.m_position[0];
+	quadrotor_data.y = m_quadrotor_sim.m_position[1];
+	quadrotor_data.z = m_quadrotor_sim.m_position[2];
+	// > For velocities
+	quadrotor_data.vx = m_quadrotor_sim.m_velocity[0];
+	quadrotor_data.vy = m_quadrotor_sim.m_velocity[1];
+	quadrotor_data.vz = m_quadrotor_sim.m_velocity[2];
+	// > For euler angles
+	quadrotor_data.roll  = m_quadrotor_sim.m_euler_angles[0];
+	quadrotor_data.pitch = m_quadrotor_sim.m_euler_angles[1];
+	quadrotor_data.yaw   = m_quadrotor_sim.m_euler_angles[2];
+	// > For euler angular rates
+	quadrotor_data.rollRate  = m_quadrotor_sim.m_euler_velocities[0];
+	quadrotor_data.pitchRate = m_quadrotor_sim.m_euler_velocities[1];
+	quadrotor_data.yawRate   = m_quadrotor_sim.m_euler_velocities[2];
+	// > For the acquiring time
+	quadrotor_data.acquiringTime = 0.0;
+
+	// Publish the current state
+	cfStateEstimatorPublisher.publish(quadrotor_data);
 }
 
 
@@ -419,8 +450,17 @@ void fetchCrazyRadioConfigYamlParameters(ros::NodeHandle& nodeHandle)
 	ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "CrazyRadioConfig");
 
 	// Frequency of requesting the battery voltage, in [milliseconds]
-	yaml_cfStateEstimate_polling_period_in_seconds = getParameterFloat(nodeHandle_for_paramaters,"cfStateEstimate_polling_period");
-	yaml_cfStateEstimate_polling_period_in_seconds = yaml_cfStateEstimate_polling_period_in_seconds * 0.001;
+	float cfStateEstimate_polling_period_in_milliseconds = getParameterFloat(nodeHandle_for_paramaters,"cfStateEstimate_polling_period");
+	yaml_cfStateEstimate_polling_period_in_seconds = cfStateEstimate_polling_period_in_milliseconds * 0.001;
+
+	// THE COEFFICIENT OF THE 16-BIT COMMAND TO THRUST CONVERSION
+	getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
+
+	// THE MIN AND MAX FOR SATURATING THE 16-BIT THRUST COMMANDS
+	yaml_command_sixteenbit_min = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_min");
+	yaml_command_sixteenbit_max = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_max");
+
+
 
 	// DEBUGGING: Print out one of the parameters that was loaded
 	ROS_INFO_STREAM("[CRAZY RADIO EMULATOR] DEBUGGING: the fetched CrazyRadioConfig/cfStateEstimate_polling_period = " << yaml_cfStateEstimate_polling_period_in_seconds);
@@ -530,7 +570,7 @@ int main(int argc, char* argv[])
 	fetchBatteryMonitorYamlParameters(nodeHandle_to_own_agent_parameter_service);
 
 	// Call the class function that loads the parameters for this class.
-	fetchCrazyRadioConfigYamlParameters(nodeHandle_to_own_agent_parameter_service);
+	fetchCrazyRadioConfigYamlParameters(nodeHandle);
 
 
 
@@ -549,15 +589,27 @@ int main(int argc, char* argv[])
 
 
 	// INITIALISE THE QUADROTOR SIMULATOR
-	// > First conver the agentID to a zero-padded string
+	// > First convert the agentID to a zero-padded string
 	// Convert the agent ID to a zero padded string
 	std::ostringstream str_stream;
-	str_stream << std::setw(3) << std::setfill('0') << m_agentID;
+	str_stream << std::setw(2) << std::setfill('0') << m_agentID;
 	std::string agentID_as_string(str_stream.str());
 	// > Initialise the quadrotor simulator class variable
-	QuadrotorSimulator m_quadrotor_sim = QuadrotorSimulator( "CFSIM" + agentID_as_string , 0.032  );
+	m_quadrotor_sim = QuadrotorSimulator( "CF" + agentID_as_string , 0.032  );
+	// Set the agent id of quadrotor simulator
+	// > This determines which message topic the quadrotor
+	//   simulator subscribes to for commands
+	m_quadrotor_sim.update_commanding_agent_id( m_agentID );
+	// Set the reset state
+	m_quadrotor_sim.setResetState_xyz_yaw(0.0,0.0,0.0,0.0);
+	// Set the parameters for the 16-bit command to thrust conversion
+	m_quadrotor_sim.setParameters_for_16bitCommand_to_thrust_conversion(yaml_motorPoly[0], yaml_motorPoly[1], yaml_motorPoly[2], yaml_command_sixteenbit_min, yaml_command_sixteenbit_max);
+	// Reset the quadrotor
+	m_quadrotor_sim.reset();
 	// > Inform the user
 	ROS_INFO_STREAM("[CRAZY RADIO EMULATOR] Initilised quadrotor simulation with ID = " << m_quadrotor_sim.get_id_string() << ", and mass in kg = " << m_quadrotor_sim.get_mass_in_kg() );
+	ROS_INFO("[CRAZY RADIO EMULATOR] Initilised quadrotor simulation with details:");
+	m_quadrotor_sim.print_details();
 
 
 
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
index c36e7bc497435e8484cd07be8a07035974266ac0..638b98de6a140f6b502d3d1dd55f451e05b77487 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
@@ -799,73 +799,98 @@ void smoothSetpointChanges( float target_setpoint[4] , float (&current_setpoint)
 void performEstimatorUpdate_forStateInterial(Controller::Request &request)
 {
 
-	// PUT THE CURRENT MEASURED DATA INTO THE CLASS VARIABLE
-	// > for (x,y,z) position
-	m_current_xzy_rpy_measurement[0] = request.ownCrazyflie.x;
-	m_current_xzy_rpy_measurement[1] = request.ownCrazyflie.y;
-	m_current_xzy_rpy_measurement[2] = request.ownCrazyflie.z;
-	// > for (roll,pitch,yaw) angles
-	m_current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
-	m_current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
-	m_current_xzy_rpy_measurement[5] = request.ownCrazyflie.yaw;
+	if (request.ownCrazyflie.type == FLYING_VEHICLE_STATE_TYPE_MOCAP_MEASUREMENT)
+	{
 
+		// PUT THE CURRENT MEASURED DATA INTO THE CLASS VARIABLE
+		// > for (x,y,z) position
+		m_current_xzy_rpy_measurement[0] = request.ownCrazyflie.x;
+		m_current_xzy_rpy_measurement[1] = request.ownCrazyflie.y;
+		m_current_xzy_rpy_measurement[2] = request.ownCrazyflie.z;
+		// > for (roll,pitch,yaw) angles
+		m_current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
+		m_current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
+		m_current_xzy_rpy_measurement[5] = request.ownCrazyflie.yaw;
 
-	// RUN THE FINITE DIFFERENCE ESTIMATOR
-	performEstimatorUpdate_forStateInterial_viaFiniteDifference(request.isFirstControllerCall);
 
+		// RUN THE FINITE DIFFERENCE ESTIMATOR
+		performEstimatorUpdate_forStateInterial_viaFiniteDifference(request.isFirstControllerCall);
 
-	// RUN THE POINT MASS KALMAN FILTER ESTIMATOR
-	performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(request.isFirstControllerCall);
 
+		// RUN THE POINT MASS KALMAN FILTER ESTIMATOR
+		performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(request.isFirstControllerCall);
 
-	// FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
-	switch (yaml_estimator_method)
-	{
-		// Estimator based on finte differences
-		case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
+
+		// FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
+		switch (yaml_estimator_method)
 		{
-			// Transfer the estimate
-			for(int i = 0; i < 9; ++i)
+			// Estimator based on finte differences
+			case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
 			{
-				m_current_stateInertialEstimate[i]  = m_stateInterialEstimate_viaFiniteDifference[i];
+				// Transfer the estimate
+				for(int i = 0; i < 9; ++i)
+				{
+					m_current_stateInertialEstimate[i]  = m_stateInterialEstimate_viaFiniteDifference[i];
+				}
+				break;
 			}
-			break;
-		}
-		// Estimator based on Point Mass Kalman Filter
-		case ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION:
-		{
-			// Transfer the estimate
-			for(int i = 0; i < 9; ++i)
+			// Estimator based on Point Mass Kalman Filter
+			case ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION:
 			{
-				m_current_stateInertialEstimate[i]  = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
+				// Transfer the estimate
+				for(int i = 0; i < 9; ++i)
+				{
+					m_current_stateInertialEstimate[i]  = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
+				}
+				break;
 			}
-			break;
-		}
-		// Handle the exception
-		default:
-		{
-			// Display that the "yaml_estimator_method" was not recognised
-			ROS_INFO_STREAM("[DEFAULT CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'DefaultControllerService': the 'yaml_estimator_method' is not recognised.");
-			// Transfer the finite difference estimate by default
-			for(int i = 0; i < 9; ++i)
+			// Handle the exception
+			default:
 			{
-				m_current_stateInertialEstimate[i]  = m_stateInterialEstimate_viaFiniteDifference[i];
+				// Display that the "yaml_estimator_method" was not recognised
+				ROS_INFO_STREAM("[DEFAULT CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'DefaultControllerService': the 'yaml_estimator_method' is not recognised.");
+				// Transfer the finite difference estimate by default
+				for(int i = 0; i < 9; ++i)
+				{
+					m_current_stateInertialEstimate[i]  = m_stateInterialEstimate_viaFiniteDifference[i];
+				}
+				break;
 			}
-			break;
 		}
-	}
 
 
-	// NOW THAT THE ESTIMATORS HAVE ALL BEEN RUN, PUT THE CURRENT
-	// MEASURED DATA INTO THE CLASS VARIABLE FOR THE PREVIOUS 
-	// > for (x,y,z) position
-	m_previous_xzy_rpy_measurement[0] = m_current_xzy_rpy_measurement[0];
-	m_previous_xzy_rpy_measurement[1] = m_current_xzy_rpy_measurement[1];
-	m_previous_xzy_rpy_measurement[2] = m_current_xzy_rpy_measurement[2];
-	// > for (roll,pitch,yaw) angles
-	m_previous_xzy_rpy_measurement[3] = m_current_xzy_rpy_measurement[3];
-	m_previous_xzy_rpy_measurement[4] = m_current_xzy_rpy_measurement[4];
-	m_previous_xzy_rpy_measurement[5] = m_current_xzy_rpy_measurement[5];
+		// NOW THAT THE ESTIMATORS HAVE ALL BEEN RUN, PUT THE CURRENT
+		// MEASURED DATA INTO THE CLASS VARIABLE FOR THE PREVIOUS 
+		// > for (x,y,z) position
+		m_previous_xzy_rpy_measurement[0] = m_current_xzy_rpy_measurement[0];
+		m_previous_xzy_rpy_measurement[1] = m_current_xzy_rpy_measurement[1];
+		m_previous_xzy_rpy_measurement[2] = m_current_xzy_rpy_measurement[2];
+		// > for (roll,pitch,yaw) angles
+		m_previous_xzy_rpy_measurement[3] = m_current_xzy_rpy_measurement[3];
+		m_previous_xzy_rpy_measurement[4] = m_current_xzy_rpy_measurement[4];
+		m_previous_xzy_rpy_measurement[5] = m_current_xzy_rpy_measurement[5];
+	}
+
+	else if (request.ownCrazyflie.type == FLYING_VEHICLE_STATE_TYPE_CRAZYFLIE_STATE_ESTIMATE)
+	{
+		// Transfer the onboard estimate directly
+		// > For position
+		m_current_stateInertialEstimate[0]  = request.ownCrazyflie.x;
+		m_current_stateInertialEstimate[1]  = request.ownCrazyflie.y;
+		m_current_stateInertialEstimate[2]  = request.ownCrazyflie.z;
+		// > For velocities
+		m_current_stateInertialEstimate[3]  = request.ownCrazyflie.vx;
+		m_current_stateInertialEstimate[4]  = request.ownCrazyflie.vy;
+		m_current_stateInertialEstimate[5]  = request.ownCrazyflie.vz;
+		// > For euler angles
+		m_current_stateInertialEstimate[6]  = request.ownCrazyflie.roll;
+		m_current_stateInertialEstimate[7]  = request.ownCrazyflie.pitch;
+		m_current_stateInertialEstimate[8]  = request.ownCrazyflie.yaw;
+	}
+	else
+	{
+		ROS_ERROR_STREAM("[DEFAULT CONTROLLER] Received a request.ownCrazyflie with unrecognised type, request.ownCrazyflie.type = " << request.ownCrazyflie.type );
+	}
 }
 
 
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
index ba64d0847db933703ec9d5f1b766c1d2d567287e..ec9872d5fca0a7eed7e7b474c171a3ca5eb497c3 100755
--- a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
@@ -1495,22 +1495,22 @@ void isReadyFlyingAgentClientConfigYamlCallback(const IntWithHeader & msg)
 // > Load the paramters from the Client Config YAML file
 void fetchFlyingAgentClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
 {
-    // Here we load the parameters that are specified in the file:
-    // FlyingAgentClientConfig.yaml
+	// Here we load the parameters that are specified in the file:
+	// FlyingAgentClientConfig.yaml
 
-    // Add the "FlyingAgentClientConfig" namespace to the "nodeHandle"
-    ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "FlyingAgentClientConfig");
+	// Add the "FlyingAgentClientConfig" namespace to the "nodeHandle"
+	ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "FlyingAgentClientConfig");
 
-    // Flags for which measurement streams to use
-    yaml_shouldUse_mocapMeasurements = getParameterBool(nodeHandle_for_paramaters,"shouldUse_mocapMeasurements");
+	// Flags for which measurement streams to use
+	yaml_shouldUse_mocapMeasurements = getParameterBool(nodeHandle_for_paramaters,"shouldUse_mocapMeasurements");
 	yaml_shouldUse_onboardEstimate   = getParameterBool(nodeHandle_for_paramaters,"shouldUse_onboardEstimate");
 
-    // Flag for whether to use angle for switching to the Safe Controller
-    yaml_isEnabled_strictSafety = getParameterBool(nodeHandle_for_paramaters,"isEnabled_strictSafety");
-    yaml_maxTiltAngle_for_strictSafety_degrees = getParameterFloat(nodeHandle_for_paramaters,"maxTiltAngle_for_strictSafety_degrees");
+	// Flag for whether to use angle for switching to the Safe Controller
+	yaml_isEnabled_strictSafety = getParameterBool(nodeHandle_for_paramaters,"isEnabled_strictSafety");
+	yaml_maxTiltAngle_for_strictSafety_degrees = getParameterFloat(nodeHandle_for_paramaters,"maxTiltAngle_for_strictSafety_degrees");
 
-    // Number of consecutive occulsions that will deem the
-    // object as "long-term" occuled
+	// Number of consecutive occulsions that will deem the
+	// object as "long-term" occuled
 	yaml_consecutive_invalid_threshold = getParameterInt(nodeHandle_for_paramaters,"consecutive_invalid_threshold");
 
 	// Time out duration after which Mocap is considered unavailable
@@ -1523,24 +1523,24 @@ void fetchFlyingAgentClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
 	// Flag for whether the landing should be performed
 	// with the default controller
 	yaml_shouldPerfom_landing_with_defaultController = getParameterBool(nodeHandle_for_paramaters,"shouldPerfom_landing_with_defaultController");
-    
-    // DEBUGGING: Print out one of the parameters that was loaded
-    ROS_INFO_STREAM("[FLYING AGENT CLIENT] DEBUGGING: the fetched FlyingAgentClientConfig/isEnabled_strictSafety = " << yaml_isEnabled_strictSafety);
+
+	// DEBUGGING: Print out one of the parameters that was loaded
+	ROS_INFO_STREAM("[FLYING AGENT CLIENT] DEBUGGING: the fetched FlyingAgentClientConfig/isEnabled_strictSafety = " << yaml_isEnabled_strictSafety);
 
 
 
-    // PROCESS THE PARAMTERS
-    // Convert from degrees to radians
-    yaml_maxTiltAngle_for_strictSafety_radians = DEG2RAD * yaml_maxTiltAngle_for_strictSafety_degrees;
+	// PROCESS THE PARAMTERS
+	// Convert from degrees to radians
+	yaml_maxTiltAngle_for_strictSafety_radians = DEG2RAD * yaml_maxTiltAngle_for_strictSafety_degrees;
 
-    // Adjust the measurement subscribers
+	// Adjust the measurement subscribers
 	// > Localisation data from motion capture
 	if (yaml_shouldUse_mocapMeasurements)
 	{
 		// > Create a node handle to the root of the D-FaLL system
 		ros::NodeHandle nodeHandle_dfall_root("/dfall");
 		// > keeps 50 messages because otherwise the next message may
-	//   overwrite the data while the current message is been used
+		//   overwrite the data while the current message is been used
 		viconSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 50, viconCallback);
 	}
 	else
@@ -1564,8 +1564,8 @@ void fetchFlyingAgentClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
 		onboardEstimateSubscriber.shutdown();
 	}
 
-    // DEBUGGING: Print out one of the processed values
-    ROS_INFO_STREAM("[FLYING AGENT CLIENT CONTROLLER] DEBUGGING: after processing yaml_maxTiltAngle_for_strictSafety_radians = " << yaml_maxTiltAngle_for_strictSafety_radians);
+	// DEBUGGING: Print out one of the processed values
+	ROS_INFO_STREAM("[FLYING AGENT CLIENT CONTROLLER] DEBUGGING: after processing yaml_maxTiltAngle_for_strictSafety_radians = " << yaml_maxTiltAngle_for_strictSafety_radians);
 }