diff --git a/Files_4_thesis/Different_angle/docking_different_angle.m b/Files_4_thesis/Different_angle/docking_different_angle.m
index 70d6c89bd026272c67de3edd6535f7de30652a7a..2d7c590b88c9d9cfdde0f0ecfb6b34b48275c314 100644
--- a/Files_4_thesis/Different_angle/docking_different_angle.m
+++ b/Files_4_thesis/Different_angle/docking_different_angle.m
@@ -155,13 +155,9 @@ 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.6);
-
-title('Camera Pose Estimation');
-ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',14);
-
-l2 = legend('Left', 'Right');
-set(l2,'interpreter','latex','FontSize',11);
-% legend('boxoff');
+ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',20);
+title('a) ${x_{mar}}$','interpreter','latex','FontSize',20);
+set(gca,'fontsize',20);
subplot(323);
plot(t_P_sec_data_1,Pose_Y_data_1,'LineWidth',3);
@@ -169,7 +165,9 @@ 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);
-ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',14);
+ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',20);
+title('b) ${y_{mar}}$','interpreter','latex','FontSize',20);
+set(gca,'fontsize',20);
subplot(325);
plot(t_P_sec_data_1,Theta_data_1,'LineWidth',3);
@@ -178,8 +176,10 @@ 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);
-xlabel('$ time$ [s]','interpreter','latex','FontSize',14);
-ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',14);
+title('c) ${\theta_{mar}}$','interpreter','latex','FontSize',20);
+ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',20);
+xlabel('$ time$ [s]','interpreter','latex','FontSize',20);
+set(gca,'fontsize',20);
% % control signals
% figure;
@@ -193,11 +193,14 @@ 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);
-title('Control Signals');
-ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);
+title('d) $\dot{x}_{Rob}$','interpreter','latex','FontSize',20);
+ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
+
l2 = legend('${Left}$', '${Right}$');
-set(l2,'interpreter','latex','FontSize',11);
+set(l2,'interpreter','latex','FontSize',14);
legend('boxoff')
+axis ([0 40 -.01 .18]);
+set(gca,'fontsize',20);
subplot(324);
plot(t_V_sec_data_1,Vel_Y_data_1,'LineWidth',3);
@@ -205,8 +208,9 @@ 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.6);
-ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);
-
+ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
+title('e) $\dot{y}_{Rob}$','interpreter','latex','FontSize',20);
+set(gca,'fontsize',20);
subplot(326);
plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',3);
@@ -214,15 +218,17 @@ 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);
-ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14);
+xlabel('$ time$ [s]','interpreter','latex','FontSize',20);
+ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',20);
+title('f) $\dot{\theta}_{Rob}$','interpreter','latex','FontSize',20);
+set(gca,'fontsize',20);
% Trajectory
figure;
set(gcf,'color','white');
-subplot(1,3,1);
+subplot(131);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
% hold on
% quiver(Pose_Y_data_1,Pose_X_data_1,gradient(Pose_Y_data_1),gradient(Pose_X_data_1),'r--','LineWidth',.01);
@@ -231,20 +237,21 @@ hold on
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
% hold on
% quiver(Pose_Y_data_2,Pose_X_data_2,gradient(Pose_Y_data_2),gradient(Pose_X_data_2),'m--','LineWidth',.01);
-%
-hold on
-plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
+% %
+% hold on
+% plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
% hold on
% quiver(Pose_Y_data_3,Pose_X_data_3,gradient(Pose_Y_data_3),gradient(Pose_X_data_3),'k--','LineWidth',.01);
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([-.4 .4 .18 1.4]);
+title('a) Approach zone + SM zone + Target area','FontSize',17);
+ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',20);
+set(gca,'fontsize',20);
-subplot(1,3,2);
+subplot(132);
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);
@@ -253,11 +260,12 @@ plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',1.6);
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);
-title('SM zone + Target area');
-xlabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',14);
+title('b) SM zone + Target area','FontSize',17);
+xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',20);
axis([-.03 0.03 -.29 -.191]);
+set(gca,'fontsize',20);
-subplot(1,3,3);
+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);
@@ -266,11 +274,13 @@ plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);
-title('Target area');
-axis([-.0205 -.0168 -.2054 -.199]);
-l = legend('${Left}$', '${Right}$','Target');
-set(l,'interpreter','latex','FontSize',11);
+title('c) Target area','FontSize',17);
+axis([-.0202 -.017 -.2054 -.199]);
+set(gca,'fontsize',20);
+l = legend('${Left}$', '${Right}$','Target');
+set(l,'interpreter','latex','FontSize',14);
+legend('boxoff');
MaxOvershoot_1 = -ref_y + min(Pose_Y_data_1)
diff --git a/Files_4_thesis/Report_with_updated_plots.docx b/Files_4_thesis/Report_with_updated_plots.docx
index 19f92ef6436568fab66f24e1a847ffd4808dd552..74c8fa4b2efff8087f12356f633ffeff101c478f 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/x_tANDy_t/y_tANDx_t.asv b/Files_4_thesis/x_tANDy_t/y_tANDx_t.asv
new file mode 100644
index 0000000000000000000000000000000000000000..013bcea40873570cbe8302ee269f0cdb45a8afe5
--- /dev/null
+++ b/Files_4_thesis/x_tANDy_t/y_tANDx_t.asv
@@ -0,0 +1,431 @@
+clc;
+close all;
+clear all;
+
+%% Import Data
+
+% data 1:
+Pose_Matrix_data_1 = csvread('CAM_PID_.266_.0003_.014.txt',1,0);
+Vel_Matrix_data_1 = csvread('vel_PID_.266_.0003_.014.txt',1,0);
+Raw_meas_1 = csvread('Trans_PID_.266_.0003_.014.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 RAW MEASUREMENT
+T_Raw_ros_1 = Raw_meas_1(:,1); % ros time, needs to be converted to sec...
+
+% duriation = end_time - start_time
+ duRAW_1 = (T_Raw_ros_1(size(T_Raw_ros_1,1),:) - T_Raw_ros_1(1,:))*10^(-9);
+
+t_Raw_ave_1 = 0:duRAW_1/size(T_Raw_ros_1,1):duRAW_1;
+t_Raw_ave_1(:,size(t_Raw_ave_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:
+
+Pose_Matrix_data_2 = csvread('CAM_PID_.28_0_.006.txt',1,0);
+Vel_Matrix_data_2 = csvread('vel_PID_.28_0_.006.txt',1,0);
+RAW_meas_2 = csvread('Trans_PID_.28_0_.006.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 RAW MEASUREMEMT
+T_RAW_ros_2 = RAW_meas_2(:,1); % ros time, needs to be converted to sec...
+
+% duriation = end_time - start_time
+ duRAW_2 = (T_RAW_ros_2(size(T_RAW_ros_2,1),:) - T_RAW_ros_2(1,:))*10^(-9);
+
+t_RAW_ave_2 = 0:duRAW_2/size(T_RAW_ros_2,1):duRAW_2;
+t_RAW_ave_2(:,size(t_RAW_ave_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('CAM_PID_.29_.00026_.015.txt',1,0);
+Vel_Matrix_data_3 = csvread('vel_PID_.29_.00026_.015.txt',1,0);
+RAW_meas_3 = csvread('Trans_PID_.29_.00026_.015.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 RAW MEASUREMENT
+T_RAW_ros_3 = RAW_meas_3(:,1); % ros time, needs to be converted to sec...
+
+% duriation = end_time - start_time
+ duRaw_3 = (T_RAW_ros_3(size(T_RAW_ros_3,1),:) - T_RAW_ros_3(1,:))*10^(-9);
+
+t_Raw_real_3 = 0:duRaw_3/size(T_RAW_ros_3,1):duRaw_3;
+t_Raw_real_3(:,size(t_Raw_real_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:
+Pose_Matrix_data_4 = csvread('CAM_PID_.3_.0004_.02.txt',1,0);
+Vel_Matrix_data_4 = csvread('vel_PID_.3_.0004_.02.txt',1,0);
+Raw_Meas_4 = csvread('Trans_PID_.3_.0004_.02.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 RAW_MEASUREMENT
+T_Raw_ros_4 = Raw_Meas_4(:,1); % ros time, needs to be converted to sec...
+
+% duriation = end_time - start_time
+ duRaw_4 = (T_Raw_ros_4(size(T_Raw_ros_4,1),:) - T_Raw_ros_4(1,:))*10^(-9);
+
+t_Raw_real_4 = 0:duRaw_4/size(T_Raw_ros_4,1):duRaw_4;
+t_Raw_real_4(:,size(t_Raw_real_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)) = [];
+
+%% Pose estimation
+
+%data 1:
+x_raw_1 = Raw_meas_1(:,4); % x_t
+y_raw_1 = Raw_meas_1(:,3); % y_t
+theta_raw_1 = Raw_meas_1(:,7); % yaw
+
+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);
+
+%data 2:
+x_raw_2 = RAW_meas_2(:,4); % x_t
+y_raw_2 = RAW_meas_2(:,3); % y_t
+theta_raw_2 = RAW_meas_2(:,7); % yaw
+
+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);
+
+%data 3:
+x_raw_3 = RAW_meas_3(:,4); % x_t
+y_raw_3 = RAW_meas_3(:,3); % y_t
+theta_raw_3 = RAW_meas_3(:,7); % yaw
+
+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);
+
+
+%data 4:
+x_raw_4 = Raw_Meas_4(:,4); % x_t
+y_raw_4 = Raw_Meas_4(:,3); % y_t
+theta_raw_4 = Raw_Meas_4(:,7); % yaw
+
+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);
+
+
+% Reference Circle
+thresh_X = .0013;
+theta = 0:.001:2*pi;
+
+% needs to be adjusted manually if docking platform is replaced!
+ref_x = -.2025;
+ref_y = -.0185;
+
+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);
+%% Plot
+
+
+figure;
+set(gcf,'color','white');
+
+% raw measurement
+subplot(331);
+
+plot(t_Raw_ave_1,x_raw_1,'b','LineWidth',1.5);
+hold on;
+plot(t_RAW_ave_2,x_raw_2,'k','LineWidth',1.5);
+hold on;
+plot(t_Raw_real_3,x_raw_3,'g','LineWidth',1.5);
+hold on;
+plot(t_Raw_real_4,x_raw_4,'r','LineWidth',1.5);
+ylabel('x_{raw}');
+title('Raw Measurement');
+
+
+subplot(334);
+
+plot(t_Raw_ave_1,y_raw_1,'b','LineWidth',1.5);
+hold on;
+plot(t_RAW_ave_2,y_raw_2,'k','LineWidth',1.5);
+hold on;
+plot(t_Raw_real_3,y_raw_3,'g','LineWidth',1.5);
+hold on;
+plot(t_Raw_real_4,y_raw_4,'r','LineWidth',1.5);
+
+ylabel('y_{raw}');
+
+subplot(337);
+
+plot(t_Raw_ave_1,theta_raw_1,'b','LineWidth',1.5);
+hold on;
+plot(t_RAW_ave_2,theta_raw_2,'k','LineWidth',1.5);
+hold on;
+plot(t_Raw_real_3,theta_raw_3,'g','LineWidth',1.5);
+hold on;
+plot(t_Raw_real_4,theta_raw_4,'r','LineWidth',1.5);
+xlabel('time');
+ylabel('\theta_{raw}');
+
+
+
+% Camera Pose Estimation
+subplot(332);
+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,'k','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,'r','LineWidth',1.3);
+% set(gca,'fontsize',20);
+ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',20);
+title('a) ${x_{mar}}$','interpreter','latex','FontSize',20);
+%
+% l2 = legend('${P_y = 0.266, I_y = 0.0003, D_y = 0.014}$', '${P_y = 0.28, I_y = 0, D_y = 0.006}$','${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.3, I_y = 0.0004, D_y = 0.02}$');
+% set(l2,'interpreter','latex','FontSize',9);
+% legend('boxoff');
+
+subplot(335);
+plot(t_P_sec_data_1,Pose_Y_data_1,'LineWidth',1.8);
+hold on
+plot(t_P_sec_data_2,Pose_Y_data_2,'k','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_4,Pose_Y_data_4,'r','LineWidth',1.4);
+% set(gca,'fontsize',20);
+ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',20);
+title('b) ${y_{mar}}$','interpreter','latex','FontSize',20);
+
+subplot(338);
+plot(t_P_sec_data_1,Theta_data_1,'LineWidth',1.8);
+hold on
+plot(t_P_sec_data_2,Theta_data_2,'k','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_4,Theta_data_4,'r','LineWidth',1.4);
+% set(gca,'fontsize',20);
+title('c) ${\theta_{mar}}$','interpreter','latex','FontSize',20);
+xlabel('$ time [s]$','interpreter','latex','FontSize',20);
+ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',20);
+
+
+% Control Signals
+
+% % control signals
+% figure;
+% set(gcf,'color','white');
+
+subplot(333);
+plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',1.8);
+hold on
+plot(t_V_sec_data_2,Vel_X_data_2,'k','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_4,Vel_X_data_4,'r','LineWidth',1.4);
+% set(gca,'fontsize',20);
+title('d) $\dot{x}_{Rob}$','interpreter','latex','FontSize',20);
+ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
+%
+% l2 = legend('${P_y = 0.266, I_y = 0.0003, D_y = 0.014}$', '${P_y = 0.28, I_y = 0, D_y = 0.006}$','${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.3, I_y = 0.0004, D_y = 0.02}$');
+% set(l2,'interpreter','latex','FontSize',13);
+% legend('boxoff');
+axis([0 60 -.01 .17]);
+
+subplot(336);
+plot(t_V_sec_data_1,Vel_Y_data_1,'LineWidth',1.8);
+hold on
+plot(t_V_sec_data_2,Vel_Y_data_2,'k','LineWidth',1.7);
+hold on
+plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',1.6);
+hold on
+plot(t_V_sec_data_4,Vel_Y_data_4,'r','LineWidth',1.4);
+% set(gca,'fontsize',20);
+ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
+title('e) $\dot{y}_{Rob}$','interpreter','latex','FontSize',20);
+
+subplot(339);
+plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',1.8);
+hold on
+plot(t_V_sec_data_2,Omega_Z_data_2,'k','LineWidth',1.7);
+hold on
+plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',1.6);
+hold on
+plot(t_V_sec_data_4,Omega_Z_data_4,'r','LineWidth',1.4);
+% set(gca,'fontsize',20);
+title('f) $\dot{\theta}_{Rob}$','interpreter','latex','FontSize',20);
+xlabel('$ time [s]$','interpreter','latex','FontSize',20);
+ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',20);
+
+% Trajectory
+
+figure;
+set(gcf,'color','white');
+subplot(131);
+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,'k','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,'r','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,'m--','LineWidth',3.4);
+set(gca,'fontsize',20);
+title('a) Approach zone + SM zone + Target area','FontSize',17);
+ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',20);
+axis([-.65 .05 -1.6 -.19]);
+
+subplot(132);
+plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
+hold on
+plot(Pose_Y_data_2,Pose_X_data_2,'k','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,'r','LineWidth',2);
+hold on
+plot(y_circle,x_circle,'m--','LineWidth',3.4);
+set(gca,'fontsize',20);
+title('b) SM zone + Target area','FontSize',17);
+xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',20);
+axis([-.025 0.005 -.29 -.19]);
+
+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,'k','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,'r','LineWidth',2);
+hold on
+plot(y_circle,x_circle,'m--','LineWidth',3.4);
+set(gca,'fontsize',20);
+title('c) Target area','FontSize',17);
+axis([-.0205 -.0168 -.2054 -.199]);
+l = legend('${P_y = .266, I_y = .0003, D_y = .014}$', '${P_y = .28, I_y = 0, D_y = .006}$','${P_y = .29, I_y = .00026, D_y = .015}$','${P_y = .3, I_y = .0004, D_y = .02}$','${Target}$');
+set(l,'interpreter','latex','FontSize',12);
+
+
+
+
+
+% no transformation
+figure;
+set(gcf,'color','white');
+plot(y_raw_1,x_raw_1,'b','LineWidth',2);
+hold on;
+plot(y_raw_2,x_raw_2,'k','LineWidth',2);
+hold on;
+plot(y_raw_3,x_raw_3,'g','LineWidth',2);
+hold on;
+plot(y_raw_4,x_raw_4,'r','LineWidth',2);
+
+
+xlabel('y_t');
+ylabel('x_t');
+% axis([-.1 .1 .19 1.55]);
diff --git a/Files_4_thesis/x_tANDy_t/y_tANDx_t.m b/Files_4_thesis/x_tANDy_t/y_tANDx_t.m
index f1459f2bb153b0e51354da81d73f583ed48cedc2..013bcea40873570cbe8302ee269f0cdb45a8afe5 100644
--- a/Files_4_thesis/x_tANDy_t/y_tANDx_t.m
+++ b/Files_4_thesis/x_tANDy_t/y_tANDx_t.m
@@ -267,7 +267,6 @@ 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,'r','LineWidth',1.3);
% set(gca,'fontsize',20);
-title('Camera Pose Estimation');
ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',20);
title('a) ${x_{mar}}$','interpreter','latex','FontSize',20);
%
diff --git a/Paper/ifacconf_office/Paper.docx b/Paper/ifacconf_office/Paper.docx
index 17182caa0eb3f799ca14d5c710610d0f08e6220e..655efb6c0f74ca01cdef3853493dd18890c70ca2 100644
Binary files a/Paper/ifacconf_office/Paper.docx and b/Paper/ifacconf_office/Paper.docx differ