diff --git a/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
index ffa37f32f264cbb5d83cdeef808dabe18af898cc..085cdc5a6a858b4474edb7ba579f7f668d4c9c0f 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
@@ -169,14 +169,11 @@ std::vector<float> yaml_gainMatrixYawRate                 =  { 0.00, 0.00, 0.00,
 float m_cf_weight_in_newtons = 0.0;
 
 // The state space matrices of lead compensator controller
-float m_A=0,m_B=0,m_C=0,m_D=1;
+float m_A=0.0,m_B=0.0,m_C=0.0,m_D=1.0;
 
 // The state of lead compensator controller
 float m_controller_state = 0.0;
 
-// previous beta_ref given by controller
-float m_beta_ref = 0;
-
 // The inertial x measurement during a time window of 200 measurements
 std::vector<float>  m_x (200,0);
 
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
index 5cde749a7e5e3eecb9a3f3fe295fefe97fee4f48..1f90286f6005417679db6f1190338c7df19fa4a8 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
@@ -252,13 +252,32 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
 		yawRate_forResponse -= yaml_gainMatrixYawRate[i] * stateErrorBody[i];
 	}
 
+
+
+	// For the x-controller:
+	// > Execute the discrete-time state space equivalent of the lead
+	//   compensator specified.
+	// > NOTE: the new pitch reference, i.e., the output to the state
+	//         space, MUST be calucated before the state update equation
+	//         is evaulated, in equations:
+	//         FIRST:    y(t)   = C x(t) + D u(t)
+	//         SECOND:   x(t+1) = A x(t) + B u(t)
+
+	// > FIRST: compute the new pitch reference
+	float pitch_ref = m_C*m_controller_state - m_D*stateErrorBody[0];
+
+	// > SECOND: evaluate the state update equation
 	m_controller_state = m_A*m_controller_state - m_B*stateErrorBody[0];
-	m_beta_ref = m_C*m_controller_state - m_D*stateErrorBody[0];
+
+	// > THIRD: perform the inner controller
+	pitchRate_forResponse = 5.0*(pitch_ref-request.ownCrazyflie.pitch);
+
+
 
 	// m_beta_ref = 0.99*m_beta_ref - 1.71/5*stateErrorBody[0] - 1.33/5*m_controller_state;
-	// pitchRate_forResponse = 5*(m_beta_ref-request.ownCrazyflie.pitch);
 	// m_controller_state = -stateErrorBody[0];
 
+
 	// Put the computed body rate commands into the "response" variable
 	response.controlOutput.roll = rollRate_forResponse;
 	response.controlOutput.pitch = pitchRate_forResponse;
@@ -579,34 +598,20 @@ void requestControllerParametersCallback(const SetpointWithHeader& newSetpoint)
 	if (isRevelant)
 	{
 		convertIntoDiscreteTimeParameters(newSetpoint.x, newSetpoint.y, newSetpoint.z);
-		m_controller_state = 0;
+		m_controller_state = 0.0;
 	}
 }
 
 // CHANGE CONTROLLER PARAMETERS INTO DISCRETE TIME FUNCTION
 void convertIntoDiscreteTimeParameters(float k, float T, float alpha)
 {
-	if (alpha > 1)
-	{
-		alpha = 1;
-	}
-	else if (alpha<0.1)
-	{
-		alpha = 0.1;
-	}
+	if (alpha > 1){alpha = 1;} else if (alpha<0.1){alpha = 0.1;}
 
-		if (T > 100)
-	{
-		T = 100;
-	}
-	else if (T<0.1)
-	{
-		T = 0.1;
-	}
+	if (T > 100){T = 100;} else if (T<0.1){T = 0.1;}
 
-	m_A = pow(2.71828,(-1/(yaml_control_frequency*alpha*T)));
-	m_B = -alpha*T*(m_A-1);
-	m_C = k/(alpha*T)*(1-1/alpha);
+	m_A = pow(2.71828,(-1.0/(yaml_control_frequency*alpha*T)));
+	m_B = -alpha*T*(m_A-1.0);
+	m_C = k/(alpha*T)*(1.0-1.0/alpha);
 	m_D = k/alpha;
 
 	ROS_INFO_STREAM("Parameters changed to k="<<k<<", T="<<T<<", alpha="<<alpha);