Paper mmodified

Files_4_thesis/Different_angle/docking_different_angle.m

@@ -157,7 +157,7 @@ plot(t_P_sec_data_2,Pose_X_data_2,'r','LineWidth',1.7);
 % plot(t_P_sec_data_3,Pose_X_data_3,'g','LineWidth',1.6);
 ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',20);
 title('a) ${x_{mar}}$','interpreter','latex','FontSize',20);
-set(gca,'fontsize',20);
+% set(gca,'fontsize',20);
 
 subplot(323);
 plot(t_P_sec_data_1,Pose_Y_data_1,'LineWidth',3);
@@ -167,7 +167,7 @@ plot(t_P_sec_data_2,Pose_Y_data_2,'r','LineWidth',1.7);
 % plot(t_P_sec_data_3,Pose_Y_data_3,'g','LineWidth',1.6);
 ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',20);
 title('b) ${y_{mar}}$','interpreter','latex','FontSize',20);
-set(gca,'fontsize',20);
+% set(gca,'fontsize',20);
 
 subplot(325);
 plot(t_P_sec_data_1,Theta_data_1,'LineWidth',3);
@@ -179,7 +179,7 @@ plot(t_P_sec_data_2,Theta_data_2,'r','LineWidth',1.7);
 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);
+% set(gca,'fontsize',20);
 
 % % control signals
 % figure;
@@ -200,7 +200,7 @@ l2 = legend('${Left}$', '${Right}$');
 set(l2,'interpreter','latex','FontSize',14);
 legend('boxoff')
 axis ([0 40 -.01 .18]);
-set(gca,'fontsize',20);
+% set(gca,'fontsize',20);
 
 subplot(324);
 plot(t_V_sec_data_1,Vel_Y_data_1,'LineWidth',3);
@@ -210,7 +210,7 @@ plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.7);
 % plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',1.6);
 ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
 title('e) $\dot{y}_{Rob}$','interpreter','latex','FontSize',20);
-set(gca,'fontsize',20);
+% set(gca,'fontsize',20);
 
 subplot(326);
 plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',3);
@@ -221,7 +221,7 @@ plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.7);
 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);
+% set(gca,'fontsize',20);
 
 % Trajectory 
 
@@ -247,9 +247,9 @@ hold on
 plot(y_circle,x_circle,'k--','LineWidth',3.4);
 
 % axis([-.4 .4 .18 1.4]);
-title('a) Approach zone + SM zone + Target area','FontSize',17);
+title('${a) Approach  + SM  + Target}$','interpreter','latex','FontSize',20);
 ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',20);
-set(gca,'fontsize',20);
+% set(gca,'fontsize',20);
 
 subplot(132);
 plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',1.6);
@@ -260,10 +260,10 @@ 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('b) SM zone + Target area','FontSize',17);
+title('$ {b) SM + Target}$','interpreter','latex','FontSize',20);
 xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',20);
 axis([-.03 0.03 -.29 -.191]);
-set(gca,'fontsize',20);
+% set(gca,'fontsize',20);
 
 subplot(133);
 plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
@@ -274,9 +274,9 @@ plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
 hold on
 plot(y_circle,x_circle,'k--','LineWidth',3.4);
 
-title('c) Target area','FontSize',17);
+title('$ {c) Target}$','interpreter','latex','FontSize',20);
 axis([-.0202 -.017 -.2054 -.199]);
-set(gca,'fontsize',20);
+% set(gca,'fontsize',20);
 
 l = legend('${Left}$', '${Right}$','Target');
 set(l,'interpreter','latex','FontSize',14);

Files_4_thesis/Different_controllers/docking_with_PID_controller.m

@@ -612,9 +612,9 @@ xlabel('Sample number','interpreter','latex','FontSize',20);
 % l6 = legend([pa3 pa4],'$ Android Camera {Time_{ROS}}$','$ Android Camera {Time_{Real}} [sec]$','Orientation','horizontal');
 % set(l6,'interpreter','latex','FontSize',12);
 % 
-set(gca,'fontsize',20);
+% set(gca,'fontsize',20);
 leg = legend('ROS Time, USB camera','Real Time, USB camera [s]','ROS Time, IP camera','Real Time, IP camera [s]','Location','northwest');
-set(leg,'FontSize',15);
+set(leg,'FontSize',17);
 legend('boxoff');
 
 % 

Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.asv

+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

Files_4_thesis/Finding_X_dot_with_Pcontroller/docking_with_P_controller.m

@@ -22,12 +22,12 @@ clear all;
 
 % 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('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);
+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...
@@ -52,13 +52,13 @@ t_V_sec_data_1(:,size(t_V_sec_data_1,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('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);
+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...
@@ -80,12 +80,12 @@ 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('Pose01_08_16_1831.txt',1,0);