diff --git a/Files_4_thesis/Laser Scanner Results/Marker_pose_est.jpg b/Files_4_thesis/Laser Scanner Results/Marker_pose_est.jpg new file mode 100644 index 0000000000000000000000000000000000000000..0894c04037daa58c46a4d61c79fd48a3422fc066 Binary files /dev/null and b/Files_4_thesis/Laser Scanner Results/Marker_pose_est.jpg differ diff --git a/Files_4_thesis/Laser Scanner Results/Trajectory.jpg b/Files_4_thesis/Laser Scanner Results/Trajectory.jpg new file mode 100644 index 0000000000000000000000000000000000000000..d2ee2241d012babaae8fbbd92795bb9e75eed7d8 Binary files /dev/null and b/Files_4_thesis/Laser Scanner Results/Trajectory.jpg differ diff --git a/Files_4_thesis/Laser Scanner Results/control_signals.jpg b/Files_4_thesis/Laser Scanner Results/control_signals.jpg index ac6583f1bf96a5f39de666cb6a34a8d8160f3f2c..90441bd9f6c35b40fb0410d37b61b204dfeac13f 100644 Binary files a/Files_4_thesis/Laser Scanner Results/control_signals.jpg and b/Files_4_thesis/Laser Scanner Results/control_signals.jpg differ diff --git a/Files_4_thesis/Report.docx b/Files_4_thesis/Report.docx index a2886c4ee3724aed57dafb4162e522446d1db6b2..c0e2dadfb8b0f18f5bdbc154062c98a03cdb7516 100644 Binary files a/Files_4_thesis/Report.docx and b/Files_4_thesis/Report.docx differ diff --git a/Files_4_thesis/docking_1.m b/Files_4_thesis/docking_1.m index cbfac9869ee05bd1d0f07f38371faeb5a03fe4d5..cab4ad336b018a055528e225dad458857825c004 100644 --- a/Files_4_thesis/docking_1.m +++ b/Files_4_thesis/docking_1.m @@ -1,34 +1,85 @@ clc; close all; clear all; +%% Import data and time conversion -Pose_Matrix = csvread('Pose19_4_16_2159.txt',1,0); -Vel_Matrix = csvread('Velocity19_4_16_2159.txt',1,0); +% data 1: +Pose_Matrix_data_1 = csvread('Pose19_4_16_2159.txt',1,0); +Vel_Matrix_data_1 = csvread('Velocity19_4_16_2159.txt',1,0); % time in position -TimeP_ros = Pose_Matrix(:,1); % ros time, needs to be converted to sec... +TimeP_ros_data_1 = Pose_Matrix_data_1(:,1); % ros time, needs to be converted to sec... % duriation = end_time - start_time - durationP = (TimeP_ros(size(TimeP_ros,1),:) - TimeP_ros(1,:))*10^(-9); + durationP_data_1 = (TimeP_ros_data_1(size(TimeP_ros_data_1,1),:) - TimeP_ros_data_1(1,:))*10^(-9); -t_P_sec = 0:durationP/size(TimeP_ros,1):durationP; -t_P_sec(:,size(t_P_sec,2)) = []; +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 = Vel_Matrix(:,1); % ros time, needs to be converted to sec... +TimeV_ros_data_1 = Vel_Matrix_data_1(:,1); % ros time, needs to be converted to sec... % duriation = end_time - start_time - durationV = (TimeV_ros(size(TimeV_ros,1),:) - TimeV_ros(1,:))*10^(-9); + 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('Pose19_4_16_2040.txt',1,0); +Vel_Matrix_data_2 = csvread('Velocity19_4_16_2040.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('Pose19_4_16_2011.txt',1,0); +Vel_Matrix_data_3 = csvread('Velocity19_4_16_2011.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)) = []; -t_V_sec = 0:durationV/size(TimeV_ros,1):durationV; -t_V_sec(:,size(t_V_sec,2)) = []; %% Pose estimation + +%data 1: % when using marker pose ,,,, -Pose_X = Pose_Matrix(:,4); -Pose_Y = Pose_Matrix(:,3); -Theta = Pose_Matrix(:,5); +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(:,5); % when using robot odometry ,,,, % Pose_X = Pose_Matrix(:,2); @@ -36,22 +87,73 @@ Theta = Pose_Matrix(:,5); % Theta = Pose_Matrix(:,7); % Extracting reference values when the robot is manually docked! -ref_X = Pose_X(size(Pose_X,1)); -ref_Y = Pose_Y(size(Pose_Y,1)); -ref_Theta = Theta(size(Theta,1)); -ref_Pose =[ref_X;ref_Y]; +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(:,5); + +% when using robot odometry ,,,, +% Pose_X = Pose_Matrix(:,2); +% Pose_Y = Pose_Matrix(:,3); +% Theta = Pose_Matrix(:,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(:,5); + +% when using robot odometry ,,,, +% Pose_X = Pose_Matrix(:,2); +% Pose_Y = Pose_Matrix(:,3); +% Theta = Pose_Matrix(:,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; +x_ref = thresh_X*cos(theta) + ref_X_data_2; % ref_X needs to be recorded +y_ref = thresh_X*sin(theta) + ref_Y_data_2; % ref_Y needs to be recorded %% Velocity estimation -Vel_X = Vel_Matrix(:,2); -Vel_Y = Vel_Matrix(:,3); -Omega_Z = Vel_Matrix(:,7); + +% 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 -% for the presenation all pose estimation in one plot +% for the presenation all pose estimation(x,y,theta) in one plot + % figure; % plot(t_P_sec,Pose_X,'b',t_P_sec,Pose_Y,'r',t_P_sec,Theta,'g'); % @@ -64,20 +166,36 @@ Omega_Z = Vel_Matrix(:,7); figure; subplot(3,1,1); -plot(t_P_sec,Pose_X); +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'); -xlabel('Time [sec]'); +% xlabel('Time [sec]'); ylabel('X [m]'); grid on subplot(3,1,2); -plot(t_P_sec,Pose_Y); +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]'); +% xlabel('Time [sec]'); ylabel('Y [m]'); grid on + subplot(3,1,3); -plot(t_P_sec,Theta); +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 [sec]'); ylabel('\theta [rad]'); @@ -105,41 +223,91 @@ grid on figure; subplot(3,1,1); -plot(t_V_sec,Vel_X,'LineWidth',1.5); + +plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',1.2); +hold on +plot(t_V_sec_data_2,Vel_X_data_2,'r','LineWidth',1.2); +hold on +plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',1.2); + title('Control Signal X-axis'); % xlabel('Time [sec]'); ylabel('V_x [m/s]'); grid on subplot(3,1,2); -plot(t_V_sec,Vel_Y,'LineWidth',1.5); +plot(t_V_sec_data_1,Vel_Y_data_1,'LineWidth',1.2); +hold on +plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.2); +hold on +plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',1.2); + title('Control Signal Y-axis'); % xlabel('Time [sec]'); ylabel('V_y [m/s]'); grid on subplot(3,1,3); -plot(t_V_sec,Omega_Z,'LineWidth',1.5); +plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',1.2); +hold on +plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.2); +hold on +plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',1.2); + title('Control Signal \theta-axis'); xlabel('Time [sec]'); ylabel('\omega [rad/s]'); grid on +% 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*'); + figure; -plot(Pose_Y,Pose_X,'m',ref_Y,ref_X,'k*'); +subplot(1,2,1); +plot(Pose_Y_data_1,Pose_X_data_1,'b'); +hold on +plot(Pose_Y_data_2,Pose_X_data_2,'r'); +hold on +plot(Pose_Y_data_3,Pose_X_data_3,'g'); +hold on +plot(y_ref,x_ref,'k--','LineWidth',2) + title('Docking Trajectroy'); xlabel('Y [m]'); ylabel('X [m]'); % axis([-.15 .15 .2 1.5]); -legend('Trajectory','Reference Position'); +% legend('Trajectory 1','Reference Position 1', 'Trajectory 2','Reference Position 2','Trajectory 3','Reference Position 3'); +grid on + +subplot(1,2,2); +plot(Pose_Y_data_1,Pose_X_data_1,'b'); +hold on +plot(Pose_Y_data_2,Pose_X_data_2,'r'); +hold on +plot(Pose_Y_data_3,Pose_X_data_3,'g'); +hold on +plot(y_ref,x_ref,'k--','LineWidth',2) + +% title('Docking Trajectroy'); +xlabel('Y [m]'); +ylabel('X [m]'); +axis([-.1 .02 .26 .3]); grid on + + figure; subplot(3,1,1); -plot(diff(TimeP_ros)/1e9,'g') +plot(diff(TimeP_ros_data_1)/1e9,'g') hold on -plot (diff(t_P_sec),'k--') +plot (diff(t_P_sec_data_1),'k--') title('Position time step'); xlabel('samples'); ylabel('time step'); @@ -148,9 +316,9 @@ legend('ROS calc.','Ordinary calc.','Orientation','horizontal'); axis([0 428 .05 .15]); subplot(3,1,2); -plot(diff(TimeV_ros)/1e9,'g') +plot(diff(TimeV_ros_data_1)/1e9,'g') hold on -plot (diff(t_V_sec),'k--') +plot (diff(t_V_sec_data_1),'k--') title('Velocity time step'); xlabel('samples'); ylabel('time step'); @@ -159,34 +327,30 @@ ylabel('time step'); axis([0 428 -.005 .025]); subplot(3,1,3); -plot(diff(TimeV_ros)/1e9,'g') +plot(diff(TimeV_ros_data_1)/1e9,'g') hold on -plot (diff(t_V_sec),'k--') +plot (diff(t_V_sec_data_1),'k--') xlabel('samples'); ylabel('time step'); axis([150 250 0 .02]); - - - - figure; subplot(3,1,1); -plot(diff(Pose_X)); +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)); +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)); +plot(diff(Theta_data_1)); title('\theta-Pose diff'); xlabel('samples'); ylabel('\theta difference [rad]');