%% Flight Test 2 Ground Track Position
% Aero 409 FLight Test
% Josh Huang
clear variables
close all
clc
% plotOrientation
load('test2.TXT')
accelReadings= test2(:,1:3);
gyroReadings= (test2(:,4:6))*((2*pi)/360); %deg/s to rad/s
magReadings= test2(:,7:9);
t= (test2(:,11)-test2(1,11))/1000; % convert ms -> s, reset first digit to 0


time =((test2(:,11)-test2(1,11))/1000);
acc = accelReadings;

%%Design High Pass Filter
fs = (max(time)-min(time))/length(time); % Sampling Rate
fc = .1/60;  % Cut off Frequency
order = 6; % 6th Order Filter
%%Filter  Acceleration Signals
[b1, a1] = butter(order,fc,'high');
accf=filtfilt(b1,a1,acc);


%%First Integration (Acceleration - Veloicty)
velocity=cumtrapz(time,accf);

%%Filter  Veloicty Signals
[b2, a2] = butter(order,fc,'high');
velf = filtfilt(b2,a2,velocity);
%%Second Integration   (Velocity - Displacement)
Displacement=cumtrapz(time, velf);
figure(4)

plot3(Displacement(:,1),Displacement(:,2),Displacement(:,3))
xlabel('x')
ylabel('y')
zlabel('z')

figure(5)
plot(Displacement(:,1),Displacement(:,2))
%% ~7500 ft