diff --git a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
index 272581979c6e3a6549ef441657a8130608da5771..7d4889183a77e57fc4897f4e662a627264c0ca3a 100644
--- a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
@@ -143,16 +143,16 @@ std::vector<float>  setpoint{0.0,0.0,0.4,0.0};     // The setpoints for (x,y,z)
 int controller_type = LQR_RATE_MODE;
 
 // The LQR Controller parameters for "LQR_RATE_MODE"
-std::vector<float> gainMatrixRollRate(9)                =  { 0.00,-1.72, 0.00, 0.00,-1.34, 0.00, 5.12, 0.00, 0.00};
-std::vector<float> gainMatrixPitchRate(9)               =  { 1.72, 0.00, 0.00, 1.34, 0.00, 0.00, 0.00, 5.12, 0.00};
-std::vector<float> gainMatrixYawRate(9)                 =  { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84};
-std::vector<float> gainMatrixThrust_NineStateVector(9)  =  { 0.00, 0.00, 0.25, 0.00, 0.00, 0.14, 0.00, 0.00, 0.00};
+std::vector<float> gainMatrixRollRate                =  { 0.00,-1.72, 0.00, 0.00,-1.34, 0.00, 5.12, 0.00, 0.00};
+std::vector<float> gainMatrixPitchRate               =  { 1.72, 0.00, 0.00, 1.34, 0.00, 0.00, 0.00, 5.12, 0.00};
+std::vector<float> gainMatrixYawRate                 =  { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84};
+std::vector<float> gainMatrixThrust_NineStateVector  =  { 0.00, 0.00, 0.25, 0.00, 0.00, 0.14, 0.00, 0.00, 0.00};
 
 
 // The LQR Controller parameters for "LQR_ANGLE_MODE"
-std::vector<float> gainMatrixRollAngle(6)               =  { 0.00,-0.20, 0.00, 0.00,-0.20, 0.00};
-std::vector<float> gainMatrixPitchAngle(6)              =  { 0.20, 0.00, 0.00, 0.20, 0.00, 0.00};
-std::vector<float> gainMatrixThrust_SixStateVector(6)   =  { 0.00, 0.00, 0.31, 0.00, 0.00, 0.14};
+std::vector<float> gainMatrixRollAngle               =  { 0.00,-0.20, 0.00, 0.00,-0.20, 0.00};
+std::vector<float> gainMatrixPitchAngle              =  { 0.20, 0.00, 0.00, 0.20, 0.00, 0.00};
+std::vector<float> gainMatrixThrust_SixStateVector   =  { 0.00, 0.00, 0.31, 0.00, 0.00, 0.14};
 
 // ROS Publisher for debugging variables
 ros::Publisher debugPublisher;