diff --git a/Files_4_thesis/Different_angle/docking_different_angle.m b/Files_4_thesis/Different_angle/docking_different_angle.m
index fb30237c89782a2ade95b4d32dc3313202662a6d..6375e94ced8ae1b33ebde921986b2cddea2802c6 100644
--- a/Files_4_thesis/Different_angle/docking_different_angle.m
+++ b/Files_4_thesis/Different_angle/docking_different_angle.m
@@ -167,18 +167,18 @@ subplot(3,1,2);
 plot(t_P_sec_data_1,Pose_Y_data_1,'LineWidth',3);
 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.6);
+% hold on
+% plot(t_P_sec_data_3,Pose_Y_data_3,'g','LineWidth',1.6);
 ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',14);
 
 subplot(3,1,3);
 plot(t_P_sec_data_1,Theta_data_1,'LineWidth',3);
 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.6);
+% hold on
+% plot(t_P_sec_data_3,Theta_data_3,'g','LineWidth',1.6);
 
-xlabel('$ time [s]$','interpreter','latex','FontSize',14);
+xlabel('$ time$ [s]','interpreter','latex','FontSize',14);
 ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',14);
 
 % control signals
@@ -190,8 +190,8 @@ subplot(311);
 plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',3);
 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.6);
+% hold on
+% plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',1.6);
 
 title('Control Signals');
 ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);
@@ -212,9 +212,9 @@ subplot(3,1,3);
 plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',3);
 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.6);
-xlabel('$ time [s]$','interpreter','latex','FontSize',14);
+% hold on
+% plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',1.6);
+xlabel('$ time$ [s]','interpreter','latex','FontSize',14);
 ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14);
 
 % Trajectory 
@@ -248,8 +248,8 @@ subplot(1,3,2);
 plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',1.6);
 hold on
 plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',1.6);
-hold on
-plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',1.6);
+% hold on
+% plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',1.6);
 hold on
 plot(y_circle,x_circle,'k--','LineWidth',3.4);
 
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 8880524b001dfefa3601ae028e759d91e3e7651e..0198c84553e4ea238fc5046296641acfae37956c 100644
--- a/Files_4_thesis/Different_controllers/docking_with_PID_controller.m
+++ b/Files_4_thesis/Different_controllers/docking_with_PID_controller.m
@@ -525,118 +525,118 @@ l = legend('${P_y = 0.28, I_y = 0.0001, D_y = 0.01}$', '${P_y = 0.29, I_y = 0.00
 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)
+
+% 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
-% 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);
+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;
-% plot (diff(t_V_sec_data_3),'k--','LineWidth',1.8);
-% % hold on;
-% title('Velocity');
-% xlabel('samples','interpreter','latex','FontSize',13);
+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);
+hold on
+pa2 = plot (diff(t_P_sec_data_3),'k--','LineWidth',1.5);
+hold on
+pa3 = plot(diff(TimeP_ros_data_5)/1e9,'g','LineWidth',1.4);
+hold on;
+pa4 = plot (diff(t_P_sec_data_5),'b--','LineWidth',1.8);
+hold off;
+axis([0 234 0 .3]);
+title('Position');
 % 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]);
 % 
+% l5 = legend([pa1 pa2],'$USB Camera {Time_{ROS}}$','$ USB Camera {Time_{Real}} [sec]$','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');
+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);
+% 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);
+
+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;
-% set(gcf,'color','white');
-% 
-% subplot(2,1,1);
-% pa1 = plot(diff(TimeP_ros_data_3)/1e9,'r','LineWidth',2.1);
-% hold on
-% pa2 = plot (diff(t_P_sec_data_3),'k--','LineWidth',1.5);
-% hold on
-% pa3 = plot(diff(TimeP_ros_data_5)/1e9,'g','LineWidth',1.4);
-% hold on;
-% pa4 = plot (diff(t_P_sec_data_5),'b--','LineWidth',1.8);
-% hold off;
-% axis([0 234 0 .3]);
-% title('Position');
-% % ylabel('Sampling time [sec]','interpreter','latex','FontSize',13);
-% % 
-% % l5 = legend([pa1 pa2],'$USB Camera {Time_{ROS}}$','$ USB Camera {Time_{Real}} [sec]$','Orientation','horizontal');
-% % set(l5,'interpreter','latex','FontSize',11);
+% subplot(3,1,1);
+% plot(diff(Pose_X_data_1));
+% title('X-Pose diff');
+% xlabel('samples');
+% ylabel('X difference [m]');
+% grid on
 % 
-% % leg = 
-% legend('ROS Time, USB camera','Real Time, USB camera [sec]','ROS Time, IP camera','Real Time, IP camera [sec]','Location','northwest');
-% 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);
-% % 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);
+% subplot(3,1,2);
+% plot(diff(Pose_X_data_1));
+% title('Y-Pose diff');
+% xlabel('samples');
+% ylabel('Y difference [m]');
+% grid on
 % 
-% 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
+% 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 5fcf78754af1775985773cb8259488356f2b573a..d9b541c7506fb48faad29953248766c33d5bdc6c 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
@@ -258,7 +258,7 @@ 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);
+xlabel('$ time $ [s]','interpreter','latex','FontSize',14);
 ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14);
 % grid on
 
diff --git a/Files_4_thesis/Laser Scanner Results/Scan_Data_5_exp.csv b/Files_4_thesis/Laser Scanner Results/Scan_Data_5_exp.csv
new file mode 100644
index 0000000000000000000000000000000000000000..e4bff4dedc4256e0a70302006bb66b2bfff36949
--- /dev/null
+++ b/Files_4_thesis/Laser Scanner Results/Scan_Data_5_exp.csv	
@@ -0,0 +1,535 @@
+0,0,0,0,0,0
+1,0,0,0,0,0
+2,0,0,0,0,0
+3,0,0,0,0,0
+4,0,0,0,0,0
+5,0,0,0,0,0
+6,0,0,0,0,0
+7,0,0,0,0,0
+8,0,0,0,0,0
+9,0,0,0,0,0
+10,0,0,0,0,0
+11,0,0,0,0,0
+12,3.93,3.93,3.93,3.93,3.93
+13,3.93,3.93,3.93,3.94,3.94
+14,3.95,3.94,3.93,3.93,3.94
+15,3.94,3.94,3.94,3.95,3.94
+16,3.87,3.87,3.86,3.86,3.87
+17,3.86,3.86,3.86,3.86,3.87
+18,3.87,3.87,3.87,3.86,3.87
+19,3.88,3.87,3.87,3.88,3.88
+20,3.89,3.88,3.88,3.88,3.89
+21,4.03,4.03,4.03,4.04,4.05
+22,4.49,4.49,4.48,4.49,4.49
+23,4.5,4.5,4.51,4.5,4.5
+24,4.51,4.5,4.5,4.51,4.51
+25,4.53,4.53,4.52,4.54,4.54
+26,29.96,29.96,29.96,29.96,29.96
+27,29.96,29.96,29.96,29.96,29.96
+28,3,3,2.99,2.99,3.01
+29,0.21,0.23,0.2,0.21,0.23
+30,0.25,0.24,0.26,0.25,0.25
+31,0.28,0.28,0.29,0.28,0.28
+32,0.29,0.29,0.29,0.3,0.3
+33,0.3,0.3,0.29,0.3,0.29
+34,0.29,0.29,0.29,0.3,0.29
+35,0.27,0.27,0.27,0.26,0.27
+36,0.24,0.23,0.23,0.24,0.23
+37,29.96,29.96,3.09,29.96,29.96
+38,3.1,3.11,3.1,3.12,3.11
+39,3.12,3.13,3.13,3.12,3.13
+40,3.15,3.13,3.15,3.13,3.14
+41,3.16,3.15,3.17,3.16,3.16
+42,29.96,29.96,29.96,29.96,29.96
+43,4,4,4.02,4.01,4
+44,3.98,3.97,3.98,3.97,3.99
+45,3.88,3.88,3.88,3.89,3.87
+46,3.79,3.79,3.78,3.78,3.79
+47,3.68,3.68,3.68,3.69,3.68
+48,3.59,3.59,3.59,3.59,3.59
+49,3.5,3.49,3.49,3.49,3.48
+50,3.5,3.5,3.49,3.49,3.49
+51,3.51,3.51,3.5,3.51,3.51
+52,1.68,1.68,1.68,1.69,1.68
+53,1.69,1.69,1.69,1.69,1.69
+54,29.96,29.96,29.96,3.52,29.96
+55,1.39,1.4,1.39,1.4,1.39
+56,1.36,1.36,1.36,1.36,1.36
+57,1.37,1.37,1.37,1.38,1.38
+58,3.62,3.62,3.62,3.62,3.63
+59,3.62,3.62,3.62,3.61,3.62
+60,3.64,3.64,3.63,3.63,3.64
+61,3.66,3.65,3.65,3.65,3.66
+62,3.67,3.67,3.66,3.67,3.67
+63,3.69,3.69,3.68,3.69,3.69
+64,3.7,3.71,3.71,3.71,3.71
+65,3.73,3.73,3.73,3.74,3.72
+66,3.74,3.74,3.74,3.74,3.74
+67,29.96,29.96,29.96,29.96,29.96
+68,29.96,29.96,29.96,29.96,29.96
+69,29.96,29.96,29.96,29.96,29.96
+70,4.94,4.92,4.91,4.93,4.93
+71,29.96,29.96,29.96,29.96,29.96
+72,4.92,4.93,4.94,4.93,4.93
+73,4.87,4.86,4.87,4.87,4.86
+74,4.7,4.7,4.71,4.71,4.7
+75,4.72,4.74,4.74,4.74,4.74
+76,4.84,4.83,4.83,4.83,4.84
+77,4.87,4.86,4.87,4.87,4.87
+78,4.63,4.63,4.63,4.62,4.61
+79,4.43,4.43,4.43,4.43,4.43
+80,4.34,4.33,4.34,4.33,4.34
+81,4.34,4.35,4.33,4.34,4.34
+82,4.36,4.35,4.36,4.36,4.35
+83,4.37,4.37,4.37,4.37,4.37
+84,4.39,4.39,4.39,4.39,4.4
+85,4.41,4.41,4.41,4.41,4.41
+86,4.42,4.43,4.42,4.42,4.42
+87,4.43,4.45,4.44,4.44,4.44
+88,4.47,4.46,4.45,4.47,4.47
+89,4.48,4.48,4.48,4.48,4.48
+90,9.26,9.26,9.25,9.24,9.25
+91,9.3,9.31,9.31,9.32,9.31
+92,9.37,9.37,9.39,9.38,9.39
+93,9.48,9.49,9.48,9.48,9.49
+94,2.52,2.54,2.53,2.53,2.53
+95,2.52,2.53,2.51,2.53,2.53
+96,2,1.99,1.99,1.99,1.99
+97,1.97,1.96,1.97,1.96,1.96
+98,2,1.99,1.99,1.98,2
+99,29.96,29.96,29.96,29.96,29.96
+100,3.01,29.96,29.96,29.96,29.96
+101,2.29,2.28,2.3,2.28,2.29
+102,2.29,2.27,2.29,2.28,2.28
+103,29.96,29.96,29.96,29.96,29.96
+104,0.6,0.61,0.6,0.61,0.61
+105,0.63,0.62,0.62,0.62,0.63
+106,0.69,0.69,0.69,0.69,0.7
+107,1.68,1.67,1.67,1.67,1.66
+108,1.73,1.72,1.72,1.72,1.72
+109,2.25,2.25,2.25,2.26,2.25
+110,2.29,2.28,2.28,2.28,2.29
+111,2.29,2.28,2.29,2.29,2.29
+112,2.39,2.38,2.4,2.39,2.4
+113,2.42,2.43,2.43,2.42,2.43
+114,2.4,2.41,2.39,2.41,2.4
+115,2.39,2.38,2.38,2.39,2.38
+116,2.36,2.35,2.35,2.35,2.36
+117,2.33,2.33,2.33,2.33,2.33
+118,2.31,2.31,2.3,2.31,2.31
+119,2.29,2.29,2.29,2.3,2.29
+120,2.27,2.27,2.27,2.26,2.27
+121,2.26,2.25,2.24,2.24,2.24
+122,1.2,1.17,1.17,1.17,1.16
+123,0.82,0.82,0.82,0.83,0.82
+124,0.84,0.85,0.85,0.86,0.86
+125,1.36,1.35,1.33,1.35,1.35
+126,1.01,1.01,1.03,1.03,1.02
+127,1,1.02,1.01,1.02,0.99
+128,1.02,1.01,1.03,1.02,1.03
+129,1.72,1.8,1.8,1.77,1.77
+130,1.32,1.23,1.24,1.29,1.25
+131,0.72,0.72,0.73,0.72,0.72
+132,0.72,0.72,0.72,0.73,0.72
+133,0.72,0.72,0.71,0.72,0.72
+134,0.72,0.72,0.72,0.72,0.72
+135,0.73,0.72,0.72,0.75,0.73
+136,1.09,1.1,1.08,1.06,1.1
+137,1.97,1.96,1.97,1.97,1.97
+138,1.95,1.95,1.96,1.95,1.96
+139,1.94,1.95,1.94,1.94,1.94
+140,1.93,1.91,1.92,1.92,1.91
+141,1.91,1.91,1.9,1.91,1.91
+142,1.9,1.89,1.91,1.9,1.91
+143,1.89,1.88,1.89,1.89,1.88
+144,1.88,1.88,1.88,1.87,1.88
+145,1.87,1.86,1.86,1.86,1.87
+146,1.85,1.85,1.84,1.85,1.85
+147,1.84,1.83,1.83,1.85,1.84
+148,1.83,1.83,1.82,1.82,1.83
+149,1.82,1.81,1.82,1.82,1.82
+150,1.8,1.8,1.8,1.8,1.8
+151,1.79,1.8,1.79,1.8,1.79
+152,1.79,1.79,1.78,1.78,1.78
+153,1.78,1.77,1.78,1.77,1.78
+154,1.77,1.76,1.76,1.76,1.76
+155,1.76,1.76,1.76,1.76,1.76
+156,1.75,1.74,1.74,1.74,1.74
+157,1.75,1.72,1.72,1.73,1.73
+158,1.74,1.75,1.75,1.75,1.74
+159,1.75,1.74,1.74,1.74,1.75
+160,1.74,1.74,1.74,1.75,1.74
+161,1.78,1.78,1.77,1.78,1.77
+162,1.8,1.81,1.8,1.81,1.8
+163,1.84,1.84,1.84,1.84,1.84
+164,1.88,1.87,1.87,1.87,1.88
+165,1.91,1.91,1.91,1.91,1.92
+166,1.94,1.94,1.95,1.96,1.94
+167,1.99,1.99,1.98,1.98,1.99
+168,2.03,2.03,2.04,2.03,2.03
+169,2.07,2.07,2.07,2.07,2.07
+170,2.11,2.12,2.11,2.12,2.11
+171,2.16,2.16,2.16,2.16,2.16
+172,2.21,2.2,2.21,2.21,2.21
+173,2.26,2.26,2.26,2.26,2.26
+174,2.31,2.31,2.31,2.32,2.32
+175,2.37,2.38,2.36,2.37,2.36
+176,2.39,2.39,2.4,2.4,2.39
+177,6.42,6.43,6.42,6.43,6.43
+178,6.22,6.21,6.22,6.22,6.22
+179,6.21,6.21,6.21,6.23,6.22
+180,4.51,4.5,4.51,4.51,4.5
+181,6.31,6.29,6.3,6.29,6.28
+182,6.24,6.23,6.25,6.25,6.23
+183,4.02,4.02,4.03,4.03,4.03
+184,4.03,4.02,4.02,4.04,4.02
+185,29.96,29.96,29.96,29.96,29.96
+186,4.7,4.71,4.71,4.7,4.71
+187,6.33,6.33,6.33,6.33,6.34
+188,6.28,6.28,6.28,6.28,6.29
+189,6.36,6.35,6.32,6.34,6.34
+190,1.37,1.38,1.38,1.39,1.39
+191,1.36,1.35,1.35,1.35,1.35
+192,1.33,1.33,1.32,1.32,1.33
+193,0.53,0.55,0.73,0.53,0.59
+194,0.48,0.47,0.49,0.48,0.48
+195,0.46,0.46,0.47,0.46,0.47
+196,0.44,0.44,0.44,0.45,0.44
+197,0.45,0.44,0.44,0.45,0.44
+198,0.45,0.44,0.44,0.43,0.44
+199,0.43,0.44,0.45,0.45,0.45
+200,0.44,0.45,0.44,0.45,0.44
+201,0.45,0.44,0.44,0.45,0.45
+202,0.45,0.45,0.45,0.45,0.45
+203,0.45,0.44,0.44,0.45,0.45
+204,0.45,0.45,0.45,0.46,0.45
+205,0.45,0.46,0.45,0.45,0.45
+206,0.45,0.45,0.45,0.45,0.45
+207,0.46,0.45,0.45,0.46,0.45
+208,0.45,0.45,0.45,0.46,0.46
+209,0.46,0.45,0.46,0.46,0.45
+210,0.45,0.46,0.45,0.46,0.45
+211,0.46,0.46,0.46,0.45,0.46
+212,0.47,0.46,0.46,0.46,0.46
+213,0.46,0.47,0.46,0.46,0.46
+214,0.46,0.47,0.46,0.45,0.46
+215,0.46,0.46,0.46,0.46,0.47
+216,0.46,0.46,0.47,0.46,0.46
+217,0.46,0.46,0.45,0.46,0.46
+218,0.46,0.47,0.46,0.47,0.47
+219,0.47,0.47,0.46,0.47,0.47
+220,0.48,0.46,0.46,0.47,0.47
+221,0.47,0.47,0.47,0.46,0.47
+222,0.47,0.47,0.47,0.47,0.47
+223,0.46,0.47,0.47,0.47,0.47
+224,0.47,0.47,0.47,0.47,0.46
+225,0.47,0.47,0.47,0.47,0.47
+226,0.48,0.48,0.47,0.47,0.48
+227,0.45,0.46,0.45,0.45,0.45
+228,0.45,0.44,0.45,0.45,0.44
+229,0.39,0.39,0.4,0.4,0.4
+230,0.36,0.36,0.36,0.36,0.36
+231,0.36,0.36,0.36,0.35,0.35
+232,0.35,0.36,0.36,0.37,0.36
+233,0.45,0.45,0.46,0.46,0.46
+234,0.5,0.5,0.5,0.5,0.5
+235,0.5,0.51,0.5,0.5,0.51
+236,0.49,0.51,0.5,0.5,0.5
+237,0.51,0.5,0.5,0.5,0.51
+238,0.51,0.5,0.5,0.51,0.51
+239,0.51,0.51,0.51,0.51,0.51
+240,0.51,0.51,0.51,0.5,0.51
+241,0.52,0.51,0.52,0.51,0.52
+242,0.51,0.52,0.51,0.53,0.53
+243,0.53,0.52,0.52,0.52,0.52
+244,0.52,0.52,0.53,0.52,0.52
+245,0.52,0.53,0.52,0.53,0.53
+246,0.53,0.53,0.53,0.53,0.52
+247,0.53,0.52,0.53,0.53,0.53
+248,0.54,0.54,0.54,0.54,0.54
+249,0.55,0.54,0.55,0.54,0.56
+250,0.55,0.56,0.56,0.55,0.55
+251,0.55,0.56,0.55,0.56,0.56
+252,0.57,0.57,0.55,0.56,0.56
+253,0.53,0.53,0.53,0.54,0.54
+254,0.51,0.51,0.51,0.51,0.5
+255,0.54,0.54,0.54,0.55,0.55
+256,0.54,0.53,0.55,0.53,0.54
+257,0.46,0.47,0.46,0.47,0.46
+258,0.45,0.44,0.45,0.43,0.44
+259,0.39,0.39,0.39,0.39,0.39
+260,0.13,0.13,0.11,0.12,0.12
+261,0.13,0.11,0.11,0.12,0.12
+262,0.12,0.12,0.12,0.12,0.13
+263,0.12,0.13,0.13,0.13,0.12
+264,0.11,0.12,0.12,0.13,0.12
+265,0.11,0.11,0.12,0.11,0.12
+266,0.13,0.11,0.11,0.12,0.11
+267,0.12,0.12,0.11,0.12,0.11
+268,0.12,0.12,0.11,0.12,0.11
+269,0.13,0.11,0.11,0.11,0.11
+270,0.11,0.12,0.11,0.11,0.12
+271,0.11,0.11,0.12,0.11,0.12
+272,0.11,0.11,0.11,0.11,0.11
+273,0.11,0.1,0.1,0.11,0.11
+274,0.11,0.11,0.11,0.11,0.11
+275,0.11,0.1,0.1,0.12,0.11
+276,0.11,0.1,0.1,0.11,0.1
+277,0.11,0.11,0.11,0.11,0.11
+278,0.1,0.1,0.1,0.1,0.1
+279,0.1,0.1,0.1,0.1,0.09
+280,0.09,0.09,0.09,0.1,0.1
+281,0.1,0.11,0.09,0.1,0.1
+282,0.11,0.1,0.1,0.1,0.1
+283,0.1,0.1,0.1,0.1,0.1
+284,0.1,0.09,0.1,0.1,0.1
+285,0.1,0.1,0.09,0.1,0.1
+286,0.1,0.1,0.1,0.09,0.1
+287,0.1,0.11,0.09,0.1,0.1
+288,0.1,0.1,0.1,0.1,0.1
+289,0.1,0.1,0.1,0.09,0.1
+290,0.11,0.11,0.11,0.12,0.11
+291,0.11,0.11,0.1,0.11,0.11
+292,0.11,0.12,0.1,0.11,0.12
+293,0.11,0.11,0.11,0.12,0.11
+294,0.11,0.11,0.11,0.11,0.11
+295,0.11,0.12,0.11,0.1,0.11
+296,0.11,0.11,0.11,0.1,0.11
+297,0.11,0.11,0.11,0.1,0.11
+298,0.11,0.11,0.1,0.12,0.11
+299,0.12,0.12,0.12,0.11,0.1
+300,0.1,0.1,0.1,0.11,0.11
+301,0.09,0.11,0.11,0.1,0.1
+302,0.1,0.1,0.11,0.12,0.11
+303,0.11,0.11,0.11,0.11,0.1
+304,0.12,0.1,0.12,0.11,0.11
+305,0.1,0.12,0.11,0.12,0.11
+306,0.12,0.11,0.12,0.12,0.11
+307,0.11,0.11,0.11,0.11,0.11
+308,0.12,0.12,0.13,0.12,0.13
+309,0.11,0.12,0.12,0.13,0.12
+310,0.14,0.14,0.13,0.13,0.12
+311,0.17,0.17,0.18,0.18,0.19
+312,0.39,0.39,0.39,0.39,0.39
+313,0.44,0.44,0.44,0.45,0.46
+314,0.46,0.46,0.45,0.47,0.47
+315,0.47,0.46,0.46,0.46,0.47
+316,0.47,0.47,0.47,0.46,0.46
+317,0.47,0.47,0.47,0.46,0.48
+318,0.46,0.47,0.47,0.48,0.46
+319,0.47,0.46,0.47,0.47,0.47
+320,0.47,0.47,0.47,0.46,0.48
+321,0.47,0.47,0.47,0.47,0.47
+322,0.47,0.46,0.46,0.46,0.47
+323,0.47,0.46,0.46,0.46,0.46
+324,0.46,0.46,0.45,0.47,0.46
+325,0.46,0.46,0.46,0.46,0.47
+326,0.47,0.45,0.46,0.46,0.46
+327,0.44,0.45,0.45,0.45,0.46
+328,0.46,0.45,0.46,0.46,0.46
+329,0.46,0.45,0.46,0.47,0.46
+330,0.46,0.46,0.45,0.46,0.46
+331,0.46,0.46,0.45,0.46,0.46
+332,0.45,0.45,0.45,0.45,0.46
+333,0.46,0.45,0.45,0.45,0.45
+334,0.46,0.44,0.45,0.46,0.45
+335,0.46,0.45,0.45,0.46,0.44
+336,0.45,0.46,0.46,0.45,0.46
+337,0.45,0.45,0.46,0.46,0.45
+338,0.45,0.45,0.45,0.45,0.45
+339,0.44,0.45,0.45,0.44,0.46
+340,0.43,0.44,0.43,0.44,0.44
+341,0.44,0.45,0.43,0.45,0.44
+342,0.45,0.45,0.44,0.45,0.44
+343,0.45,0.45,0.45,0.45,0.46
+344,0.45,0.44,0.44,0.45,0.44
+345,0.44,0.45,0.45,0.45,0.44
+346,0.45,0.45,0.45,0.44,0.45
+347,0.44,0.45,0.45,0.45,0.45
+348,0.45,0.44,0.44,0.44,0.44
+349,0.44,0.44,0.44,0.44,0.44
+350,0.44,0.44,0.44,0.44,0.46
+351,0.44,0.44,0.46,0.45,0.45
+352,0.44,0.45,0.45,0.45,0.45
+353,0.45,0.45,0.45,0.45,0.44
+354,0.44,0.44,0.45,0.45,0.44
+355,0.45,0.44,0.44,0.45,0.44
+356,0.45,0.44,0.45,0.44,0.45
+357,0.44,0.45,0.45,0.44,0.45
+358,0.44,0.45,0.44,0.44,0.45
+359,0.44,0.44,0.45,0.45,0.44
+360,0.45,0.45,0.45,0.45,0.45
+361,0.45,0.45,0.45,0.45,0.45
+362,0.45,0.45,0.44,0.45,0.45
+363,0.44,0.44,0.45,0.44,0.46
+364,0.44,0.45,0.45,0.45,0.45
+365,0.44,0.45,0.44,0.44,0.44
+366,0.45,0.45,0.45,0.45,0.45
+367,0.45,0.45,0.45,0.45,0.45
+368,0.45,0.44,0.45,0.45,0.45
+369,0.45,0.45,0.44,0.45,0.45
+370,0.45,0.44,0.45,0.45,0.45
+371,0.44,0.46,0.45,0.45,0.46
+372,0.45,0.45,0.44,0.45,0.46
+373,0.46,0.45,0.45,0.46,0.46
+374,0.45,0.46,0.45,0.46,0.45
+375,0.46,0.46,0.45,0.46,0.45
+376,0.46,0.45,0.46,0.46,0.45
+377,0.45,0.46,0.45,0.46,0.45
+378,0.45,0.46,0.45,0.46,0.45
+379,0.45,0.45,0.45,0.45,0.46
+380,0.46,0.45,0.46,0.45,0.46
+381,0.45,0.46,0.45,0.46,0.46
+382,0.46,0.46,0.45,0.45,0.46
+383,0.46,0.46,0.46,0.46,0.46
+384,0.45,0.46,0.45,0.45,0.45
+385,0.45,0.46,0.45,0.46,0.46
+386,0.46,0.46,0.45,0.46,0.47
+387,0.46,0.46,0.45,0.46,0.46
+388,0.46,0.47,0.46,0.46,0.46
+389,0.47,0.47,0.46,0.47,0.46
+390,0.46,0.47,0.46,0.47,0.47
+391,0.46,0.47,0.47,0.46,0.47
+392,0.47,0.47,0.48,0.47,0.46
+393,0.47,0.47,0.46,0.47,0.47
+394,0.47,0.47,0.47,0.48,0.47
+395,0.47,0.46,0.47,0.47,0.46
+396,0.47,0.47,0.48,0.46,0.47
+397,0.46,0.47,0.46,0.47,0.47
+398,0.46,0.47,0.47,0.48,0.47
+399,0.46,0.47,0.47,0.47,0.48
+400,0.47,0.47,0.47,0.48,0.48
+401,0.48,0.47,0.48,0.49,0.48
+402,0.48,0.49,0.47,0.48,0.48
+403,0.48,0.49,0.49,0.47,0.48
+404,0.49,0.47,0.48,0.48,0.49
+405,0.48,0.48,0.47,0.48,0.48
+406,0.49,0.49,0.49,0.48,0.49
+407,0.48,0.48,0.49,0.48,0.48
+408,0.48,0.49,0.48,0.5,0.49
+409,0.49,0.48,0.48,0.49,0.49
+410,0.48,0.5,0.49,0.49,0.49
+411,0.49,0.49,0.49,0.5,0.49
+412,0.49,0.49,0.5,0.49,0.5
+413,0.5,0.49,0.5,0.51,0.5
+414,0.5,0.5,0.5,0.51,0.51
+415,0.5,0.5,0.51,0.51,0.5
+416,0.51,0.5,0.5,0.51,0.51
+417,0.51,0.51,0.51,0.51,0.51
+418,0.51,0.51,0.51,0.51,0.51
+419,0.52,0.52,0.51,0.52,0.51
+420,0.51,0.52,0.52,0.52,0.52
+421,0.52,0.53,0.51,0.52,0.52
+422,0.52,0.52,0.52,0.53,0.53
+423,0.53,0.53,0.53,0.53,0.52
+424,0.53,0.53,0.53,0.53,0.53
+425,0.54,0.54,0.54,0.53,0.53
+426,0.54,0.54,0.54,0.53,0.53
+427,0.54,0.54,0.53,0.53,0.53
+428,0.54,0.53,0.54,0.55,0.54
+429,0.55,0.53,0.55,0.53,0.54
+430,0.54,0.54,0.54,0.54,0.55
+431,0.54,0.54,0.55,0.55,0.54
+432,0.55,0.55,0.54,0.55,0.55
+433,0.55,0.56,0.55,0.55,0.55
+434,0.55,0.56,0.56,0.55,0.55
+435,0.55,0.56,0.56,0.55,0.56
+436,0.56,0.56,0.57,0.56,0.56
+437,0.57,0.56,0.56,0.57,0.57
+438,0.56,0.57,0.57,0.56,0.57
+439,0.58,0.57,0.57,0.57,0.58
+440,0.58,0.58,0.57,0.59,0.58
+441,0.57,0.58,0.58,0.59,0.58
+442,0.58,0.58,0.58,0.59,0.58
+443,0.59,0.58,0.59,0.6,0.59
+444,0.59,0.59,0.59,0.6,0.59
+445,0.6,0.58,0.6,0.61,0.59
+446,0.61,0.6,0.6,0.59,0.61
+447,0.6,0.59,0.59,0.6,0.6
+448,0.61,0.6,0.6,0.6,0.6
+449,0.6,0.6,0.6,0.6,0.6
+450,0.61,0.62,0.61,0.6,0.62
+451,0.62,0.61,0.61,0.62,0.62
+452,0.62,0.62,0.63,0.63,0.64
+453,0.63,0.63,0.63,0.63,0.64
+454,0.64,0.64,0.63,0.65,0.64
+455,0.64,0.65,0.65,0.66,0.65
+456,0.67,0.66,0.66,0.67,0.66
+457,0.68,0.67,0.67,0.67,0.67
+458,0.68,0.67,0.68,0.68,0.68
+459,0.67,0.7,0.68,0.68,0.69
+460,0.7,0.69,0.69,0.7,0.7
+461,0.7,0.7,0.7,0.7,0.71
+462,0.72,0.71,0.71,0.71,0.72
+463,0.71,0.72,0.72,0.72,0.73
+464,0.72,0.73,0.71,0.74,0.73
+465,0.74,0.75,0.73,0.76,0.75
+466,0.76,0.75,0.75,0.76,0.75
+467,0.76,0.76,0.76,0.77,0.76
+468,0.78,0.78,0.78,0.79,0.79
+469,0.8,0.77,0.76,0.8,0.79
+470,0.8,0.8,0.81,0.8,0.8
+471,0.81,0.82,0.81,0.81,0.8
+472,0.82,0.82,0.82,0.82,0.82
+473,0.84,0.82,0.84,0.83,0.84
+474,0.86,0.85,0.84,0.85,0.86
+475,0.86,0.87,0.86,0.86,0.88
+476,0.88,0.88,0.88,0.88,0.88
+477,0.91,0.89,0.9,0.89,0.9
+478,0.91,0.92,0.92,0.92,0.91
+479,0.93,0.92,0.93,0.94,0.93
+480,0.95,0.94,0.95,0.95,0.96
+481,0.97,0.97,0.97,0.98,0.98
+482,1,0.99,0.99,1,0.99
+483,1.01,1,1,1,1.01
+484,0.99,0.99,0.99,1,1.01
+485,1,0.99,1.01,1.01,1
+486,0.99,1,0.99,1,1
+487,0.99,0.98,0.99,0.99,1
+488,0.98,0.97,0.98,0.99,1
+489,0.97,0.98,0.99,0.97,0.97
+490,0.97,0.97,0.96,0.97,0.98
+491,0.97,0.97,0.97,0.97,0.97
+492,0.97,0.96,0.96,0.97,0.98
+493,0.96,0.96,0.95,0.95,0.96
+494,0.9,0.9,0.9,0.89,0.9
+495,0.56,0.55,0.54,0.56,0.55
+496,0.51,0.53,0.52,0.51,0.52
+497,0.54,0.53,0.54,0.54,0.54
+498,0.68,0.67,0.68,0.68,0.67
+499,0.91,0.92,0.91,0.9,0.92
+500,0.93,0.93,0.93,0.93,0.91
+501,0.93,0.91,0.94,0.93,0.93
+502,0.87,0.87,0.88,0.88,0.88
+503,0.61,0.6,0.6,0.62,0.61
+504,0.63,0.62,0.62,0.62,0.63
+505,0.88,0.88,0.87,0.85,0.87
+506,0.9,0.91,0.9,0.91,0.91
+507,0.91,0.91,0.91,0.92,0.91
+508,0.88,0.89,0.89,0.9,0.9
+509,0.89,0.9,0.88,0.88,0.9
+510,0.89,0.89,0.89,0.89,0.9
+511,0.81,0.81,0.81,0.82,0.82
+512,0.7,0.7,0.7,0.71,0.7
+513,0.62,0.62,0.61,0.63,0.64
+514,0.75,0.75,0.77,0.77,0.75
+515,0.8,0.79,0.78,0.8,0.78
+516,0.72,0.71,0.7,0.72,0.73
+517,0.7,0.73,0.71,0.72,0.72
+518,0.72,0.73,0.73,0.71,0.72
+519,0.78,0.78,0.78,0.79,0.79
+520,0.8,0.81,0.81,0.8,0.81
+521,0.81,0.79,0.81,0.81,0.81
+522,0,0,0,0,0
+523,0,0,0,0,0
+524,0,0,0,0,0
+525,0,0,0,0,0
+526,0,0,0,0,0
+527,0,0,0,0,0
+528,0,0,0,0,0
+529,0,0,0,0,0
+530,0,0,0,0,0
+531,0,0,0,0,0
+532,0,0,0,0,0
+533,0,0,0,0,0
+534,0,0,0,0,0
diff --git a/Files_4_thesis/Laser Scanner Results/Scan_Data_5_exp.xlsx b/Files_4_thesis/Laser Scanner Results/Scan_Data_5_exp.xlsx
new file mode 100644
index 0000000000000000000000000000000000000000..8dabb74a4cfdf859dfb43e9461d4f9fe104c4670
Binary files /dev/null and b/Files_4_thesis/Laser Scanner Results/Scan_Data_5_exp.xlsx differ
diff --git a/Files_4_thesis/Report_with_updated_plots.docx b/Files_4_thesis/Report_with_updated_plots.docx
index dcf8a178e94e8b68ea38127dae849f02c0910f6d..a90e8304de62558dfb9d43e9c107fee3e21ef6b7 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/scanner_analysis.m b/Files_4_thesis/scanner_analysis.m
new file mode 100644
index 0000000000000000000000000000000000000000..f1ac8d0e79ff301f670ae8ddf892937838356692
--- /dev/null
+++ b/Files_4_thesis/scanner_analysis.m
@@ -0,0 +1,323 @@
+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('${Angle}$ [rad]','interpreter','latex','FontSize',14);
+ylabel('${Distance [m]}$','interpreter','latex','FontSize',14);
+axis([-.09 .42 .08 .2]);
+% 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',14);
+ylabel('${Y}$ [m]','interpreter','latex','FontSize',14);
+axis([-.1 .16 -.18 .18]);
+l = legend('Experiment 1','Experiment 2','Experiment 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
+
+% checking accuracy in robot coordinate system
+figure;
+set(gcf,'color','white');
+
+subplot(3,1,1);
+plot(X_DockedwithNOmar,Y_DockedwithNOmar);
+hold on;
+plot(X_obst_DockedwithNOmar,Y_obst_DockedwithNOmar,'r','LineWidth',3);
+% xlabel('$ X [m] $','interpreter','latex','FontSize',14);
+% ylabel('$ Y [m] $','interpreter','latex','FontSize',14);
+title('No marker on docking platform');
+legend('Obstacles','Laser scanner safety curve (r = 10 cm)');
+% grid on
+axis([-2.5 4 -4 1]);
+
+subplot(3,1,2);
+plot(X_DockedwithMar,Y_DockedwithMar,'b');
+hold on;
+plot(X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',3);
+ylabel('$ Y [m] $','interpreter','latex','FontSize',14);
+title('Marker �circular bottle� on docking platform');
+% grid on
+axis([-2.5 4 -4 1]);
+
+subplot(3,1,3);
+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)');
+axis([.04 .16 -.12 .12]);
+
+figure;
+set(gcf,'color','white');
+
+subplot(3,1,1);
+plot(s_angle_DockedwithNOmar,s_val_DockedwithNOmar);
+hold on
+plot(s_angle_DockedwithNOmar,dist_to_obst,'r','LineWidth',.2);
+% xlabel('Angle [rad]');
+% ylabel('Distance to Obstacle [m]');
+title('No marker in docking platform');
+% 
+% l2 = legend('${Obstacles}$', '${Safety Margin (r = 10 cm)}$');
+% set(l2,'interpreter','latex','FontSize',12);
+
+axis([-2.2 2.2 -.5 5]);
+
+
+subplot(3,1,2);
+plot(s_angle_DockedwithMar,s_val_DockedwithMar);
+hold on
+plot(s_angle_DockedwithMar,dist_to_obst,'r','LineWidth',.2);
+% xlabel('Angle [rad]');
+ylabel('Distance to objects [m]');
+title('Marker �circular bottle� on docking platform');
+axis([-2.2 2.2 -.5 5]);
+
+subplot(3,1,3);
+plot(s_angle_DockedwithMar,s_val_DockedwithMar);
+hold on
+plot(s_angle_DockedwithMar,dist_to_obst,'r','LineWidth',10);
+title('Detected marker (zoomed area from middle plot)');
+xlabel('Angle [rad]');
+% ylabel('Distance to Obstacle [m]');
+axis([-.15 .35 .08 .15]);
+
+%% 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('$Angle [rad]$','interpreter','latex','FontSize',14);
+ylabel('$Obstacle [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);
+
+grid on;
+axis([-1.2 5 -3.5 1.5]);
+
+
+%% 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
+
+figure;
+set(gcf,'color','white');
+subplot(2,1,1);
+plot(s_angle_DockedwithBOX,s_val_DockedwithBOX,'b','LineWidth',.01);
+hold on
+plot(s_angle_DockedwithBOX,dist_to_obst, 'r','LineWidth',5);
+% xlabel('Angle [rad]');
+% ylabel('Distance to Obstacle [m]');
+title('Box on docking platform');
+legend('Surrounding objects','Laser scanner safety line (r = 10 cm)');
+axis([-2.2 2.2 -.5 5]);
+
+subplot(2,1,2);
+plot(s_angle_DockedwithBOX,s_val_DockedwithBOX,'b','LineWidth',.01);
+hold on
+plot(s_angle_DockedwithBOX,dist_to_obst, 'r','LineWidth',5);
+% xlabel('Angle [rad]');
+% ylabel('Distance to Obstacle [m]');
+axis([-1.9 0 .08 .22]);
+title('Detected marker (zoomed in area from the first plot)');
+
+
+% 
+% figure;
+% plot(X_DockedwithBOX,Y_DockedwithBOX);
\ No newline at end of file
diff --git a/MobileRobot/rviz_main_folder/docking_marker/sixDOFdockingMarker/src/user_marker.cpp b/MobileRobot/rviz_main_folder/docking_marker/sixDOFdockingMarker/src/user_marker.cpp
index f49fc0a40ae6e355933c44c1c0683a3a6e7c3f66..063df1d7cda64f8ffb641f4c4f65617031ababc0 100644
--- a/MobileRobot/rviz_main_folder/docking_marker/sixDOFdockingMarker/src/user_marker.cpp
+++ b/MobileRobot/rviz_main_folder/docking_marker/sixDOFdockingMarker/src/user_marker.cpp
@@ -7,7 +7,6 @@
 #include <math.h>
 
 
-using namespace visualization_msgs;
 
 uint32_t MarkerForDocking::counter = 0;