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]');