diff --git a/Files_4_thesis/Different_controllers/docking_with_PID_controller.asv b/Files_4_thesis/Different_controllers/docking_with_PID_controller.asv
new file mode 100644
index 0000000000000000000000000000000000000000..9a382908b10e95ae252a188b32e9fb79862150eb
--- /dev/null
+++ b/Files_4_thesis/Different_controllers/docking_with_PID_controller.asv
@@ -0,0 +1,648 @@
+clc;
+close all;
+clear all;
+
+yaw_off = pi;
+%% Import data and time conversion
+
+% data 1: 556 ros messages for geometry_msgs/Twist 
+%         557 ros messages for geometry_msgs/PoseStamped
+
+
+% Pose_Matrix_data_1 = csvread('POSE_with_Py0.86_Iy0_Dy0.002ANDPt0.08_It0_Dt0.txt',1,0);
+% Vel_Matrix_data_1 = csvread('VEL_with_Py0.86_Iy0_Dy0.002ANDPt0.08_It0_Dt0.txt',1,0);
+Pose_Matrix_data_1 = csvread('CAM_PID_.28_.0001_.01.txt',1,0);
+Vel_Matrix_data_1 = csvread('vel_PID_.28_.0001_.01.txt',1,0);
+
+% time in position
+TimeP_ros_data_1 = Pose_Matrix_data_1(:,1); % ros time, needs to be converted to sec...
+
+% duriation =         end_time        -     start_time
+  durationP_data_1 = (TimeP_ros_data_1(size(TimeP_ros_data_1,1),:) - TimeP_ros_data_1(1,:))*10^(-9); 
+  
+t_P_sec_data_1 = 0:durationP_data_1/size(TimeP_ros_data_1,1):durationP_data_1;
+t_P_sec_data_1(:,size(t_P_sec_data_1,2)) = [];
+
+% time in vel
+TimeV_ros_data_1 = Vel_Matrix_data_1(:,1); % ros time, needs to be converted to sec...
+
+% duriation =         end_time        -     start_time
+  durationV_data_1 = (TimeV_ros_data_1(size(TimeV_ros_data_1,1),:) - TimeV_ros_data_1(1,:))*10^(-9);
+
+t_V_sec_data_1 = 0:durationV_data_1/size(TimeV_ros_data_1,1):durationV_data_1;
+t_V_sec_data_1(:,size(t_V_sec_data_1,2)) = [];
+
+% -----------------------------------------------------------------------------------------------------
+% data 2: 588 ros messages for geometry_msgs/Twist 
+%         589 ros messages for geometry_msgs/PoseStamped
+% y-axis gains:
+% P = .5; I = 0.001; D = 0.03;
+
+% theta-axis gains:
+% P = .28; I = 0.1; D = 0.15;
+% 
+% Pose_Matrix_data_2 = csvread('POSE_with_Py0.66_Iy0_Dy0.1ANDPt0.08_It0_Dt0.txt',1,0);
+% Vel_Matrix_data_2 = csvread('VEL_with_Py0.66_Iy0_Dy0.1ANDPt0.08_It0_Dt0.txt',1,0);
+% 
+Pose_Matrix_data_2 = csvread('CAM_PID_.29_.00026_.015.txt',1,0);
+Vel_Matrix_data_2 = csvread('vel_PID_.29_.00026_.015.txt',1,0);
+
+% time in position
+TimeP_ros_data_2 = Pose_Matrix_data_2(:,1); % ros time, needs to be converted to sec...
+
+% duriation =         end_time        -     start_time
+  durationP_data_2 = (TimeP_ros_data_2(size(TimeP_ros_data_2,1),:) - TimeP_ros_data_2(1,:))*10^(-9); 
+  
+t_P_sec_data_2 = 0:durationP_data_2/size(TimeP_ros_data_2,1):durationP_data_2;
+t_P_sec_data_2(:,size(t_P_sec_data_2,2)) = [];
+
+% time in vel
+TimeV_ros_data_2 = Vel_Matrix_data_2(:,1); % ros time, needs to be converted to sec...
+
+% duriation =         end_time        -     start_time
+  durationV_data_2 = (TimeV_ros_data_2(size(TimeV_ros_data_2,1),:) - TimeV_ros_data_2(1,:))*10^(-9);
+
+t_V_sec_data_2 = 0:durationV_data_2/size(TimeV_ros_data_2,1):durationV_data_2;
+t_V_sec_data_2(:,size(t_V_sec_data_2,2)) = [];
+
+% -----------------------------------------------------------------------------------------------------
+% data 3: 538 ros messages for geometry_msgs/Twist 
+%         539 ros messages for geometry_msgs/PoseStamped
+% y-axis gains:
+% P = .44; I = .0005; D = 0.15;
+
+% theta-axis gains:
+% P = .08; I = 0; D = 0;
+
+% Pose_Matrix_data_3 = csvread('POSE_with_Py0.86_Iy0_Dy0.1ANDPt0.08_It0_Dt0.txt',1,0);
+% Vel_Matrix_data_3 = csvread('VEL_with_Py0.86_Iy0_Dy0.1ANDPt0.08_It0_Dt0.txt',1,0);
+
+Pose_Matrix_data_3 = csvread('Trans_PID_.28_0_.006.txt',1,0);
+Vel_Matrix_data_3 = csvread('vel_PID_.28_0_.006.txt',1,0);
+
+% time in position
+TimeP_ros_data_3 = Pose_Matrix_data_3(:,1); % ros time, needs to be converted to sec...
+
+% duriation =         end_time        -     start_time
+  durationP_data_3 = (TimeP_ros_data_3(size(TimeP_ros_data_3,1),:) - TimeP_ros_data_3(1,:))*10^(-9); 
+  
+t_P_sec_data_3 = 0:durationP_data_3/size(TimeP_ros_data_3,1):durationP_data_3;
+t_P_sec_data_3(:,size(t_P_sec_data_3,2)) = [];
+
+% time in vel
+TimeV_ros_data_3 = Vel_Matrix_data_3(:,1); % ros time, needs to be converted to sec...
+
+% duriation =         end_time        -     start_time
+  durationV_data_3 = (TimeV_ros_data_3(size(TimeV_ros_data_3,1),:) - TimeV_ros_data_3(1,:))*10^(-9);
+
+t_V_sec_data_3 = 0:durationV_data_3/size(TimeV_ros_data_3,1):durationV_data_3;
+t_V_sec_data_3(:,size(t_V_sec_data_3,2)) = [];
+
+% -----------------------------------------------------------------------------------------------------
+% data 4: 450 ros messages for geometry_msgs/Twist 
+%         451 ros messages for geometry_msgs/PoseStamped
+% y-axis gains:
+% P = .86; I = 0; D = 0.002;
+
+% theta-axis gains:
+% P = .08; I = 0; D = 0;
+% 
+% Pose_Matrix_data_4 = csvread('POSE_with_Py0.51_Iy0.0005_Dy0.05ANDPt0.08_It0_Dt0.txt',1,0);
+% Vel_Matrix_data_4 = csvread('VEL_with_Py0.51_Iy0.0005_Dy0.05ANDPt0.08_It0_Dt0.txt',1,0);
+
+Pose_Matrix_data_4 = csvread('CAM_PID_.266_.0003_.014.txt',1,0);
+Vel_Matrix_data_4 = csvread('vel_PID_.266_.0003_.014.txt',1,0);
+
+% time in position
+TimeP_ros_data_4 = Pose_Matrix_data_4(:,1); % ros time, needs to be converted to sec...
+
+% duriation =         end_time        -     start_time
+  durationP_data_4 = (TimeP_ros_data_4(size(TimeP_ros_data_4,1),:) - TimeP_ros_data_4(1,:))*10^(-9); 
+  
+t_P_sec_data_4 = 0:durationP_data_4/size(TimeP_ros_data_4,1):durationP_data_4;
+t_P_sec_data_4(:,size(t_P_sec_data_4,2)) = [];
+
+% time in vel
+TimeV_ros_data_4 = Vel_Matrix_data_4(:,1); % ros time, needs to be converted to sec...
+
+% duriation =         end_time        -     start_time
+  durationV_data_4 = (TimeV_ros_data_4(size(TimeV_ros_data_4,1),:) - TimeV_ros_data_4(1,:))*10^(-9);
+
+t_V_sec_data_4 = 0:durationV_data_4/size(TimeV_ros_data_4,1):durationV_data_4;
+t_V_sec_data_4(:,size(t_V_sec_data_4,2)) = [];
+
+% -----------------------------------------------------------------------------------------------------
+% data 5: ANDROID CAMERA
+
+
+Pose_Matrix_data_5 = csvread('Pose_with_Android.txt',1,0);
+Vel_Matrix_data_5 = csvread('Velocity_with_Android.txt',1,0);
+
+% time in position
+TimeP_ros_data_5 = Pose_Matrix_data_5(:,1); % ros time, needs to be converted to sec...
+
+% duriation =         end_time        -     start_time
+  durationP_data_5 = (TimeP_ros_data_5(size(TimeP_ros_data_5,1),:) - TimeP_ros_data_5(1,:))*10^(-9); 
+  
+t_P_sec_data_5 = 0:durationP_data_5/size(TimeP_ros_data_5,1):durationP_data_5;
+t_P_sec_data_5(:,size(t_P_sec_data_5,2)) = [];
+
+% time in vel
+TimeV_ros_data_5 = Vel_Matrix_data_5(:,1); % ros time, needs to be converted to sec...
+
+% duriation =         end_time        -     start_time
+  durationV_data_5 = (TimeV_ros_data_5(size(TimeV_ros_data_5,1),:) - TimeV_ros_data_5(1,:))*10^(-9);
+
+t_V_sec_data_5 = 0:durationV_data_5/size(TimeV_ros_data_5,1):durationV_data_5;
+t_V_sec_data_5(:,size(t_V_sec_data_5,2)) = [];
+
+% -----------------------------------------------------------------------------------------------------
+% data 6: with several oscilations
+
+
+Pose_Matrix_data_6 = csvread('CAM_PID_.39_.0005_.01.txt',1,0);
+Vel_Matrix_data_6 = csvread('vel_PID_.39_.0005_.01.txt',1,0);
+
+% time in position
+TimeP_ros_data_6 = Pose_Matrix_data_6(:,1); % ros time, needs to be converted to sec...
+
+% duriation =         end_time        -     start_time
+  durationP_data_6 = (TimeP_ros_data_6(size(TimeP_ros_data_6,1),:) - TimeP_ros_data_6(1,:))*10^(-9); 
+  
+t_P_sec_data_6 = 0:durationP_data_6/size(TimeP_ros_data_6,1):durationP_data_6;
+t_P_sec_data_6(:,size(t_P_sec_data_6,2)) = [];
+
+% time in vel
+TimeV_ros_data_6 = Vel_Matrix_data_6(:,1); % ros time, needs to be converted to sec...
+
+% duriation =         end_time        -     start_time
+  durationV_data_6 = (TimeV_ros_data_6(size(TimeV_ros_data_6,1),:) - TimeV_ros_data_6(1,:))*10^(-9);
+
+t_V_sec_data_6 = 0:durationV_data_6/size(TimeV_ros_data_6,1):durationV_data_6;
+t_V_sec_data_6(:,size(t_V_sec_data_6,2)) = [];
+
+
+%% Pose estimation
+
+%data 1:
+% when using marker pose ,,,,
+Pose_X_data_1 = Pose_Matrix_data_1(:,4);
+Pose_Y_data_1 = Pose_Matrix_data_1(:,3);
+Theta_data_1 = Pose_Matrix_data_1(:,7);
+
+% for i = 1:size(Theta_data_1,1)
+%     if (Theta_data_1(i) > 0)
+%         Theta_data_1(i) = Theta_data_1(i) - yaw_off;
+%     else if(Theta_data_1(i) < 0)
+%             Theta_data_1(i) = Theta_data_1(i) + yaw_off;
+%         end
+%     end
+% end
+% Extracting reference values when the robot is manually docked!
+ref_X_data_1 = Pose_X_data_1(size(Pose_X_data_1,1));
+ref_Y_data_1 = Pose_Y_data_1(size(Pose_Y_data_1,1));
+ref_Theta_data_1 = Theta_data_1(size(Theta_data_1,1));
+ref_Pose_data_1 =[ref_X_data_1;ref_Y_data_1];
+
+%data 2:
+% when using marker pose ,,,,
+Pose_X_data_2 = Pose_Matrix_data_2(:,4);
+Pose_Y_data_2 = Pose_Matrix_data_2(:,3);
+Theta_data_2 = Pose_Matrix_data_2(:,7);
+
+% for i = 1:size(Theta_data_2,1)
+%     if (Theta_data_2(i) > 0)
+%         Theta_data_2(i) = Theta_data_2(i) - yaw_off;
+%     else if(Theta_data_2(i) < 0)
+%             Theta_data_2(i) = Theta_data_2(i) + yaw_off;
+%         end
+%     end
+% end
+
+% Extracting reference values when the robot is manually docked!
+ref_X_data_2 = Pose_X_data_2(size(Pose_X_data_2,1));
+ref_Y_data_2 = Pose_Y_data_2(size(Pose_Y_data_2,1));
+ref_Theta_data_2 = Theta_data_2(size(Theta_data_2,1));
+ref_Pose_data_2 =[ref_X_data_2;ref_Y_data_2];
+
+%data 3:
+% when using marker pose ,,,,
+Pose_X_data_3 = Pose_Matrix_data_3(:,4);
+Pose_Y_data_3 = Pose_Matrix_data_3(:,3);
+Theta_data_3 = Pose_Matrix_data_3(:,7);
+
+% 
+% for i = 1:size(Theta_data_3,1)
+%     if (Theta_data_3(i) > 0)
+%         Theta_data_3(i) = Theta_data_3(i) - yaw_off;
+%     else if(Theta_data_3(i) < 0)
+%             Theta_data_3(i) = Theta_data_3(i) + yaw_off;
+%         end
+%     end
+% end
+% Extracting reference values when the robot is manually docked!
+ref_X_data_3 = Pose_X_data_3(size(Pose_X_data_3,1));
+ref_Y_data_3 = Pose_Y_data_3(size(Pose_Y_data_3,1));
+ref_Theta_data_3 = Theta_data_3(size(Theta_data_3,1));
+ref_Pose_data_3 =[ref_X_data_3;ref_Y_data_3];
+
+%data 4:
+% when using marker pose ,,,,
+Pose_X_data_4 = Pose_Matrix_data_4(:,4);
+Pose_Y_data_4 = Pose_Matrix_data_4(:,3);
+Theta_data_4 = Pose_Matrix_data_4(:,7);
+% 
+% for i = 1:size(Theta_data_4,1)
+%     if (Theta_data_4(i) > 0)
+%         Theta_data_4(i) = Theta_data_4(i) - yaw_off;
+%     else if(Theta_data_4(i) < 0)
+%             Theta_data_4(i) = Theta_data_4(i) + yaw_off;
+%         end
+%     end
+% end
+
+% Extracting reference values when the robot is manually docked!
+ref_X_data_4 = Pose_X_data_4(size(Pose_X_data_4,1));
+ref_Y_data_4 = Pose_Y_data_4(size(Pose_Y_data_4,1));
+ref_Theta_data_4 = Theta_data_4(size(Theta_data_4,1));
+ref_Pose_data_4 =[ref_X_data_4;ref_Y_data_4];
+
+
+%data 5:
+% when using marker pose ,,,,
+Pose_X_data_5 = Pose_Matrix_data_5(:,4);
+Pose_Y_data_5 = Pose_Matrix_data_5(:,3);
+Theta_data_5 = Pose_Matrix_data_5(:,7);
+
+% Extracting reference values when the robot is manually docked!
+ref_X_data_5 = Pose_X_data_5(size(Pose_X_data_5,1));
+ref_Y_data_5 = Pose_Y_data_5(size(Pose_Y_data_5,1));
+ref_Theta_data_5 = Theta_data_5(size(Theta_data_5,1));
+ref_Pose_data_5 =[ref_X_data_5;ref_Y_data_5];
+
+%data 6:
+% when using marker pose ,,,,
+Pose_X_data_6 = Pose_Matrix_data_6(:,4);
+Pose_Y_data_6 = Pose_Matrix_data_6(:,3);
+Theta_data_6 = Pose_Matrix_data_6(:,7);
+
+% Extracting reference values when the robot is manually docked!
+ref_X_data_6 = Pose_X_data_6(size(Pose_X_data_6,1));
+ref_Y_data_6 = Pose_Y_data_6(size(Pose_Y_data_6,1));
+ref_Theta_data_6 = Theta_data_6(size(Theta_data_6,1));
+ref_Pose_data_6 =[ref_X_data_6;ref_Y_data_6];
+
+% Reference Circle 
+thresh_X = .001;
+theta = 0:.001:2*pi;
+% needs to be adjusted manually if docking platform is replaced!
+ref_x = .2026;
+ref_y = .0087;
+x_circle = thresh_X*cos(theta) + ref_x; % ref_X needs to be recorded
+y_circle = thresh_X*sin(theta) + ref_y; % ref_Y needs to be recorded
+
+%% Velocity estimation
+
+% data 1:
+Vel_X_data_1 = Vel_Matrix_data_1(:,2);
+Vel_Y_data_1 = Vel_Matrix_data_1(:,3);
+Omega_Z_data_1 = Vel_Matrix_data_1(:,7);
+
+% data 2:
+Vel_X_data_2 = Vel_Matrix_data_2(:,2);
+Vel_Y_data_2 = Vel_Matrix_data_2(:,3);
+Omega_Z_data_2 = Vel_Matrix_data_2(:,7);
+
+% data 3:
+Vel_X_data_3 = Vel_Matrix_data_3(:,2);
+Vel_Y_data_3 = Vel_Matrix_data_3(:,3);
+Omega_Z_data_3 = Vel_Matrix_data_3(:,7);
+
+% data 4:
+Vel_X_data_4 = Vel_Matrix_data_4(:,2);
+Vel_Y_data_4 = Vel_Matrix_data_4(:,3);
+Omega_Z_data_4 = Vel_Matrix_data_4(:,7);
+
+% data 5:
+Vel_X_data_5 = Vel_Matrix_data_5(:,2);
+Vel_Y_data_5 = Vel_Matrix_data_5(:,3);
+Omega_Z_data_5 = Vel_Matrix_data_5(:,7);
+
+% data 6:
+Vel_X_data_6 = Vel_Matrix_data_6(:,2);
+Vel_Y_data_6 = Vel_Matrix_data_6(:,3);
+Omega_Z_data_6 = Vel_Matrix_data_6(:,7);
+
+%% Plots
+
+% % marker position
+% figure;
+% plot(t_P_sec_data_6,Pose_Y_data_6,'k','LineWidth',1.6);
+% xlabel('time');
+% ylabel('y');
+% 
+% figure;
+% plot(t_P_sec_data_6,Theta_data_6,'k','LineWidth',1.6);
+% xlabel('time');
+% ylabel('\theta');
+
+
+figure;
+set(gcf,'color','white');
+subplot(3,1,1);
+plot(t_P_sec_data_1,Pose_X_data_1,'LineWidth',2.1);
+hold on
+plot(t_P_sec_data_2,Pose_X_data_2,'r','LineWidth',1.7);
+hold on
+plot(t_P_sec_data_3,Pose_X_data_3,'g','LineWidth',1.8);
+hold on
+plot(t_P_sec_data_4,Pose_X_data_4,'m','LineWidth',1.5);
+title('Pose estimation in marker coordinate system');
+ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',14);
+
+l2 = legend('${P_y = 0.28, I_y = 0.0001, D_y = 0.01}$', '${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.28, I_y = 0, D_y = 0.006}$','${P_y = 0.266, I_y = 0.0003, D_y = 0.014}$');
+set(l2,'interpreter','latex','FontSize',11);
+legend('boxoff');
+
+subplot(3,1,2);
+plot(t_P_sec_data_1,Pose_Y_data_1,'LineWidth',2.1);
+hold on
+plot(t_P_sec_data_2,Pose_Y_data_2,'r','LineWidth',1.7);
+hold on
+plot(t_P_sec_data_3,Pose_Y_data_3,'g','LineWidth',1.8);
+hold on
+plot(t_P_sec_data_4,Pose_Y_data_4,'m','LineWidth',1.5);
+ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',14);
+
+subplot(3,1,3);
+plot(t_P_sec_data_1,Theta_data_1,'LineWidth',2.1);
+hold on
+plot(t_P_sec_data_2,Theta_data_2,'r','LineWidth',1.7);
+hold on
+plot(t_P_sec_data_3,Theta_data_3,'g','LineWidth',1.8);
+hold on
+plot(t_P_sec_data_4,Theta_data_4,'m','LineWidth',1.5);
+
+xlabel('$ time [sec]$','interpreter','latex','FontSize',14);
+ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',14);
+
+
+% control signals
+figure;
+set(gcf,'color','white');
+subplot(3,1,1);
+
+plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',2.1);
+hold on
+plot(t_V_sec_data_2,Vel_X_data_2,'r','LineWidth',1.7);
+hold on
+plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',1.8);
+hold on
+plot(t_V_sec_data_4,Vel_X_data_4,'m','LineWidth',1.5);
+
+title('Control Signals');
+ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);
+
+l2 = legend('${P_y = 0.28, I_y = 0.0001, D_y = 0.01}$', '${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.28, I_y = 0, D_y = 0.006}$','${P_y = 0.266, I_y = 0.0003, D_y = 0.014}$');
+set(l2,'interpreter','latex','FontSize',11);
+legend('boxoff');
+
+
+subplot(3,1,2);
+plot(t_V_sec_data_1,Vel_Y_data_1,'LineWidth',2.1);
+hold on
+plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.7);
+hold on
+plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',1.8);
+hold on
+plot(t_V_sec_data_4,Vel_Y_data_4,'m','LineWidth',1.5);
+ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);
+
+subplot(3,1,3);
+plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',2.1);
+hold on
+plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.7);
+hold on
+plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',1.8);
+hold on
+plot(t_V_sec_data_4,Omega_Z_data_4,'m','LineWidth',1.5);
+xlabel('$ time [sec]$','interpreter','latex','FontSize',14);
+ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14);
+
+
+% Trajectory 
+
+% 
+% figure;
+% plot(Pose_Y_data_1,Pose_X_data_1,'b',ref_Y_data_1,ref_X_data_1,'k*');
+% hold on
+% plot(Pose_Y_data_2,Pose_X_data_2,'r',ref_Y_data_2,ref_X_data_2,'k*');
+% 
+% hold on
+% plot(Pose_Y_data_3,Pose_X_data_3,'g',ref_Y_data_3,ref_X_data_3,'k*');
+
+
+% 
+% for k = Pose_X_data_1(1):Pose_X_data_1(size(Pose_X_data_1,1))
+%     x_arrow(k) = 
+%     
+% end
+% 
+% x_arrow = linspace(Pose_X_data_1(1),Pose_X_data_1(size(Pose_X_data_1,1)),150);
+% y_arrow = linspace(Pose_Y_data_1(1),Pose_Y_data_1(size(Pose_Y_data_1,1)),150);
+% 
+% Vx = gradient(Pose_X_data_1);
+% Vy = gradient(Pose_Y_data_1);
+
+% figure;
+% plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',3.5);
+% hold on
+% quiver(Pose_Y_data_1,Pose_X_data_1,Vy,Vx,'r--','LineWidth',.1);
+
+figure;
+plot(Pose_Y_data_6,Pose_X_data_6,'k','LineWidth',1.4);
+axis([-.3 .7 .1 1.6]);
+
+
+figure;
+subplot(1,3,1);
+plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
+% hold on
+% quiver(abs(Pose_Y_data_1),Pose_X_data_1,gradient(Pose_Y_data_1),gradient(Pose_X_data_1),'r--','LineWidth',.1);
+
+hold on
+plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
+% hold on
+% quiver(abs(Pose_Y_data_2),Pose_X_data_2,gradient(Pose_Y_data_2),gradient(Pose_X_data_2),'m--','LineWidth',.1);
+
+hold on
+plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
+% hold on
+% quiver(abs(Pose_Y_data_3),Pose_X_data_3,gradient(Pose_Y_data_3),gradient(Pose_X_data_3),'k--','LineWidth',.1);
+
+hold on
+plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
+% hold on
+% quiver(abs(Pose_Y_data_4),Pose_X_data_4,gradient(Pose_Y_data_4),gradient(Pose_X_data_4),'b--','LineWidth',.1);
+
+hold on
+plot(y_circle,x_circle,'k--','LineWidth',3.4);
+
+title('Approach zone + SM zone + Target area');
+ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',14);
+axis([-.05 .26 .19 1.52]);
+
+subplot(1,3,2);
+plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
+hold on
+plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
+hold on
+plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
+hold on
+plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
+hold on
+plot(y_circle,x_circle,'k--','LineWidth',3.4);
+
+title('SM zone + Target area');
+xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14);
+axis([-.023 0.02 .19 .29]);
+
+subplot(1,3,3);
+plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
+hold on
+plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
+hold on
+plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
+hold on
+plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
+hold on
+plot(y_circle,x_circle,'k--','LineWidth',3.4);
+
+title('Target area');
+axis([0.0072 .0101 .20 .2056]);
+
+l = legend('${P_y = 0.28, I_y = 0.0001, D_y = 0.01}$', '${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.28, I_y = 0, D_y = 0.006}$','${P_y = 0.266, I_y = 0.0003, D_y = 0.014}$','${Target}$');
+set(l,'interpreter','latex','FontSize',11);
+% legend('boxoff');
+
+
+% % sampling time
+% figure;
+% set(gcf,'color','white');
+% 
+% subplot(2,2,1);
+% plot(diff(TimeP_ros_data_3)/1e9,'r','LineWidth',1.4)
+% hold on
+% plot (diff(t_P_sec_data_3),'k--','LineWidth',1.8);
+% % hold on
+% title({'USB Camera';'Position'});
+% ylabel('Sampling time [sec]','interpreter','latex','FontSize',11);
+% axis([0 450 .068 .11]);
+% l3 = legend('${Time_{ROS}}$','${Time_{Real}} [sec]$','Orientation','horizontal');
+% set(l3,'interpreter','latex','FontSize',13);
+% 
+% subplot(2,2,2);
+% plot(diff(TimeP_ros_data_5)/1e9,'g','LineWidth',1.4)
+% hold on;
+% plot (diff(t_P_sec_data_5),'b--','LineWidth',1.8);
+% 
+% title({'Android Camera';'Position'});
+% axis([0 234 .068 .3]);
+% 
+% l4 = legend('${Time_{ROS}}$','${Time_{Real}} [sec]$','Orientation','horizontal');
+% set(l4,'interpreter','latex','FontSize',13);
+% legend('boxoff');
+% 
+% subplot(2,2,3);
+% plot(diff(TimeV_ros_data_3)/1e9,'r','LineWidth',1.4);
+% hold on;
+% plot (diff(t_V_sec_data_3),'k--','LineWidth',1.8);
+% % hold on;
+% title('Velocity');
+% xlabel('samples','interpreter','latex','FontSize',13);
+% ylabel('Sampling time [sec]','interpreter','latex','FontSize',13);
+% axis([0 450 .068 .11]);
+% 
+% subplot(2,2,4);
+% plot(diff(TimeV_ros_data_5)/1e9,'g','LineWidth',1.4);
+% hold on;
+% plot (diff(t_V_sec_data_5),'b--','LineWidth',1.8);
+% 
+% title('Velocity');
+% xlabel('samples','interpreter','latex','FontSize',13);
+% % ylabel('Sampling time [sec]','interpreter','latex','FontSize',11);
+% axis([0 234 .068 .3]);
+% 
+
+figure;
+set(gcf,'color','white');
+% 
+% subplot(2,1,1);
+pa1 = plot(diff(TimeP_ros_data_3)/1e9,'k','LineWidth',2.1);
+hold on
+pa2 = plot (diff(t_P_sec_data_3),'g--','LineWidth',1.5);
+hold on
+pa3 = plot(diff(TimeP_ros_data_5)/1e9,'b','LineWidth',1.4);
+hold on;
+pa4 = plot (diff(t_P_sec_data_5),'r--','LineWidth',1.8);
+hold off;
+axis([0 234 .05 .3]);
+% 
+% title('Position');
+ylabel('Sampling time [s]','interpreter','latex','FontSize',19);
+xlabel('Sample number','interpreter','latex','FontSize',19);
+% 
+% l5 = legend([pa1 pa2],'$USB Camera {Time_{ROS}}$','$ USB Camera {Time_{Real}} [s]$','Orientation','horizontal');
+% set(l5,'interpreter','latex','FontSize',11);
+% l6 = legend([pa3 pa4],'$ Android Camera {Time_{ROS}}$','$ Android Camera {Time_{Real}} [sec]$','Orientation','horizontal');
+% set(l6,'interpreter','latex','FontSize',12);
+% 
+set(gca,'fontsize',17);
+leg = legend('ROS Time, USB camera','Real Time, USB camera [s]','ROS Time, IP camera','Real Time, IP camera [s]','Location','northwest');
+set(leg,'FontSize',13);
+legend('boxoff');
+
+% 
+% subplot(2,1,2);
+% p1 = plot(diff(TimeV_ros_data_3)/1e9,'r','LineWidth',2.1);
+% hold on;
+% p2 = plot (diff(t_V_sec_data_3),'k--','LineWidth',1.5);
+% hold on;
+% p3 = plot(diff(TimeV_ros_data_5)/1e9,'g','LineWidth',1.4);
+% hold on;
+% p4 = plot (diff(t_V_sec_data_5),'b--','LineWidth',1.8);
+% hold off;
+% axis([0 234 0 .3]);
+% title('Velocity');
+% ylabel('Sampling time [s]','interpreter','latex','FontSize',13);
+% xlabel('Sample','interpreter','latex','FontSize',13);
+% 
+% set(gca,'fontsize',13);
+
+
+MaxOvershoot_1 = -ref_y + min(Pose_Y_data_1)
+MaxOvershoot_2 = -ref_y + min(Pose_Y_data_2)
+MaxOvershoot_3 = -ref_y + min(Pose_Y_data_3)
+MaxOvershoot_4 = -ref_y + min(Pose_Y_data_4)
+% Pose diff 
+% figure;
+% subplot(3,1,1);
+% plot(diff(Pose_X_data_1));
+% title('X-Pose diff');
+% xlabel('samples');
+% ylabel('X difference [m]');
+% grid on
+% 
+% subplot(3,1,2);
+% plot(diff(Pose_X_data_1));
+% title('Y-Pose diff');
+% xlabel('samples');
+% ylabel('Y difference [m]');
+% grid on
+% 
+% subplot(3,1,3);
+% plot(diff(Theta_data_1));
+% title('\theta-Pose diff');
+% xlabel('samples');
+% ylabel('\theta difference [rad]');
+% grid on
\ No newline at end of file
diff --git a/Files_4_thesis/Different_controllers/docking_with_PID_controller.m b/Files_4_thesis/Different_controllers/docking_with_PID_controller.m
index 0198c84553e4ea238fc5046296641acfae37956c..1effce590e4d3051abc28642ef8ddbb197a10a22 100644
--- a/Files_4_thesis/Different_controllers/docking_with_PID_controller.m
+++ b/Files_4_thesis/Different_controllers/docking_with_PID_controller.m
@@ -335,16 +335,16 @@ Omega_Z_data_6 = Vel_Matrix_data_6(:,7);
 
 %% Plots
 
-% marker position
-figure;
-plot(t_P_sec_data_6,Pose_Y_data_6,'k','LineWidth',1.6);
-xlabel('time');
-ylabel('y');
-
-figure;
-plot(t_P_sec_data_6,Theta_data_6,'k','LineWidth',1.6);
-xlabel('time');
-ylabel('\theta');
+% % marker position
+% figure;
+% plot(t_P_sec_data_6,Pose_Y_data_6,'k','LineWidth',1.6);
+% xlabel('time');
+% ylabel('y');
+% 
+% figure;
+% plot(t_P_sec_data_6,Theta_data_6,'k','LineWidth',1.6);
+% xlabel('time');
+% ylabel('\theta');
 
 
 figure;
@@ -526,93 +526,99 @@ set(l,'interpreter','latex','FontSize',11);
 % legend('boxoff');
 
 
-% sampling time
-figure;
-set(gcf,'color','white');
-
-subplot(2,2,1);
-plot(diff(TimeP_ros_data_3)/1e9,'r','LineWidth',1.4)
-hold on
-plot (diff(t_P_sec_data_3),'k--','LineWidth',1.8);
+% % sampling time
+% figure;
+% set(gcf,'color','white');
+% 
+% subplot(2,2,1);
+% plot(diff(TimeP_ros_data_3)/1e9,'r','LineWidth',1.4)
 % hold on
-title({'USB Camera';'Position'});
-ylabel('Sampling time [sec]','interpreter','latex','FontSize',11);
-axis([0 450 .068 .11]);
-l3 = legend('${Time_{ROS}}$','${Time_{Real}} [sec]$','Orientation','horizontal');
-set(l3,'interpreter','latex','FontSize',13);
-
-subplot(2,2,2);
-plot(diff(TimeP_ros_data_5)/1e9,'g','LineWidth',1.4)
-hold on;
-plot (diff(t_P_sec_data_5),'b--','LineWidth',1.8);
-
-title({'Android Camera';'Position'});
-axis([0 234 .068 .3]);
-
-l4 = legend('${Time_{ROS}}$','${Time_{Real}} [sec]$','Orientation','horizontal');
-set(l4,'interpreter','latex','FontSize',13);
-legend('boxoff');
-
-subplot(2,2,3);
-plot(diff(TimeV_ros_data_3)/1e9,'r','LineWidth',1.4);
-hold on;
-plot (diff(t_V_sec_data_3),'k--','LineWidth',1.8);
-% hold on;
-title('Velocity');
-xlabel('samples','interpreter','latex','FontSize',13);
-ylabel('Sampling time [sec]','interpreter','latex','FontSize',13);
-axis([0 450 .068 .11]);
-
-subplot(2,2,4);
-plot(diff(TimeV_ros_data_5)/1e9,'g','LineWidth',1.4);
-hold on;
-plot (diff(t_V_sec_data_5),'b--','LineWidth',1.8);
-
-title('Velocity');
-xlabel('samples','interpreter','latex','FontSize',13);
+% plot (diff(t_P_sec_data_3),'k--','LineWidth',1.8);
+% % hold on
+% title({'USB Camera';'Position'});
 % ylabel('Sampling time [sec]','interpreter','latex','FontSize',11);
-axis([0 234 .068 .3]);
-
+% axis([0 450 .068 .11]);
+% l3 = legend('${Time_{ROS}}$','${Time_{Real}} [sec]$','Orientation','horizontal');
+% set(l3,'interpreter','latex','FontSize',13);
+% 
+% subplot(2,2,2);
+% plot(diff(TimeP_ros_data_5)/1e9,'g','LineWidth',1.4)
+% hold on;
+% plot (diff(t_P_sec_data_5),'b--','LineWidth',1.8);
+% 
+% title({'Android Camera';'Position'});
+% axis([0 234 .068 .3]);
+% 
+% l4 = legend('${Time_{ROS}}$','${Time_{Real}} [sec]$','Orientation','horizontal');
+% set(l4,'interpreter','latex','FontSize',13);
+% legend('boxoff');
+% 
+% subplot(2,2,3);
+% plot(diff(TimeV_ros_data_3)/1e9,'r','LineWidth',1.4);
+% hold on;
+% plot (diff(t_V_sec_data_3),'k--','LineWidth',1.8);
+% % hold on;
+% title('Velocity');
+% xlabel('samples','interpreter','latex','FontSize',13);
+% ylabel('Sampling time [sec]','interpreter','latex','FontSize',13);
+% axis([0 450 .068 .11]);
+% 
+% subplot(2,2,4);
+% plot(diff(TimeV_ros_data_5)/1e9,'g','LineWidth',1.4);
+% hold on;
+% plot (diff(t_V_sec_data_5),'b--','LineWidth',1.8);
+% 
+% title('Velocity');
+% xlabel('samples','interpreter','latex','FontSize',13);
+% % ylabel('Sampling time [sec]','interpreter','latex','FontSize',11);
+% axis([0 234 .068 .3]);
+% 
 
 figure;
 set(gcf,'color','white');
-
-subplot(2,1,1);
-pa1 = plot(diff(TimeP_ros_data_3)/1e9,'r','LineWidth',2.1);
+% 
+% subplot(2,1,1);
+pa1 = plot(diff(TimeP_ros_data_3)/1e9,'k','LineWidth',1.5);
 hold on
-pa2 = plot (diff(t_P_sec_data_3),'k--','LineWidth',1.5);
+pa2 = plot (diff(t_P_sec_data_3),'g--','LineWidth',1.5);
 hold on
-pa3 = plot(diff(TimeP_ros_data_5)/1e9,'g','LineWidth',1.4);
+pa3 = plot(diff(TimeP_ros_data_5)/1e9,'b','LineWidth',1.5);
 hold on;
-pa4 = plot (diff(t_P_sec_data_5),'b--','LineWidth',1.8);
+pa4 = plot (diff(t_P_sec_data_5),'r--','LineWidth',1.8);
 hold off;
-axis([0 234 0 .3]);
-title('Position');
-% ylabel('Sampling time [sec]','interpreter','latex','FontSize',13);
+axis([0 234 .05 .3]);
+% 
+% title('Position');
+ylabel('Sampling time [s]','interpreter','latex','FontSize',20);
+xlabel('Sample number','interpreter','latex','FontSize',20);
 % 
-% l5 = legend([pa1 pa2],'$USB Camera {Time_{ROS}}$','$ USB Camera {Time_{Real}} [sec]$','Orientation','horizontal');
+% l5 = legend([pa1 pa2],'$USB Camera {Time_{ROS}}$','$ USB Camera {Time_{Real}} [s]$','Orientation','horizontal');
 % set(l5,'interpreter','latex','FontSize',11);
-
-% leg = 
-legend('ROS Time, USB camera','Real Time, USB camera [s]','ROS Time, IP camera','Real Time, IP camera [s]','Location','northwest');
+% l6 = legend([pa3 pa4],'$ Android Camera {Time_{ROS}}$','$ Android Camera {Time_{Real}} [sec]$','Orientation','horizontal');
+% set(l6,'interpreter','latex','FontSize',12);
+% 
+set(gca,'fontsize',20);
+leg = legend('ROS Time, USB camera','Real Time, USB camera [s]','ROS Time, IP camera','Real Time, IP camera [s]','Location','northwest');
+set(leg,'FontSize',15);
 legend('boxoff');
 
-subplot(2,1,2);
-p1 = plot(diff(TimeV_ros_data_3)/1e9,'r','LineWidth',2.1);
-hold on;
-p2 = plot (diff(t_V_sec_data_3),'k--','LineWidth',1.5);
-hold on;
-p3 = plot(diff(TimeV_ros_data_5)/1e9,'g','LineWidth',1.4);
-hold on;
-p4 = plot (diff(t_V_sec_data_5),'b--','LineWidth',1.8);
-hold off;
-axis([0 234 0 .3]);
-title('Velocity');
-% ylabel('Sampling time [sec]','interpreter','latex','FontSize',13);
+% 
+% subplot(2,1,2);
+% p1 = plot(diff(TimeV_ros_data_3)/1e9,'r','LineWidth',2.1);
+% hold on;
+% p2 = plot (diff(t_V_sec_data_3),'k--','LineWidth',1.5);
+% hold on;
+% p3 = plot(diff(TimeV_ros_data_5)/1e9,'g','LineWidth',1.4);
+% hold on;
+% p4 = plot (diff(t_V_sec_data_5),'b--','LineWidth',1.8);
+% hold off;
+% axis([0 234 0 .3]);
+% title('Velocity');
+% ylabel('Sampling time [s]','interpreter','latex','FontSize',13);
 % xlabel('Sample','interpreter','latex','FontSize',13);
 % 
-% l6 = legend([p3 p4],'$ Android Camera {Time_{ROS}}$','$ Android Camera {Time_{Real}} [sec]$','Orientation','horizontal');
-% set(l6,'interpreter','latex','FontSize',12);
+% set(gca,'fontsize',13);
+
 
 MaxOvershoot_1 = -ref_y + min(Pose_Y_data_1)
 MaxOvershoot_2 = -ref_y + min(Pose_Y_data_2)
diff --git a/Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.asv b/Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.asv
new file mode 100644
index 0000000000000000000000000000000000000000..3a19580683d35af3496a2bffffbc179178cbe5e6
--- /dev/null
+++ b/Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.asv
@@ -0,0 +1,400 @@
+clc;
+close all;
+clear all;
+%% Import data and time conversion
+
+% data 1: 556 ros messages for geometry_msgs/Twist 
+%         557 ros messages for geometry_msgs/PoseStamped
+% y-axis gains:
+% (010816_1718) best!!!
+% P = .4; I = 0; D = 0; 
+
+% y-axis gains:
+% (280716_2024) better!!!
+% P = .44; I = 0; D = 0; 
+
+% theta-axis gains:
+% P = .08; I = 0; D = 0;
+
+% y-axis gains:
+% (280716_2002)
+% P = .46; I = 0; D = 0; 
+
+% theta-axis gains:
+% P = .08; I = 0; D = 0;
+
+% Pose_Matrix_data_1 = csvread('Pose01_08_16_1718.txt',1,0);
+% Vel_Matrix_data_1 = csvread('Velocity01_08_16_1718.txt',1,0);
+
+Pose_Matrix_data_1 = csvread('CAM_x_dot_.1.txt',1,0);
+Vel_Matrix_data_1 = csvread('x_dot_.1.txt',1,0);
+
+% time in position
+TimeP_ros_data_1 = Pose_Matrix_data_1(:,1); % ros time, needs to be converted to sec...
+
+% duriation =         end_time        -     start_time
+  durationP_data_1 = (TimeP_ros_data_1(size(TimeP_ros_data_1,1),:) - TimeP_ros_data_1(1,:))*10^(-9); 
+  
+t_P_sec_data_1 = 0:durationP_data_1/size(TimeP_ros_data_1,1):durationP_data_1;
+t_P_sec_data_1(:,size(t_P_sec_data_1,2)) = [];
+
+% time in vel
+TimeV_ros_data_1 = Vel_Matrix_data_1(:,1); % ros time, needs to be converted to sec...
+
+% duriation =         end_time        -     start_time
+  durationV_data_1 = (TimeV_ros_data_1(size(TimeV_ros_data_1,1),:) - TimeV_ros_data_1(1,:))*10^(-9);
+
+t_V_sec_data_1 = 0:durationV_data_1/size(TimeV_ros_data_1,1):durationV_data_1;
+t_V_sec_data_1(:,size(t_V_sec_data_1,2)) = [];
+
+% -----------------------------------------------------------------------------------------------------
+% data 2: 
+% py = .25;     ptheta = .25;
+% iy = 0;       itheta = 0;
+% dy = 0;       dtheta = 0;
+
+% Pose_Matrix_data_2 = csvread('Pose01_08_16_1749.txt',1,0);
+% Vel_Matrix_data_2 = csvread('Velocity01_08_16_1749.txt',1,0);
+
+
+Pose_Matrix_data_2 = csvread('CAM_x_dot_.15.txt',1,0);
+Vel_Matrix_data_2 = csvread('x_dot_.15.txt',1,0);
+
+% time in position
+TimeP_ros_data_2 = Pose_Matrix_data_2(:,1); % ros time, needs to be converted to sec...
+
+% duriation =         end_time        -     start_time
+  durationP_data_2 = (TimeP_ros_data_2(size(TimeP_ros_data_2,1),:) - TimeP_ros_data_2(1,:))*10^(-9); 
+  
+t_P_sec_data_2 = 0:durationP_data_2/size(TimeP_ros_data_2,1):durationP_data_2;
+t_P_sec_data_2(:,size(t_P_sec_data_2,2)) = [];
+
+% time in vel
+TimeV_ros_data_2 = Vel_Matrix_data_2(:,1); % ros time, needs to be converted to sec...
+
+% duriation =         end_time        -     start_time
+  durationV_data_2 = (TimeV_ros_data_2(size(TimeV_ros_data_2,1),:) - TimeV_ros_data_2(1,:))*10^(-9);
+
+t_V_sec_data_2 = 0:durationV_data_2/size(TimeV_ros_data_2,1):durationV_data_2;
+t_V_sec_data_2(:,size(t_V_sec_data_2,2)) = [];
+
+% -----------------------------------------------------------------------------------------------------
+% data 3: 
+% Pose_Matrix_data_3 = csvread('Pose01_08_16_1831.txt',1,0);
+% Vel_Matrix_data_3 = csvread('Velocity01_08_16_1831.txt',1,0);
+
+
+Pose_Matrix_data_3 = csvread('CAM_x_dot_.16.txt',1,0);
+Vel_Matrix_data_3 = csvread('x_dot_.16.txt',1,0);
+
+% time in position
+TimeP_ros_data_3 = Pose_Matrix_data_3(:,1); % ros time, needs to be converted to sec...
+
+% duriation =         end_time        -     start_time
+  durationP_data_3 = (TimeP_ros_data_3(size(TimeP_ros_data_3,1),:) - TimeP_ros_data_3(1,:))*10^(-9); 
+  
+t_P_sec_data_3 = 0:durationP_data_3/size(TimeP_ros_data_3,1):durationP_data_3;
+t_P_sec_data_3(:,size(t_P_sec_data_3,2)) = [];
+
+% time in vel
+TimeV_ros_data_3 = Vel_Matrix_data_3(:,1); % ros time, needs to be converted to sec...
+
+% duriation =         end_time        -     start_time
+  durationV_data_3 = (TimeV_ros_data_3(size(TimeV_ros_data_3,1),:) - TimeV_ros_data_3(1,:))*10^(-9);
+
+t_V_sec_data_3 = 0:durationV_data_3/size(TimeV_ros_data_3,1):durationV_data_3;
+t_V_sec_data_3(:,size(t_V_sec_data_3,2)) = [];
+
+
+%% Pose estimation
+
+%data 1:
+% when using marker pose ,,,,
+Pose_X_data_1 = Pose_Matrix_data_1(:,4);
+Pose_Y_data_1 = Pose_Matrix_data_1(:,3);
+Theta_data_1 = Pose_Matrix_data_1(:,7);
+
+
+% Extracting reference values when the robot is manually docked!
+ref_X_data_1 = Pose_X_data_1(size(Pose_X_data_1,1));
+ref_Y_data_1 = Pose_Y_data_1(size(Pose_Y_data_1,1));
+ref_Theta_data_1 = Theta_data_1(size(Theta_data_1,1));
+ref_Pose_data_1 =[ref_X_data_1;ref_Y_data_1];
+
+%data 2:
+% when using marker pose ,,,,
+Pose_X_data_2 = Pose_Matrix_data_2(:,4);
+Pose_Y_data_2 = Pose_Matrix_data_2(:,3);
+Theta_data_2 = Pose_Matrix_data_2(:,7);
+
+% Extracting reference values when the robot is manually docked!
+ref_X_data_2 = Pose_X_data_2(size(Pose_X_data_2,1));
+ref_Y_data_2 = Pose_Y_data_2(size(Pose_Y_data_2,1));
+ref_Theta_data_2 = Theta_data_2(size(Theta_data_2,1));
+ref_Pose_data_2 =[ref_X_data_2;ref_Y_data_2];
+
+%data 3:
+% when using marker pose ,,,,
+Pose_X_data_3 = Pose_Matrix_data_3(:,4);
+Pose_Y_data_3 = Pose_Matrix_data_3(:,3);
+Theta_data_3 = Pose_Matrix_data_3(:,7);
+
+
+% Extracting reference values when the robot is manually docked!
+ref_X_data_3 = Pose_X_data_3(size(Pose_X_data_3,1));
+ref_Y_data_3 = Pose_Y_data_3(size(Pose_Y_data_3,1));
+ref_Theta_data_3 = Theta_data_3(size(Theta_data_3,1));
+ref_Pose_data_3 =[ref_X_data_3;ref_Y_data_3];
+
+thresh_X = .001;
+theta = 0:.001:2*pi;
+
+% needs to be adjusted manually if docking platform is replaced!
+ref_x = -.203;
+ref_y = -.019;
+
+x_circle = thresh_X*cos(theta) + ref_x; % ref_X needs to be recorded
+y_circle = thresh_X*sin(theta) + ref_y; % ref_Y needs to be recorded
+
+
+%% Velocity estimation
+
+% data 1:
+Vel_X_data_1 = Vel_Matrix_data_1(:,2);
+Vel_Y_data_1 = Vel_Matrix_data_1(:,3);
+Omega_Z_data_1 = Vel_Matrix_data_1(:,7);
+
+% data 2:
+Vel_X_data_2 = Vel_Matrix_data_2(:,2);
+Vel_Y_data_2 = Vel_Matrix_data_2(:,3);
+Omega_Z_data_2 = Vel_Matrix_data_2(:,7);
+
+% data 3:
+Vel_X_data_3 = Vel_Matrix_data_3(:,2);
+Vel_Y_data_3 = Vel_Matrix_data_3(:,3);
+Omega_Z_data_3 = Vel_Matrix_data_3(:,7);
+
+
+%% Plots
+% 
+% 
+% % marker position
+% figure;
+% subplot(3,1,1);
+% plot(t_P_sec_data_1,Pose_X_data_1);
+% hold on
+% plot(t_P_sec_data_2,Pose_X_data_2,'r');
+% hold on
+% plot(t_P_sec_data_3,Pose_X_data_3,'g');
+% 
+% title('Marker X-axis','Color','r');
+% % xlabel('Time [sec]');
+% ylabel('X [m]');
+% grid on
+% 
+% subplot(3,1,2);
+% plot(t_P_sec_data_1,Pose_Y_data_1);
+% hold on
+% plot(t_P_sec_data_2,Pose_Y_data_2,'r');
+% hold on
+% plot(t_P_sec_data_3,Pose_Y_data_3,'g');
+% 
+% title('Marker Y-axis');
+% % xlabel('Time [sec]');
+% ylabel('Y [m]');
+% grid on
+% 
+% subplot(3,1,3);
+% plot(t_P_sec_data_1,Theta_data_1);
+% hold on
+% plot(t_P_sec_data_2,Theta_data_2,'r');
+% hold on
+% plot(t_P_sec_data_3,Theta_data_3,'g');
+% 
+% title('Marker \theta-axis');
+% xlabel('Time [s]');
+% ylabel('\theta [rad]');
+% grid on
+
+
+
+% control signals
+figure;
+set(gcf,'color','white');
+
+subplot(3,1,1);
+plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',2);
+hold on
+plot(t_V_sec_data_2,Vel_X_data_2,'r','LineWidth',1.6);
+hold on
+plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',2);
+
+title('a) $\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
+% set(get(gca,'title'),'Position',([17 5]));
+% xlabel('$ time [sec]$','interpreter','latex','FontSize',14);
+ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
+% grid on
+axis([0 35 -.01 .2]);
+l = legend('$\dot{x}_{Rob}$ = 0.1 [m/s]','$\dot{x}_{Rob}$ = 0.15[m/s]','$\dot{x}_{Rob}$ = 0.16[m/s]');
+set(l,'interpreter','latex','FontSize',12);
+legend('boxoff');
+
+subplot(3,1,2);
+plot(t_V_sec_data_1,Vel_Y_data_1,'LineWidth',2.5);
+hold on
+plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.5);
+hold on
+plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',2.1);
+title('a) $\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
+ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
+% grid on
+% axis([0 46 -.2 .1]);
+
+subplot(3,1,3);
+plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',2.5);
+hold on
+plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.45);
+hold on
+plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',2.1);
+title('a) $\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
+xlabel('$ time $ [s]','interpreter','latex','FontSize',14);
+ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14);
+% grid on
+
+% axis([0 46 -.1 .15]);
+
+
+
+% Trajectory 
+
+% figure;
+% plot(Pose_Y_data_1,Pose_X_data_1,'b',ref_Y_data_1,ref_X_data_1,'k*');
+% hold on
+% plot(Pose_Y_data_2,Pose_X_data_2,'r',ref_Y_data_2,ref_X_data_2,'k*');
+% 
+% hold on
+% plot(Pose_Y_data_3,Pose_X_data_3,'g',ref_Y_data_3,ref_X_data_3,'k*');
+% 
+
+% 
+% for k = Pose_X_data_1(1):Pose_X_data_1(size(Pose_X_data_1,1))
+%     x_arrow(k) = 
+%     
+% end
+% 
+% x_arrow = linspace(Pose_X_data_1(1),Pose_X_data_1(size(Pose_X_data_1,1)),150);
+% y_arrow = linspace(Pose_Y_data_1(1),Pose_Y_data_1(size(Pose_Y_data_1,1)),150);
+% 
+% Vx = gradient(x_arrow,.01);
+% Vy = gradient(y_arrow,.01);
+% 
+% figure;
+% plot(y_arrow,x_arrow,'b--','LineWidth',.5);
+% % hold on
+% % quiver(y_arrow,x_arrow,Vy,Vx,'r--','LineWidth',1.5);
+% 
+
+figure;
+set(gcf,'color','white');
+
+subplot(131);
+plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
+hold on
+plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
+hold on
+plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
+hold on
+plot(y_circle,x_circle,'k--','LineWidth',3.4)
+
+title('Approach zone + SM zone + Target area');
+% xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14);
+ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',16);
+axis([-.65 .01 -1.47 -.185]);
+% grid on
+l2 = legend('$\dot{x}_{Rob}$ = 0.1[m/s]','$\dot{x}_{Rob}$ = 0.15[m/s]','$\dot{x}_{Rob}$ = 0.16[m/s]','Target','Location','northwest');
+set(l2,'interpreter','latex','FontSize',14);
+legend('boxoff');
+
+subplot(1,3,2);
+plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
+hold on
+plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
+hold on
+plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
+hold on
+plot(y_circle,x_circle,'k--','LineWidth',3.4);
+title('SM zone + Target area');
+xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',16);
+% ylabel('${X}$ [m]','interpreter','latex');
+axis([-.033 -.01 -.26 -.195]);
+% grid on
+
+subplot(133);
+plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
+hold on
+plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
+hold on
+plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
+hold on
+plot(y_circle,x_circle,'k--','LineWidth',3.4);
+title('Target area');
+% xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14);
+% ylabel('${X}$ [m]','interpreter','latex');
+axis([-.0205 -.0175 -.206 -.20]);
+% grid on
+% l2 = legend('x_dot = 0.15','x_dot = 0.1','x_dot = 0.16','Reference Position');
+
+figure;
+set(gcf,'color','white');
+
+subplot(2,1,1);
+plot(diff(TimeP_ros_data_1)/1e9,'g')
+hold on
+plot (diff(t_P_sec_data_1),'k--')
+title('Position sampling time');
+ylabel('Sampling time');
+legend('ROS calc.','Real time calc. [sec]','Orientation','horizontal');
+% grid on
+% axis([0 428 .05 .15]);
+
+subplot(2,1,2);
+plot(diff(TimeV_ros_data_1)/1e9,'g');
+hold on
+plot (diff(t_V_sec_data_1),'k--')
+title('Velocity sampling time');
+xlabel('samples');
+ylabel('Sampling time');
+% legend('ROS calc.','Ordinary calc.','Orientation','horizontal');
+% grid on
+% axis([0 428 -.005 .025]);
+% 
+% subplot(3,1,3);
+% plot(diff(TimeV_ros_data_1)/1e9,'g')
+% hold on
+% plot (diff(t_V_sec_data_1),'k--')
+% xlabel('samples');
+% ylabel('time step');
+% axis([150 250 0 .02]);
+% figure;
+% set(gcf,'color','white');
+% 
+% subplot(3,1,1);
+% plot(diff(Pose_X_data_1));
+% title('X-Pose diff');
+% xlabel('samples');
+% ylabel('X difference [m]');
+% grid on
+% 
+% subplot(3,1,2);
+% plot(diff(Pose_X_data_1));
+% title('Y-Pose diff');
+% xlabel('samples');
+% ylabel('Y difference [m]');
+% grid on
+% 
+% subplot(3,1,3);
+% plot(diff(Theta_data_1));
+% title('\theta-Pose diff');
+% xlabel('samples');
+% ylabel('\theta difference [rad]');
+% grid on
\ No newline at end of file
diff --git a/Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.m b/Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.m
index d9b541c7506fb48faad29953248766c33d5bdc6c..9aa19655367fe866d9e5b052f6031566766b241f 100644
--- a/Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.m
+++ b/Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.m
@@ -176,45 +176,45 @@ Omega_Z_data_3 = Vel_Matrix_data_3(:,7);
 
 
 %% Plots
-
-
-% marker position
-figure;
-subplot(3,1,1);
-plot(t_P_sec_data_1,Pose_X_data_1);
-hold on
-plot(t_P_sec_data_2,Pose_X_data_2,'r');
-hold on
-plot(t_P_sec_data_3,Pose_X_data_3,'g');
-
-title('Marker X-axis','Color','r');
-% xlabel('Time [sec]');
-ylabel('X [m]');
-grid on
-
-subplot(3,1,2);
-plot(t_P_sec_data_1,Pose_Y_data_1);
-hold on
-plot(t_P_sec_data_2,Pose_Y_data_2,'r');
-hold on
-plot(t_P_sec_data_3,Pose_Y_data_3,'g');
-
-title('Marker Y-axis');
-% xlabel('Time [sec]');
-ylabel('Y [m]');
-grid on
-
-subplot(3,1,3);
-plot(t_P_sec_data_1,Theta_data_1);
-hold on
-plot(t_P_sec_data_2,Theta_data_2,'r');
-hold on
-plot(t_P_sec_data_3,Theta_data_3,'g');
-
-title('Marker \theta-axis');
-xlabel('Time [s]');
-ylabel('\theta [rad]');
-grid on
+% 
+% 
+% % marker position
+% figure;
+% subplot(3,1,1);
+% plot(t_P_sec_data_1,Pose_X_data_1);
+% hold on
+% plot(t_P_sec_data_2,Pose_X_data_2,'r');
+% hold on
+% plot(t_P_sec_data_3,Pose_X_data_3,'g');
+% 
+% title('Marker X-axis','Color','r');
+% % xlabel('Time [sec]');
+% ylabel('X [m]');
+% grid on
+% 
+% subplot(3,1,2);
+% plot(t_P_sec_data_1,Pose_Y_data_1);
+% hold on
+% plot(t_P_sec_data_2,Pose_Y_data_2,'r');
+% hold on
+% plot(t_P_sec_data_3,Pose_Y_data_3,'g');
+% 
+% title('Marker Y-axis');
+% % xlabel('Time [sec]');
+% ylabel('Y [m]');
+% grid on
+% 
+% subplot(3,1,3);
+% plot(t_P_sec_data_1,Theta_data_1);
+% hold on
+% plot(t_P_sec_data_2,Theta_data_2,'r');
+% hold on
+% plot(t_P_sec_data_3,Theta_data_3,'g');
+% 
+% title('Marker \theta-axis');
+% xlabel('Time [s]');
+% ylabel('\theta [rad]');
+% grid on
 
 
 
@@ -229,13 +229,14 @@ plot(t_V_sec_data_2,Vel_X_data_2,'r','LineWidth',1.6);
 hold on
 plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',2);
 
-title('Control Signals');
+title('a) $\dot{x}_{Rob}$','interpreter','latex','FontSize',20);
+% set(get(gca,'title'),'Position',([17 5]));
 % xlabel('$ time [sec]$','interpreter','latex','FontSize',14);
-ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);
+ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
 % grid on
 axis([0 35 -.01 .2]);
 l = legend('$\dot{x}_{Rob}$ = 0.1 [m/s]','$\dot{x}_{Rob}$ = 0.15[m/s]','$\dot{x}_{Rob}$ = 0.16[m/s]');
-set(l,'interpreter','latex','FontSize',12);
+set(l,'interpreter','latex','FontSize',18);
 legend('boxoff');
 
 subplot(3,1,2);
@@ -244,9 +245,8 @@ hold on
 plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.5);
 hold on
 plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',2.1);
-
-% title('Control Signal  y_{Rob} - axis');
-ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);
+title('b) $\dot{y}_{Rob}$','interpreter','latex','FontSize',20);
+ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
 % grid on
 % axis([0 46 -.2 .1]);
 
@@ -256,16 +256,14 @@ hold on
 plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.45);
 hold on
 plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',2.1);
-
-% title('Control Signal  \theta_{Rob} - axis');
-xlabel('$ time $ [s]','interpreter','latex','FontSize',14);
-ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14);
+title('c) $\dot{\theta}_{Rob}$','interpreter','latex','FontSize',20);
+xlabel('$ time $ [s]','interpreter','latex','FontSize',20);
+ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',20);
 % grid on
 
 % axis([0 46 -.1 .15]);
 
 
-
 % Trajectory 
 
 % figure;
@@ -306,10 +304,9 @@ hold on
 plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
 hold on
 plot(y_circle,x_circle,'k--','LineWidth',3.4)
-
-title('Approach zone + SM zone + Target area');
+title('a) Approach zone + SM zone + Target area','FontSize',17);
 % xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14);
-ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',16);
+ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',20);
 axis([-.65 .01 -1.47 -.185]);
 % grid on
 l2 = legend('$\dot{x}_{Rob}$ = 0.1[m/s]','$\dot{x}_{Rob}$ = 0.15[m/s]','$\dot{x}_{Rob}$ = 0.16[m/s]','Target','Location','northwest');
@@ -324,8 +321,8 @@ hold on
 plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
 hold on
 plot(y_circle,x_circle,'k--','LineWidth',3.4);
-title('SM zone + Target area');
-xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',16);
+title('b) SM zone + Target area','FontSize',17);
+xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',20);
 % ylabel('${X}$ [m]','interpreter','latex');
 axis([-.033 -.01 -.26 -.195]);
 % grid on
@@ -338,7 +335,7 @@ hold on
 plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
 hold on
 plot(y_circle,x_circle,'k--','LineWidth',3.4);
-title('Target area');
+title('c) Target area','FontSize',17);
 % xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14);
 % ylabel('${X}$ [m]','interpreter','latex');
 axis([-.0205 -.0175 -.206 -.20]);
diff --git a/Files_4_thesis/Report_with_updated_plots.docx b/Files_4_thesis/Report_with_updated_plots.docx
index 6a5a6c9e27327b47496f51d29a35d6dd88515cc1..7e90183996c4256ed1661ee3edb7fbcc864ed6c8 100644
Binary files a/Files_4_thesis/Report_with_updated_plots.docx and b/Files_4_thesis/Report_with_updated_plots.docx differ
diff --git a/Files_4_thesis/UP_REP_Farid_Alijani.pdf b/Files_4_thesis/UP_REP_Farid_Alijani.pdf
index 0b5cb49fdfbfca996acf77aed4165dc827fb537e..ef29a9eb61244b667ed8c6cfb4c367c276baa865 100644
Binary files a/Files_4_thesis/UP_REP_Farid_Alijani.pdf and b/Files_4_thesis/UP_REP_Farid_Alijani.pdf differ
diff --git a/Files_4_thesis/scanner_analysis.asv b/Files_4_thesis/scanner_analysis.asv
new file mode 100644
index 0000000000000000000000000000000000000000..025b70470ae2b206f7b73bff3ac4ac7beff576bc
--- /dev/null
+++ b/Files_4_thesis/scanner_analysis.asv
@@ -0,0 +1,402 @@
+clc;
+close all;
+clear all;
+
+upper_angle = 135; % deg.
+lower_angle = -135; % deg.
+dist_to_obst = .1; % m.
+
+%% docked robot for 5 different experiences...
+
+S_mat = csvread('Scan_Data_5_exp.csv');
+
+indx = S_mat(:,1);
+
+s_exp_1 = S_mat(:,2);
+s_exp_2 = S_mat(:,3);
+s_exp_3 = S_mat(:,4);
+s_exp_4 = S_mat(:,5);
+s_exp_5 = S_mat(:,6);
+
+
+points = size(indx,1); % to calculate the angle...
+laser_beam = points -1;
+
+angle = zeros(points,1);
+X_exp_1 = zeros(points,1);
+X_exp_2 = zeros(points,1);
+X_exp_3 = zeros(points,1);
+X_exp_4 = zeros(points,1);
+X_exp_5 = zeros(points,1);
+X_obst = zeros(points,1);
+
+Y_exp_1 = zeros(points,1);
+Y_exp_2 = zeros(points,1);
+Y_exp_3 = zeros(points,1);
+Y_exp_4 = zeros(points,1);
+Y_exp_5 = zeros(points,1);
+Y_obst = zeros(points,1);
+
+
+
+for i = 1:points
+    
+    angle(i) = (lower_angle + (indx(i)* ((upper_angle - lower_angle)/(laser_beam))))* pi/180;
+    
+    X_exp_1(i) = s_exp_1(i) * cos(angle(i));
+    X_exp_2(i) = s_exp_2(i) * cos(angle(i));
+    X_exp_3(i) = s_exp_3(i) * cos(angle(i));
+    X_exp_4(i) = s_exp_4(i) * cos(angle(i));
+    X_exp_5(i) = s_exp_5(i) * cos(angle(i));
+    X_obst(i) = dist_to_obst * cos(angle(i));
+    
+    Y_exp_1(i) = s_exp_1(i) * sin(angle(i));
+    Y_exp_2(i) = s_exp_2(i) * sin(angle(i));
+    Y_exp_3(i) = s_exp_3(i) * sin(angle(i));
+    Y_exp_4(i) = s_exp_4(i) * sin(angle(i));
+    Y_exp_5(i) = s_exp_5(i) * sin(angle(i));
+    Y_obst(i) = dist_to_obst * sin(angle(i));    
+end
+
+% theta vs. scanner_value & reference
+figure;
+set(gcf,'color','white');
+subplot(1,2,1);
+
+% plot(angle,s_exp_1,'m--','LineWidth',1.1);
+% hold on;
+plot(angle,s_exp_2,'m--','LineWidth',1.9);
+hold on;
+plot(angle,s_exp_3,'b--','LineWidth',1.9);
+hold on;
+plot(angle,s_exp_4,'g--','LineWidth',1.1);
+hold on;
+% plot(angle,s_exp_5,'b--','LineWidth',.8);
+% hold on;
+plot(angle,dist_to_obst,'rx','LineWidth',.05);
+title('');
+xlabel('${\theta_b}$ [rad]','interpreter','latex','FontSize',20);
+ylabel('${Distance}$ [m]','interpreter','latex','FontSize',20);
+axis([-.09 .42 .08 .2]);
+set(gca,'box','off','fontsize',20);
+% legend('Experiment 1','Experiment 2','Experiment 3','Safety Curve');
+subplot(1,2,2);
+% plot(X_exp_1,Y_exp_1,'m--','LineWidth',.9);
+% hold on
+plot(X_exp_2,Y_exp_2,'m--','LineWidth',1.9);
+hold on
+plot(X_exp_3,Y_exp_3,'b--','LineWidth',1.9);
+hold on
+plot(X_exp_4,Y_exp_4,'g--','LineWidth',1.1);
+hold on
+% plot(X_exp_5,Y_exp_5,'b--','LineWidth',.1);
+% hold on
+plot(X_obst,Y_obst,'r--','LineWidth',.05);
+title('');
+xlabel('${X}$ [m]','interpreter','latex','FontSize',20);
+ylabel('${Y}$ [m]','interpreter','latex','FontSize',20);
+axis([-.1 .16 -.18 .18]);
+set(gca,'box','off','fontsize',20);
+l = legend('Trial 1','Trial 2','Trial 3','Safety Curve','Orientation','horizontal');
+set(l,'interpreter','latex','FontSize',13);
+
+%% docked robot with marker vs. docked robot without marker!
+S_Mat_DockedwithMar = csvread('scanner_data_Docked_WITH_Marker.txt',0,0);
+S_Mat_DockedwithNOmar = csvread('scanner_data_no_cylinder.txt',0,0);
+
+indx_DockedwithMar = S_Mat_DockedwithMar(:,1);
+s_val_DockedwithMar = S_Mat_DockedwithMar(:,2);
+
+indx_DockedwithNOmar = S_Mat_DockedwithNOmar(:,1);
+s_val_DockedwithNOmar = S_Mat_DockedwithNOmar(:,2);
+
+points_DockedwithMar = size(indx_DockedwithMar,1);
+points_DockedwithNOmar = size(indx_DockedwithNOmar,1);
+
+laser_beam_DockedwithMar = points_DockedwithMar - 1;
+laser_beam_DockedwithNOmar = points_DockedwithNOmar - 1;
+
+s_angle_DockedwithMar = zeros(points_DockedwithMar,1);
+s_angle_DockedwithNOmar = zeros(points_DockedwithNOmar,1);
+
+X_DockedwithMar = zeros(points_DockedwithMar,1); Y_DockedwithMar = zeros(points_DockedwithMar,1);
+X_DockedwithNOmar = zeros(points_DockedwithNOmar,1); Y_DockedwithNOmar = zeros(points_DockedwithNOmar,1);
+
+X_obst_DockedwithMar = zeros(points_DockedwithMar,1); Y_obst_DockedwithMar = zeros(points_DockedwithMar,1);
+X_obst_DockedwithNOmar = zeros(points_DockedwithNOmar,1); Y_obst_DockedwithNOmar = zeros(points_DockedwithNOmar,1);
+
+
+for i = 1:points_DockedwithMar
+    s_angle_DockedwithMar(i) = (lower_angle + (indx_DockedwithMar(i)* ((upper_angle - lower_angle)/(laser_beam_DockedwithMar))))* pi/180;
+    X_DockedwithMar(i) = s_val_DockedwithMar(i) * cos(s_angle_DockedwithMar(i));
+    Y_DockedwithMar(i) = s_val_DockedwithMar(i) * sin(s_angle_DockedwithMar(i));
+    X_obst_DockedwithMar(i) = dist_to_obst * cos(s_angle_DockedwithMar(i));
+    Y_obst_DockedwithMar(i)= dist_to_obst * sin(s_angle_DockedwithMar(i));
+end
+
+
+for i = 1:points_DockedwithNOmar
+    s_angle_DockedwithNOmar(i) = (lower_angle + (indx_DockedwithNOmar(i)* ((upper_angle - lower_angle)/(laser_beam_DockedwithNOmar))))* pi/180;
+    X_DockedwithNOmar(i) = s_val_DockedwithNOmar(i) * cos(s_angle_DockedwithNOmar(i));
+    Y_DockedwithNOmar(i) = s_val_DockedwithNOmar(i) * sin(s_angle_DockedwithNOmar(i));
+    X_obst_DockedwithNOmar(i) = dist_to_obst * cos(s_angle_DockedwithNOmar(i));
+    Y_obst_DockedwithNOmar(i)= dist_to_obst * sin(s_angle_DockedwithNOmar(i));
+end
+
+
+
+% The robot is totally docked and a box is besides of laser scanner!
+S_Mat_DockedwithBOX = csvread('scanner_data_box.txt',0,0);
+indx_DockedwithBOX = S_Mat_DockedwithBOX(:,1);
+s_val_DockedwithBOX = S_Mat_DockedwithBOX(:,2);
+
+points_DockedwithBOX = size(indx_DockedwithBOX,1);
+laser_beam_DockedwithBOX = points_DockedwithBOX - 1;
+s_angle_DockedwithBOX = zeros(points_DockedwithBOX,1);
+
+X_DockedwithBOX = zeros(points_DockedwithBOX,1); Y_DockedwithBOX = zeros(points_DockedwithBOX,1);
+X_obst_DockedwithBOX = zeros(points_DockedwithBOX,1); Y_obst_DockedwithBOX = zeros(points_DockedwithBOX,1);
+
+for i = 1:points_DockedwithBOX
+    s_angle_DockedwithBOX(i) = (lower_angle + (indx_DockedwithBOX(i)* ((upper_angle - lower_angle)/(laser_beam_DockedwithBOX))))* pi/180;
+    X_DockedwithBOX(i) = s_val_DockedwithBOX(i) * cos(s_angle_DockedwithBOX(i));
+    Y_DockedwithBOX(i) = s_val_DockedwithBOX(i) * sin(s_angle_DockedwithBOX(i));
+    
+    
+    X_obst_DockedwithBOX(i) = dist_to_obst * cos(s_angle_DockedwithBOX(i));
+    Y_obst_DockedwithBOX(i)= dist_to_obst * sin(s_angle_DockedwithBOX(i));
+end
+
+
+% checking accuracy in robot coordinate system
+figure;
+set(gcf,'color','white');
+
+subplot(3,2,1);
+plot(s_angle_DockedwithNOmar,s_val_DockedwithNOmar);
+hold on
+plot(s_angle_DockedwithNOmar,dist_to_obst,'r','LineWidth',.2);
+% title('No marker on docking platform');
+axis([-2.2 2.2 -.5 10]);
+set(gca,'box','off','fontsize',20);
+
+subplot(3,2,2);
+plot(X_DockedwithNOmar,Y_DockedwithNOmar);
+hold on;
+plot(X_obst_DockedwithNOmar,Y_obst_DockedwithNOmar,'r','LineWidth',3);
+% title('No marker on docking platform, TOP VIEW');
+% legend('Obstacles','Laser scanner safety curve (r = 10 cm)');
+% grid on
+axis([-2 4.5 -4 1.5]);
+set(gca,'box','off','fontsize',20);
+
+
+subplot(3,2,3);
+orig_coordinate_cir = gca; % create axes
+plot(orig_coordinate_cir,s_angle_DockedwithMar,s_val_DockedwithMar);
+hold on
+plot(orig_coordinate_cir,s_angle_DockedwithMar,dist_to_obst,'r','LineWidth',.2);
+ylabel('$ {Distance} $ [m]','interpreter','latex','FontSize',14);
+axis([-2.3 2.3 -.5 10]);
+set(orig_coordinate_cir,'box','off','fontsize',20);
+
+% ----- Zoomed in ------
+hkids_cir = get(orig_coordinate_cir, 'child'); % get original coordinate properties
+
+set(hkids_cir(1),'marker','o','Color','r')
+set(hkids_cir(2),'marker','.')
+
+zoomed_coordinate_cir = axes('Position',get(orig_coordinate_cir,'Position'),...
+           'XAxisLocation','bottom',...
+           'YAxisLocation','left',...
+           'fontsize',15,...
+           'Color','none',...
+           'XColor','k','YColor','k', 'NextPlot', 'add'); % make ax2. Use nexplot to maintain your settings
+
+plot(zoomed_coordinate_cir,s_angle_DockedwithMar,s_val_DockedwithMar);
+hold on;
+plot(zoomed_coordinate_cir,s_angle_DockedwithMar,dist_to_obst,'Color','r');
+
+set(zoomed_coordinate_cir,'xlim',[-.15 .32])% axes limit for zoom out,
+set(zoomed_coordinate_cir,'ylim',[.08 .2])% axes limit for zoom out,
+set(zoomed_coordinate_cir, 'Units', 'normalized', 'Position', [.35 0.48 0.15 0.18])
+% ----- Zoomed in ------
+
+subplot(3,2,4);
+orig_coordinate_cir_T = gca; % create axes
+plot(orig_coordinate_cir_T,X_DockedwithMar,Y_DockedwithMar);
+hold on;
+plot(orig_coordinate_cir_T,X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',3);
+ylabel('$ {Y} $ [m]','interpreter','latex','FontSize',14);
+axis([-2 4.5 -4 1.5]);
+set(orig_coordinate_cir_T,'box','off','fontsize',20);
+
+% ----- Zoomed in ------
+hkids_cir_T = get(orig_coordinate_cir_T, 'child'); % get original coordinate properties
+% 
+% set(hkids_cir_T(1),'marker','.','Color','r')
+% set(hkids_cir_T(2),'marker','*')
+
+set(hkids_cir_T(1),'Color','r')
+set(hkids_cir_T(2),'Color','b')
+
+
+zoomed_coordinate_cir_T = axes('Position',get(orig_coordinate_cir_T,'Position'),...
+           'XAxisLocation','bottom',...
+           'YAxisLocation','left',...
+           'fontsize',13,...
+           'Color','none',...
+           'XColor','k','YColor','k', 'NextPlot', 'add'); % make ax2. Use nexplot to maintain your settings
+
+plot(zoomed_coordinate_cir_T,X_DockedwithMar,Y_DockedwithMar);
+hold on;
+plot(zoomed_coordinate_cir_T,X_obst_DockedwithMar,Y_obst_DockedwithMar,'Color','r');
+
+set(zoomed_coordinate_cir_T,'xlim',[.08 .13])% axes limit for zoom out,
+set(zoomed_coordinate_cir_T,'ylim',[-.06 .06])% axes limit for zoom out,
+set(zoomed_coordinate_cir_T, 'Units', 'normalized', 'Position', [.84 0.485 0.15 0.18])
+% ----- Zoomed in ------
+
+
+
+subplot(3,2,5);
+orig_coordinate_box = gca; % create axes
+plot(orig_coordinate_box,s_angle_DockedwithBOX,s_val_DockedwithBOX,'b','LineWidth',.01);
+hold on
+plot(orig_coordinate_box,s_angle_DockedwithBOX,dist_to_obst, 'r','LineWidth',5);
+xlabel('$ {\theta_b} $ [rad]','interpreter','latex','FontSize',14);
+axis([-2.3 2.3 -.15 5]);
+set(orig_coordinate_box,'box','off','fontsize',20);
+
+% ----- Zoomed in BOX------
+hkids_box = get(orig_coordinate_box, 'child'); % get original coordinate properties
+
+set(hkids_box(1),'marker','o','Color','r')
+set(hkids_box(2),'marker','.')
+
+zoomed_coordinate_box = axes('Position',get(orig_coordinate_box,'Position'),...
+           'XAxisLocation','bottom',...
+           'YAxisLocation','left',...
+           'fontsize',13,...
+           'Color','none',...
+           'XColor','k','YColor','k', 'NextPlot', 'add'); % make ax2. Use nexplot to maintain your settings
+
+plot(zoomed_coordinate_box,s_angle_DockedwithBOX,s_val_DockedwithBOX);
+hold on;
+plot(zoomed_coordinate_box,s_angle_DockedwithBOX,dist_to_obst,'Color','r');
+
+set(zoomed_coordinate_box,'xlim',[-1.6 -.3])% axes limit for zoom out,
+set(zoomed_coordinate_box,'ylim',[.08 .2])% axes limit for zoom out,
+set(zoomed_coordinate_box, 'Units', 'normalized', 'Position', [.35 0.18 0.15 0.18])
+% ----- Zoomed in BOX------
+
+
+subplot(3,2,6);
+orig_coordinate_box_T = gca; % create axes
+plot(orig_coordinate_box_T,X_DockedwithBOX,Y_DockedwithBOX,'b','LineWidth',.01);
+hold on
+plot(orig_coordinate_box_T,X_obst_DockedwithMar,Y_obst_DockedwithMar, 'r','LineWidth',5);
+xlabel('$ {X} $ [m]','interpreter','latex','FontSize',14);
+axis([-.53 .9 -.8 1.5]);
+set(orig_coordinate_box_T,'box','off','fontsize',20);
+
+% ----- Zoomed in BOX_T------
+hkids_box_T = get(orig_coordinate_box_T, 'child'); % get original coordinate properties
+
+set(hkids_box_T(1),'Color','r')
+set(hkids_box_T(2),'Color','b')
+
+zoomed_coordinate_box_T = axes('Position',get(orig_coordinate_box_T,'Position'),...
+           'XAxisLocation','bottom',...
+           'YAxisLocation','left',...
+           'fontsize',13,...
+           'Color','none',...
+           'XColor','k','YColor','k', 'NextPlot', 'add'); % make ax2. Use nexplot to maintain your settings
+
+plot(zoomed_coordinate_box_T,X_DockedwithBOX,Y_DockedwithBOX);
+hold on;
+plot(zoomed_coordinate_box_T,X_obst_DockedwithMar,Y_obst_DockedwithMar,'Color','r');
+
+set(zoomed_coordinate_box_T,'xlim',[-.16 .2])% axes limit for zoom out,
+set(zoomed_coordinate_box_T,'ylim',[-.22 .22])% axes limit for zoom out,
+set(zoomed_coordinate_box_T, 'Units', 'normalized', 'Position', [.84 0.2 .15 .18])
+% ----- Zoomed in BOX_T------
+
+
+%% The robot is moving Towards docking...!
+Scanner_Matrix_Moving = csvread('scanner_data_Moving_Towards_Docking_WITH_Marker.txt',0,0);
+
+index_Moving = Scanner_Matrix_Moving(:,1);
+scanner_value_Moving = Scanner_Matrix_Moving(:,2);
+
+index_Moving_Final = Scanner_Matrix_Moving(size(Scanner_Matrix_Moving,1),1);
+scanner_value_Moving_Final = Scanner_Matrix_Moving(size(Scanner_Matrix_Moving,1),2);
+
+% figure;
+% plot(index_Moving,scanner_value_Moving,'m',index_Moving_Final,scanner_value_Moving_Final,'k*');
+% legend('Distance to obstacle','Reference Position');
+% grid on;
+% xlabel('Index');
+% ylabel('Distance to Obstacle [m]');
+% title('Scanner Analysis for a moving robot!');
+% axis([0 540 0 31]);
+
+
+%% The robot is in the docking area, while NOT docked!
+S_Mat_Out = csvread('scanner_data_outside.txt',0,0);
+indx_out = S_Mat_Out(:,1);
+s_val_out = S_Mat_Out(:,2);
+
+points_out = size(indx_out,1);
+laser_beam_out = points_out - 1;
+s_angle_out = zeros(points_out,1);
+X_out = zeros(points_out,1); Y_out = zeros(points_out,1);
+X_obst_out = zeros(points_out,1); Y_obst_out = zeros(points_out,1);
+
+for i = 1:points_out
+    s_angle_out(i) = (lower_angle + (indx_out(i)* ((upper_angle - lower_angle)/(laser_beam_out))))* pi/180;
+    X_out(i) = s_val_out(i) * cos(s_angle_out(i));
+    Y_out(i) = s_val_out(i) * sin(s_angle_out(i));
+    
+    X_obst_out(i) = dist_to_obst * cos(s_angle_out(i));
+    Y_obst_out(i)= dist_to_obst * sin(s_angle_out(i));
+end
+
+% figure;
+% set(gcf,'color','white');
+% subplot(1,2,1);
+% plot(s_angle_out,s_val_out,'b','LineWidth',2);
+% hold on
+% plot(s_angle_out,dist_to_obst,'r','LineWidth',10);
+% xlabel('${\theta_b}$ [rad]','interpreter','latex','FontSize',14);
+% ylabel('$ {Distance} $ [m]','interpreter','latex','FontSize',14);
+% grid on;
+% % axis([-2.23 2.23 -1 10]);
+% 
+% subplot(1,2,2)
+% plot(X_out,Y_out,'b','LineWidth',2);
+% hold on
+% plot(X_obst_out,Y_obst_out,'r-','LineWidth',4)
+% xlabel('$X$ [m] ','interpreter','latex','FontSize',14);
+% ylabel('$Y$ [m]','interpreter','latex','FontSize',14);
+% 
+% l2 = legend('${Obstacles}$', '${Safety Margin (r = 10 cm)}$');
+% set(l2,'interpreter','latex','FontSize',12);
+% % legend('boxoff');
+% grid on;
+% % axis([-1.2 5 -3.5 1.5]);
+
+
+
+% figure;
+% plot(X_DockedwithMar,Y_DockedwithMar,'b');
+% hold on;
+% plot(X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',.3);
+% xlabel('${X}$ [m]','interpreter','latex','FontSize',14);
+% % ylabel('$ Y [m] $','interpreter','latex','FontSize',14);
+% % grid on
+% title('Detected marker (zoomed area from middle plot), TOP VIEW');
+% axis([.04 .16 -.12 .12]);
+
+
+
diff --git a/Files_4_thesis/scanner_analysis.m b/Files_4_thesis/scanner_analysis.m
index 025b70470ae2b206f7b73bff3ac4ac7beff576bc..b2f3199c3686a10d7051192ac295e6e49a4a5c28 100644
--- a/Files_4_thesis/scanner_analysis.m
+++ b/Files_4_thesis/scanner_analysis.m
@@ -78,7 +78,7 @@ title('');
 xlabel('${\theta_b}$ [rad]','interpreter','latex','FontSize',20);
 ylabel('${Distance}$ [m]','interpreter','latex','FontSize',20);
 axis([-.09 .42 .08 .2]);
-set(gca,'box','off','fontsize',20);
+% set(gca,'box','off','fontsize',20);
 % legend('Experiment 1','Experiment 2','Experiment 3','Safety Curve');
 subplot(1,2,2);
 % plot(X_exp_1,Y_exp_1,'m--','LineWidth',.9);
@@ -96,10 +96,10 @@ title('');
 xlabel('${X}$ [m]','interpreter','latex','FontSize',20);
 ylabel('${Y}$ [m]','interpreter','latex','FontSize',20);
 axis([-.1 .16 -.18 .18]);
-set(gca,'box','off','fontsize',20);
+% set(gca,'box','off','fontsize',20);
 l = legend('Trial 1','Trial 2','Trial 3','Safety Curve','Orientation','horizontal');
-set(l,'interpreter','latex','FontSize',13);
-
+set(l,'box','off','interpreter','latex','FontSize',13);
+% legend('box','off')
 %% docked robot with marker vs. docked robot without marker!
 S_Mat_DockedwithMar = csvread('scanner_data_Docked_WITH_Marker.txt',0,0);
 S_Mat_DockedwithNOmar = csvread('scanner_data_no_cylinder.txt',0,0);
@@ -263,7 +263,7 @@ subplot(3,2,5);
 orig_coordinate_box = gca; % create axes
 plot(orig_coordinate_box,s_angle_DockedwithBOX,s_val_DockedwithBOX,'b','LineWidth',.01);
 hold on
-plot(orig_coordinate_box,s_angle_DockedwithBOX,dist_to_obst, 'r','LineWidth',5);
+plot(orig_coordinate_box,s_angle_DockedwithBOX,dist_to_obst, 'r','LineWidth',3);
 xlabel('$ {\theta_b} $ [rad]','interpreter','latex','FontSize',14);
 axis([-2.3 2.3 -.15 5]);
 set(orig_coordinate_box,'box','off','fontsize',20);
diff --git a/Paper/ifacconf_office/Laser_Scanner_Table.docx b/Paper/ifacconf_office/Laser_Scanner_Table.docx
index b404624af078ab524a54e11b79d2ccf3a4d4fb56..1c4ff4c68be4fcd487422e4762341bd433360adb 100644
Binary files a/Paper/ifacconf_office/Laser_Scanner_Table.docx and b/Paper/ifacconf_office/Laser_Scanner_Table.docx differ
diff --git a/Paper/ifacconf_office/Paper.docx b/Paper/ifacconf_office/Paper.docx
index b2a50e03f1e2ac3e82a519876cb6e3dae27b3878..a13b0a392904764faa9ec17f8518892bad8e1818 100644
Binary files a/Paper/ifacconf_office/Paper.docx and b/Paper/ifacconf_office/Paper.docx differ
diff --git a/Paper/ifacconf_office/Paper.pdf b/Paper/ifacconf_office/Paper.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..741e57992371973f29b42a7f155523601cf99909
Binary files /dev/null and b/Paper/ifacconf_office/Paper.pdf differ