diff --git a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
index 7c810af8fa057037ac7e374aa7ffe540b26ea550..a32012ed31563bed55bf824b3194935b0927af5d 100755
--- a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
+++ b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
@@ -1,6 +1,6 @@
 safeController: "SafeControllerService/RateController"
 demoController: "DemoControllerService/DemoController"
-strictSafety: true
+strictSafety: false
 angleMargin: 0.6
 battery_threshold_while_flying: 2.8       # in V
 battery_threshold_while_motors_off: 3.30  # in V
diff --git a/pps_ws/src/d_fall_pps/param/DemoController.yaml b/pps_ws/src/d_fall_pps/param/DemoController.yaml
index 8b0e4f103b8a684465ae62bff87ca4793297f9e3..dcde99270b81c7822958c78376ebdc29b9b559c5 100644
--- a/pps_ws/src/d_fall_pps/param/DemoController.yaml
+++ b/pps_ws/src/d_fall_pps/param/DemoController.yaml
@@ -1,5 +1,5 @@
 # Mass of the crazyflie
-mass : 31
+mass : 30
 
 # Frequency of the controller, in hertz
 control_frequency : 200
@@ -27,7 +27,7 @@ follow_in_a_line_agentIDs : [1, 2, 3]
 shouldPublishDebugMessage : false
 
 # Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
-shouldDisplayDebugInfo : true
+shouldDisplayDebugInfo : false
 
 
 # A flag for which LQR controller mode to use, defined as:
@@ -50,7 +50,7 @@ lqr_controller_mode : 3
 #       each of (x,y,z,roll,pitch,yaw)
 # 3  -  Quad-rotor Model Based Method
 #       Uses the model of the quad-rotor and the previous inputs
-estimator_method : 2
+estimator_method : 1
 
 
 # The LQR Controller parameters for "mode = 1"
diff --git a/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp b/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp
index 5f0bea33c2c39da9062849f1dc3d3ab63fbcd934..6a3ab506bdfbf26c5ed05e69c346d3e940956bdf 100644
--- a/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/DemoControllerService.cpp
@@ -471,10 +471,41 @@ void calculateControlOutput_viaLQRforMotors(float stateErrorBody[12], Controller
 		motor4_thrustAdjustment  -= gainMatrixMotor4[i] * stateErrorBody[i];
 	}
 
-	//motor1_thrustAdjustment = -gravity_force_quarter*0.9;
-	//motor2_thrustAdjustment = -gravity_force_quarter*0.9;
-	//motor3_thrustAdjustment = -gravity_force_quarter*0.9;
-	//motor4_thrustAdjustment = -gravity_force_quarter*0.9;
+	DebugMsg debugMsg;
+
+	// Fill the debugging message with the data provided by Vicon
+	//debugMsg.vicon_x = request.ownCrazyflie.x;
+	//debugMsg.vicon_y = request.ownCrazyflie.y;
+	//debugMsg.vicon_z = request.ownCrazyflie.z;
+	//debugMsg.vicon_roll = request.ownCrazyflie.roll;
+	//debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
+	//debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
+
+	// debugMsg.value_1 = thrustAdjustment;
+	// ......................
+	// debugMsg.value_10 = your_variable_name;
+
+	debugMsg.value_1 = stateErrorBody[6];
+	debugMsg.value_2 = stateErrorBody[9];
+
+	debugMsg.value_3 = motor1_thrustAdjustment;
+	debugMsg.value_4 = motor2_thrustAdjustment;
+	debugMsg.value_5 = motor3_thrustAdjustment;
+	debugMsg.value_6 = motor4_thrustAdjustment;
+
+
+
+
+	// Publish the "debugMsg"
+	debugPublisher.publish(debugMsg);
+
+
+
+
+	motor1_thrustAdjustment = -gravity_force_quarter*0.9;
+	motor2_thrustAdjustment = -gravity_force_quarter*0.9;
+	motor3_thrustAdjustment = -gravity_force_quarter*0.9;
+	motor4_thrustAdjustment = -gravity_force_quarter*0.9;
 
 	// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
 
@@ -497,28 +528,7 @@ void calculateControlOutput_viaLQRforMotors(float stateErrorBody[12], Controller
 	// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
 
 
-	 DebugMsg debugMsg;
-
-	// Fill the debugging message with the data provided by Vicon
-	//debugMsg.vicon_x = request.ownCrazyflie.x;
-	//debugMsg.vicon_y = request.ownCrazyflie.y;
-	//debugMsg.vicon_z = request.ownCrazyflie.z;
-	//debugMsg.vicon_roll = request.ownCrazyflie.roll;
-	//debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
-	//debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
-
-	// debugMsg.value_1 = thrustAdjustment;
-	// ......................
-	// debugMsg.value_10 = your_variable_name;
-
-	debugMsg.value_1 = motor1_thrustAdjustment;
-	debugMsg.value_2 = motor2_thrustAdjustment;
-	debugMsg.value_3 = motor3_thrustAdjustment;
-	debugMsg.value_4 = motor4_thrustAdjustment;
-
-
-	// Publish the "debugMsg"
-	debugPublisher.publish(debugMsg);
+	 
 
 
 	// An alternate debugging technique is to print out data directly to the