diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
index e1e6b02f88db1880a8c1945ffddcc88e5d8f3d16..545a6ce6939f9e060dbbe001376fb58171f0358d 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
@@ -122,6 +122,9 @@ public:
         static double Pos_Px,Pos_Ix,Pos_Dx;
 	static double Pos_Py,Pos_Iy,Pos_Dy;
 
+	//static double S_yPos_P,S_yPos_I,S_yPos_D;
+	//static double L_yPos_P,L_yPos_I,L_yPos_D;
+
         static double S_Ang_P,S_Ang_I,S_Ang_D;
         static double L_Ang_P,L_Ang_I,L_Ang_D;
         
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
index fdfa7f228957bf3b2828a8296665c990157bb060..e1e6b02f88db1880a8c1945ffddcc88e5d8f3d16 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
@@ -119,10 +119,11 @@ public:
   
   void myhandler(int value);
   
-  
-        static double Pos_P,Pos_I,Pos_D;
-        static double Ang_P,Ang_I,Ang_D;
-        
+        static double Pos_Px,Pos_Ix,Pos_Dx;
+	static double Pos_Py,Pos_Iy,Pos_Dy;
+
+        static double S_Ang_P,S_Ang_I,S_Ang_D;
+        static double L_Ang_P,L_Ang_I,L_Ang_D;
         
         // ---- CONTROLL PARAMETERS ------ //
         static double prev_errorX;
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index 786e01e4e1875dc28691c55ccc4b5d019a2c6daa..4f0cbb897dfeb536c9881d7f349b6effa0b11651 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
@@ -42,23 +42,35 @@ using namespace cv;
 using namespace aruco;
 using namespace std;
 
-double ImageConverter::Pos_Px = .0001;
+
+double ImageConverter::Pos_Px = .00005;
 double ImageConverter::Pos_Ix = 0.002;
 double ImageConverter::Pos_Dx = 0;
 
-double ImageConverter::Pos_Py = .25 * ImageConverter::Pos_Px;
-double ImageConverter::Pos_Iy = .25 * ImageConverter::Pos_Ix;
-double ImageConverter::Pos_Dy = .25 * ImageConverter::Pos_Dx;
 
-double ImageConverter::S_Ang_P = 0.05 * ImageConverter::Pos_Px;
-double ImageConverter::S_Ang_I = 0.05 * ImageConverter::Pos_Ix;
-double ImageConverter::S_Ang_D = 0.05 * ImageConverter::Pos_Dx;
+double ImageConverter::Pos_Py = .37 * ImageConverter::Pos_Px;
+double ImageConverter::Pos_Iy = .37 * ImageConverter::Pos_Ix;
+double ImageConverter::Pos_Dy = .37 * ImageConverter::Pos_Dx;
+
+/*
+double ImageConverter::S_yPos_P = .16 * ImageConverter::Pos_Px;
+double ImageConverter::S_yPos_I = .16 * ImageConverter::Pos_Ix;
+double ImageConverter::S_yPos_D = .16 * ImageConverter::Pos_Dx;
+
+double ImageConverter::L_yPos_P = .3 * ImageConverter::Pos_Px;
+double ImageConverter::L_yPos_I = .3 * ImageConverter::Pos_Ix;
+double ImageConverter::L_yPos_D = .3 * ImageConverter::Pos_Dx;
+*/
+
+double ImageConverter::S_Ang_P = .07 * ImageConverter::Pos_Px;
+double ImageConverter::S_Ang_I = .07 * ImageConverter::Pos_Ix;
+double ImageConverter::S_Ang_D = .07 * ImageConverter::Pos_Dx;
 
-double ImageConverter::L_Ang_P = 0.9 * ImageConverter::Pos_Px;
-double ImageConverter::L_Ang_I = 0.9 * ImageConverter::Pos_Ix;
-double ImageConverter::L_Ang_D = 0.9 * ImageConverter::Pos_Dx;
+double ImageConverter::L_Ang_P = .1 * ImageConverter::Pos_Px;
+double ImageConverter::L_Ang_I = .1 * ImageConverter::Pos_Ix;
+double ImageConverter::L_Ang_D = .1 * ImageConverter::Pos_Dx;
 
-float ImageConverter::TheMarkerSize = -1;
+float ImageConverter::TheMarkerSize = .1; // Default marker size
 
 int ImageConverter::Thresh1_min = 20;
 int ImageConverter::Thresh2_min = 20;
@@ -108,9 +120,9 @@ double ImageConverter::control_signalYAW;
 
 double ImageConverter::zeroMax = .0000000000000000001;
 double ImageConverter::zeroMin = -.0000000000000000001;
-double ImageConverter::Pz_eps = .01;
-double ImageConverter::Py_eps = .001;
-double ImageConverter::A_eps = .02; // ? deg.
+double ImageConverter::Pz_eps = .005;
+double ImageConverter::Py_eps = .007;
+double ImageConverter::A_eps = .01; // ? deg.
 
 
  ImageConverter::ImageConverter() : 
@@ -255,7 +267,6 @@ void ImageConverter::ProgStart(int argc,char** argv)
 		MDetector.pyrDown(ThePyrDownLevel);
         }
         
-	
 	MDetector.setCornerRefinementMethod(MarkerDetector::LINES);
 	
 	char key=0;
@@ -385,17 +396,24 @@ void ImageConverter::ProgStart(int argc,char** argv)
 				
 				// -------------------------Removed----------------------------- //
 			} 
-			// Print other rectangles that contains no valid markers
+			/*// Print other rectangles that contains no valid markers
 			 for (unsigned int i=0;i<MDetector.getCandidates().size();i++) 
 			{
 				Marker m( MDetector.getCandidates()[i],10);
-				m.draw(TheInputImageCopy,cv::Scalar(0,255,0),2);
-			}
+				m.draw(TheInputImageCopy,Scalar(0,255,0),2);
+			}*/
 			
 			for (unsigned int i=0;i<TheMarkers.size();i++)
 				{
+					int currentMarID = TheMarkers[i].id;
+					TheMarkers[i].draw(TheInputImageCopy,Scalar(0,255,0),2)	;
+				
 					CvDrawingUtils::draw3dCube(TheInputImageCopy,TheMarkers[i],TheCameraParameters);
 					CvDrawingUtils::draw3dAxis(TheInputImageCopy,TheMarkers[i],TheCameraParameters);
+
+					//Marker ID to string
+    					stringstream marker_id_string;
+    					marker_id_string << "marker_ " << currentMarID;
 				}
 
 			if (update_images)
@@ -435,9 +453,9 @@ void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB)
 {
        
 RefPose[0] = -.0957;
-RefPose[1] = -0.0965533;
-RefPose[2] = 0.371;
-RefPose[3] = -0.69;
+RefPose[1] = -0.0203153;
+RefPose[2] = 0.307922;
+RefPose[3] = -0.705028;
 
 camPose[0] = CamFB->pose.position.x;
 camPose[1] = CamFB->pose.position.y;
@@ -543,7 +561,24 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
         	p_termY = Pos_Py * curr_errorY;
         	i_termY = Pos_Iy * int_errorY;
         	d_termY = Pos_Dy * diffY;
-        
+
+		/*int  y_offset = 5;
+		if (curr_errorY < y_offset && curr_errorY > -y_offset) // -5 < err < +5
+		{
+			ROS_INFO_STREAM(" INSIDE boundry of " << y_offset << " m. \n");	        		
+			// scalling
+        		p_termYAW = S_yPos_P * curr_errorY;
+        		i_termYAW = S_yPos_I * int_errorY;
+        		d_termYAW = S_yPos_D * diffY;
+        	} else if (curr_errorY > y_offset || curr_errorYAW < y_offset) //  err > +5 or err < -5
+		{
+			ROS_INFO_STREAM(" OUTSIDE boundry of " << y_offset << " m. \n");			
+			// scalling
+        		p_termYAW = L_yPos_P * curr_errorY;
+        		i_termYAW = L_yPos_I * int_errorY;
+        		d_termYAW = L_yPos_D * diffY;
+		}*/
+
         	// control signal
         	control_signalY = p_termY + i_termY + d_termY;
         
@@ -575,16 +610,17 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
         	// differentiation
         	diffYAW = ((curr_errorYAW - prev_errorYAW) / dt);
         	
-		if (curr_errorYAW < .08726 && curr_errorYAW > -.08726) // -5 < err < +5
+		int  yaw_offset = 5;
+		if (curr_errorYAW < ((CV_PI/180) * yaw_offset) && curr_errorYAW > ((CV_PI/180) * -yaw_offset)) // -5 < err < +5
 		{
-			ROS_INFO(" -5 < err < +5 \n");	        		
+			ROS_INFO_STREAM(" INSIDE boundry of " << yaw_offset << " deg. \n");	        		
 			// scalling
         		p_termYAW = S_Ang_P * curr_errorYAW;
         		i_termYAW = S_Ang_I * int_errorYAW;
         		d_termYAW = S_Ang_D * diffYAW;
-        	} else if (curr_errorYAW > .08726 || curr_errorYAW < -.08726) //  err > +5 or err < -5
+        	} else if (curr_errorYAW > ((CV_PI/180) * yaw_offset) || curr_errorYAW < ((CV_PI/180) * -yaw_offset)) //  err > +5 or err < -5
 		{
-			ROS_INFO(" err > +5 or err < -5 \n");			
+			ROS_INFO_STREAM(" OUTSIDE boundry of " << yaw_offset << " deg. \n");			
 			// scalling
         		p_termYAW = L_Ang_P * curr_errorYAW;
         		i_termYAW = L_Ang_I * int_errorYAW;
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
index e7668aefd6ad6c76fb473140433c781a1837a4d5..786e01e4e1875dc28691c55ccc4b5d019a2c6daa 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
@@ -46,17 +46,17 @@ double ImageConverter::Pos_Px = .0001;
 double ImageConverter::Pos_Ix = 0.002;
 double ImageConverter::Pos_Dx = 0;
 
-double ImageConverter::Pos_Py = 4 * ImageConverter::Pos_Px;
-double ImageConverter::Pos_Iy = 4 * ImageConverter::Pos_Ix;
-double ImageConverter::Pos_Dy = 4 * ImageConverter::Pos_Dx;
+double ImageConverter::Pos_Py = .25 * ImageConverter::Pos_Px;
+double ImageConverter::Pos_Iy = .25 * ImageConverter::Pos_Ix;
+double ImageConverter::Pos_Dy = .25 * ImageConverter::Pos_Dx;
 
-double ImageConverter::S_Ang_P = 0.01 * ImageConverter::Pos_Px;
-double ImageConverter::S_Ang_I = 0.01 * ImageConverter::Pos_Ix;
-double ImageConverter::S_Ang_D = 0.01 * ImageConverter::Pos_Dx;
+double ImageConverter::S_Ang_P = 0.05 * ImageConverter::Pos_Px;
+double ImageConverter::S_Ang_I = 0.05 * ImageConverter::Pos_Ix;
+double ImageConverter::S_Ang_D = 0.05 * ImageConverter::Pos_Dx;
 
-double ImageConverter::L_Ang_P = 0.7 * ImageConverter::Pos_Px;
-double ImageConverter::L_Ang_I = 0.7 * ImageConverter::Pos_Ix;
-double ImageConverter::L_Ang_D = 0.7 * ImageConverter::Pos_Dx;
+double ImageConverter::L_Ang_P = 0.9 * ImageConverter::Pos_Px;
+double ImageConverter::L_Ang_I = 0.9 * ImageConverter::Pos_Ix;
+double ImageConverter::L_Ang_D = 0.9 * ImageConverter::Pos_Dx;
 
 float ImageConverter::TheMarkerSize = -1;
 
@@ -108,8 +108,8 @@ double ImageConverter::control_signalYAW;
 
 double ImageConverter::zeroMax = .0000000000000000001;
 double ImageConverter::zeroMin = -.0000000000000000001;
-double ImageConverter::Pz_eps = .012;
-double ImageConverter::Py_eps = .0001;
+double ImageConverter::Pz_eps = .01;
+double ImageConverter::Py_eps = .001;
 double ImageConverter::A_eps = .02; // ? deg.
 
 
@@ -269,7 +269,6 @@ void ImageConverter::ProgStart(int argc,char** argv)
 		{
                         // Detection of markers in the image passed
 			MDetector.detect(TheInputImage,TheMarkers,TheCameraParameters,TheMarkerSize);
-			//MDetector.detectRectangles(MDetector.getThresholdedImage(),MDetector._candidates);
 			
                         TheInputImage.copyTo(TheInputImageCopy);
 
@@ -290,17 +289,17 @@ void ImageConverter::ProgStart(int argc,char** argv)
 
 				if(curr_errorYAW > RefPose[3])
 				{
-					move2docking(-control_signalX, control_signalY, control_signalYAW); // CW (---|| \\)
+					move2docking(-control_signalX, -control_signalY, control_signalYAW); // CW (---|| \\)
 				} else
 				{
-					move2docking(-control_signalX, control_signalY, -control_signalYAW); // CCW (// || ---)
-				}	
+					move2docking(-control_signalX, -control_signalY, -control_signalYAW); // CCW (// || ---)
+				}
 			}
 			
 			if (ros::ok() && found)
 			{
-			        x_t = -TheMarkers[0].Tvec.at<Vec3f>(0,0)[0];
-				y_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[1];
+			        y_t = -TheMarkers[0].Tvec.at<Vec3f>(0,0)[0];
+				x_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[1];
 				z_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[2];
 			
 				//printf("\n X = %4.2f Y = %4.2f Z = %4.2f\n",x_t,y_t,z_t);
@@ -436,9 +435,9 @@ void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB)
 {
        
 RefPose[0] = -.0957;
-RefPose[1] = 0.0054;
-RefPose[2] = 0.365;
-RefPose[3] = -0.65;
+RefPose[1] = -0.0965533;
+RefPose[2] = 0.371;
+RefPose[3] = -0.69;
 
 camPose[0] = CamFB->pose.position.x;
 camPose[1] = CamFB->pose.position.y;
@@ -610,10 +609,10 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
 	
 	if(curr_errorYAW > RefYAW)
 	{
-		dock(-control_signalX, control_signalY, control_signalYAW); // CW (---|| \\)
+		dock(-control_signalX, -control_signalY, control_signalYAW); // CW (---|| \\)
 	} else
 	{
-		dock(-control_signalX, control_signalY, -control_signalYAW); // CCW (// || ---)	
+		dock(-control_signalX, -control_signalY, -control_signalYAW); // CCW (// || ---)	
 	}
 }
 
diff --git a/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp b/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp
index 56678bd618ff18b4dc350f1625d076564a0c67b2..3ba8c943ec7e0cfa85e01a3b16e3af3b7f4fa11d 100644
--- a/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp
+++ b/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp
@@ -201,7 +201,10 @@ int main(int argc,char **argv){
 			geometry_msgs::Pose msg;
 
 			float x_t, y_t, z_t;
+			
 			float roll,yaw,pitch;
+			float rollE,yawE,pitchE;
+			
 			bool found = (TheMarkers.size()>0)?true:false;
 
 			if (found)  {
@@ -213,6 +216,34 @@ int main(int argc,char **argv){
 				cv::Mat rot_mat(3,3,cv::DataType<float>::type);
 				// You need to apply cv::Rodrigues() in order to obatain angles wrt to camera coords
 				cv::Rodrigues(TheMarkers[0].Rvec,rot_mat);
+				
+				// ----------- Euler angle -----------------//
+                                
+                                float roll1 = -asin(rot_mat.at<float>(2,0));
+                                float roll2 = CV_PI - roll1;
+
+                                float pitch1 = atan2(rot_mat.at<float>(2,1) / cos(roll1), rot_mat.at<float>(2,2) / cos(roll1));
+                                float pitch2 = atan2(rot_mat.at<float>(2,1) / cos(roll2), rot_mat.at<float>(2,2) / cos(roll2));
+
+                                float yaw2 = atan2(rot_mat.at<float>(1,0) / cos(roll2), rot_mat.at<float>(0,0) / cos(roll2));
+                                float yaw1 = atan2(rot_mat.at<float>(1,0) / cos(roll1), rot_mat.at<float>(0,0) / cos(roll1));
+
+                                //choose one solution to return
+                                //for example the "shortest" rotation
+                                if ((abs(roll1) + abs(pitch1) + abs(yaw1)) <= (abs(roll2) + abs(pitch2) + abs(yaw2)))
+                                {
+                                        rollE = roll1;
+                                        pitchE= pitch1;
+		                        yawE = yaw1;
+                                } else 
+        
+                                {
+                                        rollE = roll2;
+                                        pitchE = pitch2;
+		                        yawE = yaw2;
+                                }                   
+                                
+                                // ----------- Euler angle -----------------//
 
 				pitch   = -atan2(rot_mat.at<float>(2,0), rot_mat.at<float>(2,1));
 				yaw     = acos(rot_mat.at<float>(2,2));
@@ -232,14 +263,23 @@ int main(int argc,char **argv){
 			float r_off = CV_PI/2;
 			float y_off = CV_PI/2;
 
-			// See: http://en.wikipedia.org/wiki/Flight_dynamics
 			if (found){
 				//printf( "Angles (deg) wrt Flight Dynamics: roll:%5.2f pitch:%5.2f yaw:%5.2f \n", (roll-r_off)*(180.0/CV_PI), (pitch-p_off)*(180.0/CV_PI), (yaw-y_off)*(180.0/CV_PI));
 				//printf( "       Marker distance in metres:  x_d:%5.2f   y_d:%5.2f z_d:%5.2f \n", x_t, y_t, z_t);
 				ROS_INFO_STREAM("Y = " << y_t << "m. \n");
-				ROS_INFO_STREAM("        X = " << x_t << "m. \n");
+				//ROS_INFO_STREAM("        X = " << x_t << "m. \n");
 			        //ROS_INFO_STREAM("Z = " << z_t << "m. \n");
-			
+			        ROS_INFO_STREAM("roll = " << (roll/*-r_off*/)*(180.0/CV_PI) << " deg. \n");
+			        ROS_INFO_STREAM("EULER R = " << (rollE/*-r_off*/)*(180.0/CV_PI) << " deg. \n");
+			        
+			        
+			        ROS_INFO_STREAM("pitch = " << (pitch/*-r_off*/)*(180.0/CV_PI) << " deg. \n");
+			        ROS_INFO_STREAM("EULER P = " << (pitchE/*-r_off*/)*(180.0/CV_PI) << " deg. \n");
+			        
+			        ROS_INFO_STREAM("yaw = " << (yaw/*-r_off*/)*(180.0/CV_PI) << " deg. \n");
+			        ROS_INFO_STREAM("EULER Y = " << (yawE/*-r_off*/)*(180.0/CV_PI) << " deg. \n");
+			        
+			        ROS_INFO_STREAM(" --------------------------------------  \n");
 			}
 
 			if (ros::ok()){
@@ -259,8 +299,8 @@ int main(int argc,char **argv){
 
 			// Print other rectangles that contains no valid markers
 			 for (unsigned int i=0;i<MDetector.getCandidates().size();i++) {
-				aruco::Marker m( MDetector.getCandidates()[i],999);
-				m.draw(TheInputImageCopy,cv::Scalar(255,0,0));
+				aruco::Marker m( MDetector.getCandidates()[i],11);
+				m.draw(TheInputImageCopy,cv::Scalar(150,10,80),2);
 			}
 			// Draw a 3d cube in each marker if there is 3d info
 
diff --git a/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp~ b/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp~
index ce0df1e971d51c529545c5d86630cd096612b569..55fe539254c55e38fef83e422d63c2652dd6498b 100644
--- a/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp~
+++ b/MobileRobot/AugReaMarker/ar_raw_detection/ar_det/src/ar_detection.cpp~
@@ -201,7 +201,10 @@ int main(int argc,char **argv){
 			geometry_msgs::Pose msg;
 
 			float x_t, y_t, z_t;
+			
 			float roll,yaw,pitch;
+			float rollE,yawE,pitchE;
+			
 			bool found = (TheMarkers.size()>0)?true:false;
 
 			if (found)  {
@@ -213,6 +216,34 @@ int main(int argc,char **argv){
 				cv::Mat rot_mat(3,3,cv::DataType<float>::type);
 				// You need to apply cv::Rodrigues() in order to obatain angles wrt to camera coords
 				cv::Rodrigues(TheMarkers[0].Rvec,rot_mat);
+				
+				// ----------- Euler angle -----------------//
+                                
+                                float roll1 = -asin(rot_mat.at<float>(2,0));
+                                float roll2 = CV_PI - roll1;
+
+                                float pitch1 = atan2(rot_mat.at<float>(2,1) / cos(roll1), rot_mat.at<float>(2,2) / cos(roll1));
+                                float pitch2 = atan2(rot_mat.at<float>(2,1) / cos(roll2), rot_mat.at<float>(2,2) / cos(roll2));
+
+                                float yaw2 = atan2(rot_mat.at<float>(1,0) / cos(roll2), rot_mat.at<float>(0,0) / cos(roll2));
+                                float yaw1 = atan2(rot_mat.at<float>(1,0) / cos(roll1), rot_mat.at<float>(0,0) / cos(roll1));
+
+                                //choose one solution to return
+                                //for example the "shortest" rotation
+                                if ((abs(roll1) + abs(pitch1) + abs(yaw1)) <= (abs(roll2) + abs(pitch2) + abs(yaw2)))
+                                {
+                                        rollE = roll1;
+                                        pitchE= pitch1;
+		                        yawE = yaw1;
+                                } else 
+        
+                                {
+                                        rollE = roll2;
+                                        pitchE = pitch2;
+		                        yawE = yaw2;
+                                }                   
+                                
+                                // ----------- Euler angle -----------------//
 
 				pitch   = -atan2(rot_mat.at<float>(2,0), rot_mat.at<float>(2,1));
 				yaw     = acos(rot_mat.at<float>(2,2));
@@ -232,14 +263,23 @@ int main(int argc,char **argv){
 			float r_off = CV_PI/2;
 			float y_off = CV_PI/2;
 
-			// See: http://en.wikipedia.org/wiki/Flight_dynamics
 			if (found){
 				//printf( "Angles (deg) wrt Flight Dynamics: roll:%5.2f pitch:%5.2f yaw:%5.2f \n", (roll-r_off)*(180.0/CV_PI), (pitch-p_off)*(180.0/CV_PI), (yaw-y_off)*(180.0/CV_PI));
 				//printf( "       Marker distance in metres:  x_d:%5.2f   y_d:%5.2f z_d:%5.2f \n", x_t, y_t, z_t);
-				ROS_INFO_STREAM("X = " << x_t << "m. \n");
-				ROS_INFO_STREAM("        Y = " << y_t << "m. \n");
+				//ROS_INFO_STREAM("Y = " << y_t << "m. \n");
+				//ROS_INFO_STREAM("        X = " << x_t << "m. \n");
 			        //ROS_INFO_STREAM("Z = " << z_t << "m. \n");
-			
+			        ROS_INFO_STREAM("roll = " << (roll/*-r_off*/)*(180.0/CV_PI) << " deg. \n");
+			        ROS_INFO_STREAM("EULER R = " << (rollE/*-r_off*/)*(180.0/CV_PI) << " deg. \n");
+			        
+			        
+			        ROS_INFO_STREAM("pitch = " << (pitch/*-r_off*/)*(180.0/CV_PI) << " deg. \n");
+			        ROS_INFO_STREAM("EULER P = " << (pitchE/*-r_off*/)*(180.0/CV_PI) << " deg. \n");
+			        
+			        ROS_INFO_STREAM("yaw = " << (yaw/*-r_off*/)*(180.0/CV_PI) << " deg. \n");
+			        ROS_INFO_STREAM("EULER Y = " << (yawE/*-r_off*/)*(180.0/CV_PI) << " deg. \n");
+			        
+			        ROS_INFO_STREAM(" --------------------------------------  \n");
 			}
 
 			if (ros::ok()){
@@ -259,8 +299,8 @@ int main(int argc,char **argv){
 
 			// Print other rectangles that contains no valid markers
 			 for (unsigned int i=0;i<MDetector.getCandidates().size();i++) {
-				aruco::Marker m( MDetector.getCandidates()[i],999);
-				m.draw(TheInputImageCopy,cv::Scalar(255,0,0));
+				aruco::Marker m( MDetector.getCandidates()[i],11);
+				m.draw(TheInputImageCopy,cv::Scalar(150,10,80),2);
 			}
 			// Draw a 3d cube in each marker if there is 3d info
 
diff --git a/MobileRobot/AugReaMarker/ar_raw_detection/build/ar_det/CMakeFiles/ar_det.dir/src/ar_detection.cpp.o b/MobileRobot/AugReaMarker/ar_raw_detection/build/ar_det/CMakeFiles/ar_det.dir/src/ar_detection.cpp.o
index f89348a836ed8d431e2fc5548c15f69710269e3f..46a2ec0587c06a375afcc0e94d353b0d32e868ea 100644
Binary files a/MobileRobot/AugReaMarker/ar_raw_detection/build/ar_det/CMakeFiles/ar_det.dir/src/ar_detection.cpp.o and b/MobileRobot/AugReaMarker/ar_raw_detection/build/ar_det/CMakeFiles/ar_det.dir/src/ar_detection.cpp.o differ
diff --git a/MobileRobot/AugReaMarker/ar_raw_detection/devel/lib/ar_det/ar_det b/MobileRobot/AugReaMarker/ar_raw_detection/devel/lib/ar_det/ar_det
index c2a2d5e866972e2f8e849b21e8bf08c0edef38c9..92a96555055252093fd1df9562ad9000553eb9a0 100755
Binary files a/MobileRobot/AugReaMarker/ar_raw_detection/devel/lib/ar_det/ar_det and b/MobileRobot/AugReaMarker/ar_raw_detection/devel/lib/ar_det/ar_det differ