function Homework2
 % Solution to Problem 3, HW2, 2016

 
 close all
data = csvread('HW2_data.csv');   % Read data acquired in class

% data(:,1) - time
% data(:,2) - x position
% data(:,3) - y position
% data(:,4) - z position
% data(:,5) - x acceleration
% data(:,6) - y acceleration
% data(:,7) - z acceleration

time = data(:,1);

% Calculate velocity by differentiating position
v = zeros(length(time),3);
for i=2:length(time)
    v(i,1:3) = (data(i,2:4)-data(i-1,2:4))/(time(i)-time(i-1));
end

figure
figure1 = figure;
axes1 = axes('Parent',figure1,'YGrid','on','XGrid','on','FontSize',14);
box(axes1,'on');
hold(axes1,'on');
ylabel({'Velocity (m/s)'},'FontSize',20);
xlabel({'Time (seconds)'},'FontSize',20);
title('Velocity from Position - v - Time');
plot1 = plot(time,v); % Velocity from observer
set(plot1(1),'DisplayName','Vx');
set(plot1(2),'DisplayName','Vy');
set(plot1(3),'DisplayName','Vz');
legend1 = legend(axes1,'show');
set(legend1,'FontSize',9);


% Calculate the velocity by integrating acceleration
data(:,7) = data(:,7)-9.8;  % Subtract g from the vertical accel
vacc = cumtrapz(time,data(:,5:7));
figure2 = figure;
axes2 = axes('Parent',figure2,'YGrid','on','XGrid','on','FontSize',14);
box(axes2,'on');
hold(axes2,'on');
ylabel({'Velocity (m/s)'},'FontSize',20);
xlabel({'Time (seconds)'},'FontSize',20);
title('Velocity from Acc - v - Time');
plot2 = plot(time,vacc); % Velocity from observer
set(plot2(1),'DisplayName','Vx');
set(plot2(2),'DisplayName','Vy');
set(plot2(3),'DisplayName','Vz');
legend2 = legend(axes2,'show');
set(legend2,'FontSize',9);





end