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',1.9); hold on plot(t_P_sec_data_2,Pose_X_data_2,'r','LineWidth',1.8); hold on plot(t_P_sec_data_3,Pose_X_data_3,'g','LineWidth',1.7); hold on plot(t_P_sec_data_4,Pose_X_data_4,'m','LineWidth',1.6); % 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.9); hold on plot(t_P_sec_data_2,Pose_Y_data_2,'r','LineWidth',1.8); hold on plot(t_P_sec_data_3,Pose_Y_data_3,'g','LineWidth',1.7); hold on plot(t_P_sec_data_4,Pose_Y_data_4,'m','LineWidth',1.6); % 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.9); hold on plot(t_P_sec_data_2,Theta_data_2,'r','LineWidth',1.8); hold on plot(t_P_sec_data_3,Theta_data_3,'g','LineWidth',1.7); hold on plot(t_P_sec_data_4,Theta_data_4,'m','LineWidth',1.6); % 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.9); hold on plot(t_V_sec_data_2,Vel_X_data_2,'r','LineWidth',1.8); hold on plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',1.7); hold on plot(t_V_sec_data_4,Vel_X_data_4,'m','LineWidth',1.6); % 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.9); hold on plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.8); hold on plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',1.7); hold on plot(t_V_sec_data_4,Vel_Y_data_4,'m','LineWidth',1.6); % 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.9); hold on plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.8); hold on plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',1.7); hold on plot(t_V_sec_data_4,Omega_Z_data_4,'m','LineWidth',1.6); % 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 + SM + Target}$','interpreter','latex','FontSize',20); % ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',20); % axis([-.65 .05 -1.6 -.19]); axis([-.65 .03 -1.47 -.16]); 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 + Target}$','interpreter','latex','FontSize',20); % xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',20); axis([-.025 0.005 -.29 -.19]); % axis([-.033 -.01 -.26 -.195]); 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,'k--','LineWidth',3.4); % set(gca,'fontsize',20); % title('$ {c) Target}$','interpreter','latex','FontSize',20); % axis([-.0205 -.0168 -.2054 -.199]); axis([-.0201 -.017 -.206 -.1995]); 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); legend('boxoff'); % 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,'r','LineWidth',2); hold on; plot(y_raw_3,x_raw_3,'g','LineWidth',2); hold on; plot(y_raw_4,x_raw_4,'m','LineWidth',2); xlabel('y_t'); ylabel('x_t'); % axis([-.1 .1 .19 1.55]);