diff --git a/Files_4_thesis/Report.docx b/Files_4_thesis/Report.docx
index df055d88950ebca25beb84e287d02c0472b98b65..19c00a41c78e47f6245ac99f701323b0a41e041d 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 a5fc7bb1c5926331a2c300528bed6d55825f59fc..81d1530d76954e5d77e0509db58cfb0703e62675 100644
--- a/Files_4_thesis/docking_1.m
+++ b/Files_4_thesis/docking_1.m
@@ -6,61 +6,128 @@ Pose_Matrix = csvread('Pose19_4_16_2159.txt',1,0);
 Vel_Matrix = csvread('Velocity19_4_16_2159.txt',1,0);
 
 % time in position
-TimeP = Pose_Matrix(:,1);
-durationP = (TimeP(size(TimeP,1),:) - TimeP(1,:))*10^(-9);
-t_P = 0:durationP/size(TimeP,1):durationP;
-t_P(:,size(t_P,2)) = [];
+TimeP_ros = Pose_Matrix(:,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); 
+  
+t_P_sec = 0:durationP/size(TimeP_ros,1):durationP;
+t_P_sec(:,size(t_P_sec,2)) = [];
 
 
 % time in vel
-TimeV = Vel_Matrix(:,1);
-durationV = (TimeV(size(TimeV,1),:) - TimeV(1,:))*10^(-9);
-t_V = 0:durationV/size(TimeV,1):durationV;
-t_V(:,size(t_V,2)) = [];
+TimeV_ros = Vel_Matrix(:,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);
 
+t_V_sec = 0:durationV/size(TimeV_ros,1):durationV;
+t_V_sec(:,size(t_V_sec,2)) = [];
+
+%% Pose estimation
 % when using marker pose ,,,,
 Pose_X = Pose_Matrix(:,4);
 Pose_Y = Pose_Matrix(:,3);
 Theta = Pose_Matrix(:,5);
 
-
 % when using robot odometry ,,,,
 % Pose_X = Pose_Matrix(:,2);
 % Pose_Y = Pose_Matrix(:,3);
 % Theta = Pose_Matrix(:,7);
 
-Docked_ref_X = Pose_X(size(Pose_X,1));
-Docked_ref_Y = Pose_Y(size(Pose_Y,1));
-Docked_ref_Theta = Theta(size(Theta,1));
+% 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];
 
-Docked_ref_Pose =[Docked_ref_X;Docked_ref_Y];
 
+
+%% Velocity estimation
 Vel_X = Vel_Matrix(:,2);
 Vel_Y = Vel_Matrix(:,3);
 Omega_Z = Vel_Matrix(:,7);
 
+
+%% Plots
+
+% for the presenation all pose estimation in one plot
+% figure;
+% plot(t_P_sec,Pose_X,'b',t_P_sec,Pose_Y,'r',t_P_sec,Theta,'g');
+% 
+% title('Marker Pose');
+% xlabel('Time [sec]');
+% ylabel('Pose');
+% legend('X [m]','Y [m]','\theta [rad]');
+% grid on
+
+
 figure;
-%plot(Time,Pose_X,'b--',Time,Pose_Y,'r',Time,Theta,'g');
-plot(t_P,Pose_X,'b--',t_P,Pose_Y,'r',t_P,Theta,'g');
+subplot(3,1,1);
+plot(t_P_sec,Pose_X);
+title('Marker X-axis');
+xlabel('Time [sec]');
+ylabel('X [m]');
+grid on
 
-title('Marker Pose');
-xlabel('Time [sec.]');
-ylabel('Pose');
-legend('X [m]','Y [m]','\theta [rad]');
+subplot(3,1,2);
+plot(t_P_sec,Pose_Y);
+title('Marker Y-axis');
+xlabel('Time [sec]');
+ylabel('Y [m]');
 grid on
+subplot(3,1,3);
+plot(t_P_sec,Theta);
+title('Marker \theta-axis');
+xlabel('Time [sec]');
+ylabel('\theta [rad]');
+grid on
+
+
+% figure;
+% plot(TimeP_ros,Pose_X,'b',TimeP_ros,Pose_Y,'r',TimeP_ros,Theta,'g');
+% 
+% title('Marker Pose ROS computation');
+% xlabel('Ros Time');
+% ylabel('Pose');
+% legend('X [m]','Y [m]','\theta [rad]');
+% grid on
+
+% for the presenation all control signals in one plot
+% figure;
+% plot(t_V_sec,Vel_X,'b',t_V_sec,Vel_Y,'r',t_V_sec,Omega_Z,'g');
+% 
+% title('Control Signal');
+% xlabel('Time [sec.]');
+% ylabel('Velocity');
+% legend('V_x [m/s]','V_y [m/s]','\omega [rad/s]');
+% grid on
 
 figure;
-% plot(Time,Vel_X,'b',Time,Vel_Y,'r',Time,Omega_Z,'g');
-plot(t_V,Vel_X,'b',t_V,Vel_Y,'r',t_V,Omega_Z,'g');
+subplot(3,1,1);
+plot(t_V_sec,Vel_X);
+title('Control Signal X-axis');
+xlabel('Time [sec]');
+ylabel('V_x [m/s]');
+grid on
 
-title('Control Signal');
-xlabel('Time [sec.]');
-ylabel('Velocity');
-legend('V_x [m/s]','V_y [m/s]','\omega [rad/s]');
+subplot(3,1,2);
+plot(t_V_sec,Vel_Y);
+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);
+title('Control Signal \theta-axis');
+xlabel('Time [sec]');
+ylabel('\omega [rad/s]');
 grid on
 
+
+
 figure;
-plot(Pose_Y,Pose_X,'m',Docked_ref_Y,Docked_ref_X,'k*');
+plot(Pose_Y,Pose_X,'m',ref_Y,ref_X,'k*');
 title('Docking Trajectroy');
 xlabel('Y [m]');
 ylabel('X [m]');
@@ -69,6 +136,45 @@ legend('Trajectory','Reference Position');
 grid on
 
 
-% figure;
-% plot(diff(TimeP)/1e9)
-% grid on
\ No newline at end of file
+figure;
+subplot(2,1,1);
+plot(diff(TimeP_ros)/1e9,'g')
+hold on
+plot (diff(t_P_sec),'k')
+title('Position time step');
+xlabel('samples');
+ylabel('time step');
+legend('ROS calc.','Ordinary calc.','Orientation','horizontal');
+% grid on
+
+subplot(2,1,2);
+plot(diff(TimeV_ros)/1e9,'g--')
+hold on
+plot (diff(t_V_sec),'k')
+title('Velocity time step');
+xlabel('samples');
+ylabel('time step');
+legend('ROS calc.','Ordinary calc.','Orientation','horizontal');
+% grid on
+
+figure;
+subplot(3,1,1);
+plot(diff(Pose_X));
+title('X-Pose diff');
+xlabel('samples');
+ylabel('X difference [m]');
+grid on
+
+subplot(3,1,2);
+plot(diff(Pose_X));
+title('Y-Pose diff');
+xlabel('samples');
+ylabel('Y difference [m]');
+grid on
+
+subplot(3,1,3);
+plot(diff(Theta));
+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.m b/Files_4_thesis/scanner_analysis.m
index 46e445114a6e3312d160e91c9c03af33e887130a..8d8d73998ac692ee2f99ef4dc096a057f2b267f8 100644
--- a/Files_4_thesis/scanner_analysis.m
+++ b/Files_4_thesis/scanner_analysis.m
@@ -2,20 +2,37 @@ clc;
 close all;
 clear all;
 
+upper_angle = 135; % deg.
+lower_angle = -135; % deg.
 
 %% The robot is totally docked and has a marker infront of laser scanner!
-Scanner_Matrix_DockedwithMarker = csvread('scanner_data_Docked_WITH_Marker.txt',0,0);
-index_DockedwithMarker = Scanner_Matrix_DockedwithMarker(:,1);
-scanner_value_DockedwithMarker = Scanner_Matrix_DockedwithMarker(:,2);
+S_Mat_DockedwithMar = csvread('scanner_data_Docked_WITH_Marker.txt',0,0);
+indx_DockedwithMar = S_Mat_DockedwithMar(:,1);
+s_val_DockedwithMar = S_Mat_DockedwithMar(:,2);
+
+
+points_DockedwithMar = size(indx_DockedwithMar,1);
+laser_beam_DockedwithMar = points_DockedwithMar - 1;
+s_angle_DockedwithMar = zeros(points_DockedwithMar,1);
+X_DockedwithMar = zeros(points_DockedwithMar,1);
+Y_DockedwithMar = zeros(points_DockedwithMar,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));
+end
+
 figure;
-plot(index_DockedwithMarker,scanner_value_DockedwithMarker);
+plot(s_angle_DockedwithMar,s_val_DockedwithMar);
 grid on;
-xlabel('Index');
+xlabel('Angle [rad]');
 ylabel('Distance to Obstacle [m]');
 title('Scanner Analysis for Docked Robot with a Marker');
-axis([0 540 0 31]);
-% 
+axis([-3 3 0 31]);
 
+figure;
+plot(X_DockedwithMar,Y_DockedwithMar)
 %% The robot is moving Towards docking...!
 Scanner_Matrix_Moving = csvread('scanner_data_Moving_Towards_Docking_WITH_Marker.txt',0,0);
 
@@ -36,42 +53,88 @@ axis([0 540 0 31]);
 
 
 %% The robot is in the docking area, while NOT docked!
-Scanner_Matrix_Outside = csvread('scanner_data_outside.txt',0,0);
-index_O = Scanner_Matrix_Outside(:,1);
-scanner_value_O = Scanner_Matrix_Outside(:,2);
+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);
+
+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));
+end
+
 figure;
-plot(index_O,scanner_value_O);
+plot(s_angle_out,s_val_out);
+% plot(index_O,scanner_value_O);
 grid on;
-xlabel('Index');
+xlabel('Angle [rad]');
 ylabel('Distance to Obstacle [m]');
 title('Scanner Analysis for Robot in the docking area');
-axis([0 540 0 31]);
-
+% axis([-140 140 0 31]);
 
+figure;
+plot(X_out,Y_out);
 
 %% The robot is totally docked and NO marker infront of laser scanner!
-Scanner_Matrix_DockedwithNOmarker = csvread('scanner_data_no_cylinder.txt',0,0);
-index_DockedwithNOmarker = Scanner_Matrix_DockedwithNOmarker(:,1);
-scanner_value_DockedwithNOmarker = Scanner_Matrix_DockedwithNOmarker(:,2);
+S_Mat_DockedwithNOmar = csvread('scanner_data_no_cylinder.txt',0,0);
+indx_DockedwithNOmar = S_Mat_DockedwithNOmar(:,1);
+s_val_DockedwithNOmar = S_Mat_DockedwithNOmar(:,2);
+
+
+points_DockedwithNOmar = size(indx_DockedwithNOmar,1);
+laser_beam_DockedwithNOmar = points_DockedwithNOmar - 1;
+s_angle_DockedwithNOmar = zeros(points_DockedwithNOmar,1);
+X_DockedwithNOmar = zeros(points_DockedwithNOmar,1);
+Y_DockedwithNOmar = zeros(points_DockedwithNOmar,1);
+
+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));
+end
+
+
 figure;
-plot(index_DockedwithNOmarker,scanner_value_DockedwithNOmarker);
+plot(s_angle_DockedwithNOmar,s_val_DockedwithNOmar);
 grid on;
-xlabel('Index');
+xlabel('Angle [rad]');
 ylabel('Distance to Obstacle [m]');
 title('Scanner Analysis for Docked Robot withOUT Marker');
-axis([0 540 0 31]);
+axis([-3 3 0 31]);
 
+figure;
+plot(X_DockedwithNOmar,Y_DockedwithNOmar);
+%% 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);
+
+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));
+end
 
 
-%% The robot is totally docked and a box is besides of laser scanner!
-Scanner_Matrix_DockedwithBOX = csvread('scanner_data_box.txt',0,0);
-index_DockedwithBOX = Scanner_Matrix_DockedwithBOX(:,1);
-scanner_value_DockedwithBOX = Scanner_Matrix_DockedwithBOX(:,2);
 figure;
-plot(index_DockedwithBOX,scanner_value_DockedwithBOX);
+plot(s_angle_DockedwithBOX,s_val_DockedwithBOX);
 grid on;
-xlabel('Index');
+xlabel('Angle [rad]');
 ylabel('Distance to Obstacle [m]');
 title('Scanner Analysis for Docked Robot with a box as marker on a side');
-axis([0 540 0 31]);
\ No newline at end of file
+axis([-3 3 0 31]);
+
+figure;
+plot(X_DockedwithBOX,Y_DockedwithBOX);
\ No newline at end of file