y_tANDx_t.m

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);
Trans_Matrix_data_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 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);
Trans_Matrix_data_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 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);
Trans_Matrix_data_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 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);
Trans_Matrix_data_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 transposed_axis
TimeTrans_ros_data_4 = Trans_Matrix_data_4(:,1); % ros time, needs to be converted to sec...

% duriation =         end_time        -     start_time
  durationT_data_4 = (TimeTrans_ros_data_4(size(TimeTrans_ros_data_4,1),:) - TimeTrans_ros_data_4(1,:))*10^(-9); 
  
t_T_sec_data_4 = 0:durationT_data_4/size(TimeTrans_ros_data_4,1):durationT_data_4;
t_T_sec_data_4(:,size(t_T_sec_data_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_t_1 = Trans_Matrix_data_1(:,4); % x_t
y_t_1 = Trans_Matrix_data_1(:,3); % y_t
theta_t_1 = Trans_Matrix_data_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_t_2 = Trans_Matrix_data_2(:,4); % x_t
y_t_2 = Trans_Matrix_data_2(:,3); % y_t
theta_t_2 = Trans_Matrix_data_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_t_3 = Trans_Matrix_data_3(:,4); % x_t
y_t_3 = Trans_Matrix_data_3(:,3); % y_t
theta_t_3 = Trans_Matrix_data_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_t_4 = Trans_Matrix_data_4(:,4); % x_t
y_t_4 = Trans_Matrix_data_4(:,3); % y_t
theta_t_4 = Trans_Matrix_data_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

% Camera Pose Estimation

figure;
set(gcf,'color','white');
subplot(311);
plot(t_P_sec_data_1,Pose_X_data_1,'LineWidth',2.1);
hold on
plot(t_P_sec_data_2,Pose_X_data_2,'r','LineWidth',1.7);
hold on
plot(t_P_sec_data_3,Pose_X_data_3,'g','LineWidth',1.8);
hold on
plot(t_P_sec_data_4,Pose_X_data_4,'m','LineWidth',1.3);
title('Camera Pose Estimation');
ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',14);

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(312);
plot(t_P_sec_data_1,Pose_Y_data_1,'LineWidth',1.8);
hold on
plot(t_P_sec_data_2,Pose_Y_data_2,'r','LineWidth',1.7);
hold on
plot(t_P_sec_data_3,Pose_Y_data_3,'g','LineWidth',1.6);
hold on
plot(t_P_sec_data_4,Pose_Y_data_4,'m','LineWidth',1.4);
ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',14);

subplot(313);
plot(t_P_sec_data_1,Theta_data_1,'LineWidth',1.8);
hold on
plot(t_P_sec_data_2,Theta_data_2,'r','LineWidth',1.7);
hold on
plot(t_P_sec_data_3,Theta_data_3,'g','LineWidth',1.6);
hold on
plot(t_P_sec_data_4,Theta_data_4,'m','LineWidth',1.4);

xlabel('$ time [s]$','interpreter','latex','FontSize',14);
ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',14);


% Control Signals

% control signals
figure;
set(gcf,'color','white');

subplot(311);
plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',1.8);
hold on
plot(t_V_sec_data_2,Vel_X_data_2,'r','LineWidth',1.7);
hold on
plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',1.6);
hold on
plot(t_V_sec_data_4,Vel_X_data_4,'m','LineWidth',1.4);

title('Control Signals');
ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);

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',11);
legend('boxoff');


subplot(312);
plot(t_V_sec_data_1,Vel_Y_data_1,'LineWidth',1.8);
hold on
plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.7);
hold on
plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',1.6);
hold on
plot(t_V_sec_data_4,Vel_Y_data_4,'m','LineWidth',1.4);
ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);

subplot(313);
plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',1.8);
hold on
plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.7);
hold on
plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',1.6);
hold on
plot(t_V_sec_data_4,Omega_Z_data_4,'m','LineWidth',1.4);
xlabel('$ time [s]$','interpreter','latex','FontSize',14);
ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14);

% 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);

title('Approach zone + SM zone + Target area');
ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',14);
axis([-.65 .05 -1.6 -.19]);

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

title('SM zone + Target area');
xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14);
axis([-.025 0.005 -.29 -.19]);

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

title('Target area');
axis([-.0205 -.0168 -.2054 -.199]);
l = 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}$','${Target}$');
set(l,'interpreter','latex','FontSize',11);


% 
% % Transposed Axis
% figure;
% subplot(3,1,1);
% plot(t_T_sec_data_4,x_t_4,'LineWidth',1.5);
% ylabel('z_{t}');
% title('Transposed Axis');
% 
% 
% subplot(3,1,2);
% plot(t_T_sec_data_4,y_t_4,'LineWidth',1.5);
% ylabel('y_{t}');
% 
% subplot(3,1,3);
% plot(t_T_sec_data_4,theta_t_4,'LineWidth',1.5);
% xlabel('time');
% ylabel('yaw');
% 
% 
% figure;
% set(gcf,'color','white');
% plot(Pose_Y_data_4,Pose_X_data_4,'b','LineWidth',2);
% xlabel('y_{mar}');
% ylabel('x_{mar}');
% 
% figure;
% set(gcf,'color','white');
% plot(y_t_4,x_t_4,'b','LineWidth',2);
% xlabel('y_t');
% ylabel('x_t');
% axis([-.1 .1 .19 1.55]);