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 deleted file mode 100644 index a0c6227a8ce4e37e15df7c26ac530a0b1002aa05..0000000000000000000000000000000000000000 --- a/Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.asv +++ /dev/null @@ -1,398 +0,0 @@ -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}$','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',18); -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('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]); - -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('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; -% 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('${a) Approach zone + SM zone + Target area}$','interpreter','latex','FontSize',17); -% xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14); -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'); -set(l2,'interpreter','latex','FontSize',15); -legend('boxoff'); - -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,'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('$ {b) SM zone + Target area}$','interpreter','latex','FontSize',17); -xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',20); -% 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('$ {c) Target area}$','interpreter','latex','FontSize',17); -% 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/scanner_analysis.asv b/Files_4_thesis/scanner_analysis.asv deleted file mode 100644 index b6482ef3ac1dc44f57bf9a9ba33a5e0eeabf3362..0000000000000000000000000000000000000000 --- a/Files_4_thesis/scanner_analysis.asv +++ /dev/null @@ -1,407 +0,0 @@ -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','vertical'); -set(l,'interpreter','latex','FontSize',16); -% 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); - -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); -set(gca,'box','off'); - -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); -set(gca,'box','off'); - -subplot(3,2,3); -orig_coordinate_cir = gca; % create axes -plot(orig_coordinate_cir,s_angle_DockedwithMar,s_val_DockedwithMar,'b','LineWidth',2.7); -hold on -plot(orig_coordinate_cir,s_angle_DockedwithMar,dist_to_obst,'r','LineWidth',2.7); -ylabel('$ {Distance} $ [m]','interpreter','latex','FontSize',20); -axis([-2.3 2.3 -.5 10]); -% set(orig_coordinate_cir,'box','off','fontsize',20); -set(orig_coordinate_cir,'box','off'); - -% ----- 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',10,... - '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,'b','LineWidth',2.7); -hold on; -plot(zoomed_coordinate_cir,s_angle_DockedwithMar,dist_to_obst,'r-','LineWidth',2.7); - -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,'b','LineWidth',2.7); -hold on; -plot(orig_coordinate_cir_T,X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',3); -ylabel('$ {Y} $ [m]','interpreter','latex','FontSize',20); -axis([-2 4.5 -4 1.5]); -% set(orig_coordinate_cir_T,'box','off','fontsize',20); -set(orig_coordinate_cir_T,'box','off'); - -% ----- 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,'b','LineWidth',2.7); -hold on; -plot(zoomed_coordinate_cir_T,X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',3); - -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',2.7); -hold on -plot(orig_coordinate_box,s_angle_DockedwithBOX,dist_to_obst, 'r','LineWidth',3); -xlabel('$ {\theta_b} $ [rad]','interpreter','latex','FontSize',20); -axis([-2.3 2.3 -.15 5]); -% set(orig_coordinate_box,'box','off','fontsize',20); -set(orig_coordinate_box,'box','off'); - -% ----- 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',20); -axis([-.53 .9 -.8 1.5]); -% set(orig_coordinate_box_T,'box','off','fontsize',20); -set(orig_coordinate_box_T,'box','off'); - -% ----- 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 054182bd834821a80132574d7d65d0277e79ba90..cfa8e40e88a3d9cade2d43d3edbbde5042690868 100644 --- a/Files_4_thesis/scanner_analysis.m +++ b/Files_4_thesis/scanner_analysis.m @@ -73,7 +73,7 @@ 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); +plot(angle,dist_to_obst,'rx','LineWidth',1.05); title(''); xlabel('${\theta_b}$ [rad]','interpreter','latex','FontSize',20); ylabel('${Distance}$ [m]','interpreter','latex','FontSize',20); @@ -91,7 +91,7 @@ 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); +plot(X_obst,Y_obst,'r','LineWidth',2.05); title(''); xlabel('${X}$ [m]','interpreter','latex','FontSize',20); ylabel('${Y}$ [m]','interpreter','latex','FontSize',20); diff --git a/Files_4_thesis/x_tANDy_t/y_tANDx_t.asv b/Files_4_thesis/x_tANDy_t/y_tANDx_t.asv deleted file mode 100644 index ad84239387c699b6046535506a6d62727bee6519..0000000000000000000000000000000000000000 --- a/Files_4_thesis/x_tANDy_t/y_tANDx_t.asv +++ /dev/null @@ -1,437 +0,0 @@ -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}'); - -% subplot(332); -subplot(321); -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.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); -subplot(323); - -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,'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_4,Pose_Y_data_4,'m','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); -subplot(325); - -plot(t_P_sec_data_1,Theta_data_1,'LineWidth',1.8); -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_4,Theta_data_4,'m','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); -subplot(322); - -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,'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_4,Vel_X_data_4,'m','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); -subplot(324); - -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,'r','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,'m','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); -subplot(326); - -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,'r','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,'m','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,'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); -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,'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); -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,'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,'--','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/Paper/ifacconf_office/Paper.docx b/Paper/ifacconf_office/Paper.docx index 074e1b640b01a8f424c9551aab1fd4cebc5ee09d..807d5e6f948206fdde431fda29ae0c2c4b74cf63 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 index dca5ceeb053de8104fbb53a0127a2552b256a5a0..c2becad8029ba4523ff580129ba0d9c901faad5c 100644 Binary files a/Paper/ifacconf_office/Paper.pdf and b/Paper/ifacconf_office/Paper.pdf differ