function HW2
% Script to read the csv file recording data from a quadcopter in flight
% and integrate the velocity to compare with position 
 close all
 data = csvread('flight_data.csv');
 time = data(:,1); % Col 1 is time
 pos = data(:,2:4); % Cols 2-4 are (x,y,z)
 vel = data(:,5:7); % Cols 5-7 are (vx,vy,vz)
%% Plot the velocity
 figure2 = figure;
 axes2 = axes('Parent',figure2,'YGrid','on','XGrid','on','FontSize',12);
 box(axes2,'on');
 hold(axes2,'on');
 plot2 = plot(time,vel); % Plot the velocity
% Create ylabel
 ylabel('Velocity (m/s)');
% Create xlabel
 xlabel('Time (s)');
 set(plot2(1),'DisplayName','Vx');
 set(plot2(2),'DisplayName','Vy');
 set(plot2(3),'DisplayName','Vz');
% Create legend
 legend1 = legend(axes2,'show');
 set(legend1,...
    'Position',[0.258640552227721 0.233558766307309 0.153256193002357 0.138038956130547],...
    'FontSize',10.8);
 
 
 
 %% Integrate the velocity using cumtrapz
 posfromvel = cumtrapz(time,vel); % integrate the velocity
 
 %% Plot the integrated velocity and measured position
 % Here we add the initial position to the integrated velocity.
 % Another way to do this would be to subtract the initial position
 % from the measured position.  Either method is fine.
 %
 for i=1:3 posfromvel(:,i) = posfromvel(:,i) + pos(1,i); end
 figure1 = figure;

 axes1 = axes('Parent',figure1,'YGrid','on','XGrid','on','FontSize',12);

 box(axes1,'on');
 hold(axes1,'on');
 plot1 = plot(time,[posfromvel,pos]);
 ylabel({'Position (m)'});
 xlabel({'Time (s)'}); 
 set(plot1(1),'DisplayName','x from velocity');
 set(plot1(2),'DisplayName','y from velocity');
 set(plot1(3),'DisplayName','z from velocity');
 set(plot1(4),'DisplayName','measured x');
 set(plot1(5),'DisplayName','measured  y');
 set(plot1(6),'DisplayName','measured z');
 legend1 = legend(axes1,'show');
 set(legend1,...
    'Position',[0.245996327026618 0.105594059336036 0.24488537272109 0.265250379391798],...
    'FontSize',10.8);

end
