function HW2_2021
close all

%% First data set (fixed orientation)

data = csvread('Figure-eight-fixed.csv');
time = data(:,1); pos = data(:,2:4)-data(1,2:4); vel = data(:,5:7); acc = data(:,8:10);


% Plot trajectory
figure
plot3(pos(:,1),pos(:,2),pos(:,3),'Linewidth',2,'Color',[1,0,0]);
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',20,'XGrid','on','YGrid',...
    'on','ZGrid','on');
title('Trajectory (fixed orientation)')
xlabel('x (m)'); ylabel('y (m)');  zlabel('z (m)');
% Plot velocity
figure
p1 = plot(time,vel(:,1),'Linewidth',1,'Color',[0,0,1]);
hold on
p2 = plot(time,vel(:,2),'Linewidth',1,'Color',[1,0,0]);
xlabel('time (s)')
ylabel('Velocity (m/s)')
title('Quadcopter velocity (fixed orientation)')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',14,'XGrid','on','YGrid',...
    'on');
legend1 = legend([p1,p2],'v_x','v_y');
set(legend1,...
    'Position',[0.41665983606557377 0.208333329529014 0.229166666666668 0.187022895474015],...
    'FontAngle','italic',...
    'FontSize',14);

% Plot acceleration
figure
p1 = plot(time,acc(:,1),'Linewidth',1,'Color',[0,0,1]);
hold on
p2 = plot(time,acc(:,2),'Linewidth',1,'Color',[1,0,0]);
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
title('Quadcopter acceleration (fixed orientation)')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',14,'XGrid','on','YGrid',...
    'on');
legend1 = legend([p1,p2],'a_x','a_y');
set(legend1,...
    'Position',[0.41665983606557377 0.208333329529014 0.229166666666668 0.187022895474015],...
    'FontAngle','italic',...
    'FontSize',14);


% Integrate acceleration
integratedaccel = cumtrapz(time,acc);
figure
p1 = plot(time,integratedaccel(:,1),'Linewidth',1,'Color',[0,0,1]);
hold on
p2 = plot(time,integratedaccel(:,2),'Linewidth',1,'Color',[1,0,0]);
p3 = plot(time,vel(:,1),'Linewidth',1,'Color',[0,0,1],'LineStyle','--');
hold on
p4 = plot(time,vel(:,2),'Linewidth',1,'Color',[1,0,0],'LineStyle','--');
xlabel('time (s)')
ylabel('Velocity (m/s)')
title('Quadcopter velocity (fixed orientation)')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',14,'XGrid','on','YGrid',...
    'on');
legend1 = legend([p1,p2,p3,p4],'integrated a_x','integrated a_y','measured v_x',' measured v_y');
set(legend1,...
    'Position',[0.126243172319535 0.6843447808423 0.297999994158745 0.250999992847443],...
    'FontAngle','italic',...
    'FontSize',14);



%% Second data set (with turn)

data = csvread('Figure-eight-with-turn.csv');
time = data(:,1); pos = data(:,2:4)-data(1,2:4); vel = data(:,5:7); acc = data(:,8:10);


% Plot trajectory
figure
plot3(pos(:,1),pos(:,2),pos(:,3),'Linewidth',2,'Color',[1,0,0]);
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',20,'XGrid','on','YGrid',...
    'on','ZGrid','on');
title('Trajectory (with turn)')
xlabel('x (m)'); ylabel('y (m)');  zlabel('z (m)');

figure
p1 = plot(time,vel(:,1),'Linewidth',1,'Color',[0,0,1]);
hold on
p2 = plot(time,vel(:,2),'Linewidth',1,'Color',[1,0,0]);
xlabel('time (s)')
ylabel('Velocity (m/s)')
title('Quadcopter velocity (with turn)')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',14,'XGrid','on','YGrid',...
    'on');
legend1 = legend([p1,p2],'v_L','v_T');
set(legend1,...
    'Position',[0.41665983606557377 0.208333329529014 0.229166666666668 0.187022895474015],...
    'FontAngle','italic',...
    'FontSize',14);


figure
p1 = plot(time,acc(:,1),'Linewidth',1,'Color',[0,0,1]);
hold on
p2 = plot(time,acc(:,2),'Linewidth',1,'Color',[1,0,0]);
xlabel('time (s)')
ylabel('Acceleration (m/s^2)')
title('Quadcopter acceleration (with turn)')
axes1 = gca;
set(axes1,'FontName','Times New Roman','FontSize',14,'XGrid','on','YGrid',...
    'on');
legend1 = legend([p1,p2],'a_L','a_T');
set(legend1,...
    'Position',[0.338659836065574 0.728333329529015 0.229166666666668 0.187022895474015],...
    'FontAngle','italic',...
    'FontSize',14);





end