close all
clear all

Nav440Filename = 'Nav440ParagonMotion2019-11-05-2.txt';
VectorNaveFilename = 'VNLog-11-5-19-01'
time_offset = 244.42;   %Seconds from start of logging in each file
roll_offset = .9; %Roll Offset between IMU's
pitch_offset = 1;  %Pitch Offset between IMU's

%These times are the start of the eight legs making the box, in time
%relative to start of logging.
legStart_times(1) = 2050;
legStart_times(2) = 3680;
legStart_times(3) = 4620;
legStart_times(4) = 6200;
legStart_times(5) = 7200;
legStart_times(6) = 8400;
legStart_times(7) = 10000;
legStart_times(8) = 11125;
legStart_times(9) = 12500;



fileID = fopen(Nav440Filename);
M = textscan(fileID,'%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %d %s');
fclose(fileID);

Nav440Data.Time = M{1};
Nav440Data.Roll = M{2}-roll_offset;
Nav440Data.Pitch = M{3}-pitch_offset;
Nav440Data.Yaw = M{4};
Nav440Data.XAngRate =M{5};
Nav440Data.YAngRate = M{6};
Nav440Data.ZAngRate = M{7};
Nav440Data.NVel = M{11};
Nav440Data.EVel = M{12};
Nav440Data.DVel = M{13};
Nav440Data.Lon = M{14};
Nav440Data.Lat = M{15};
Nav440Data.Alt = M{16};
Nav440Data.Time_ITOW = M{18};

Nav440Data.t = (1/50)*(0:length(Nav440Data.Yaw)-1);
clear M;


fileID = fopen(VectorNaveFilename);
M = textscan(fileID,'%s %f %f %f %f %f %f %f %f %f %f %f %f %s');
fclose(fileID);
VNData.Yaw = M{2};
VNData.Pitch = M{3};
VNData.Roll = M{4};
VNData.XAngRate = M{11}*180/pi;
VNData.YAngRate = M{12}*180/pi;
VNData.ZAngRate = M{13}*180/pi;

%Correct for incomplete data read.  Mystery
VNData.XAngRate(end+1) = VNData.XAngRate(end);
VNData.YAngRate(end+1) = VNData.YAngRate(end);
VNData.ZAngRate(end+1) = VNData.ZAngRate(end);

VNData.t = (1/40)*(0:length(VNData.Yaw)-1)-time_offset;



figure
plot3(Nav440Data.Lon,Nav440Data.Lat,Nav440Data.t)



figure
plot(Nav440Data.Lon,Nav440Data.Lat)
hold all
plot(Nav440Data.Lon,Nav440Data.Lat,'.')

grid on;
axis equal;
xlabel('Longitude');
ylabel('Latitude');


Nav440_idx = find((Nav440Data.t > legStart_times(1)) & (Nav440Data.t < legStart_times(9)));
VN_idx = find((VNData.t > legStart_times(1)) & (VNData.t < legStart_times(9)));


figure
ah1 = subplot(4,1,1)
plot(Nav440Data.t(Nav440_idx),Nav440Data.Roll(Nav440_idx));
hold all
plot(VNData.t(VN_idx),VNData.Roll(VN_idx));
ylabel('Roll (deg)');
ylim([-10 10]);
legend('Nav440','VectorNav On Pole');

ah2 = subplot(4,1,2)
plot(Nav440Data.t(Nav440_idx),Nav440Data.Pitch(Nav440_idx));
hold all
plot(VNData.t(VN_idx),VNData.Pitch(VN_idx));
ylabel('Pitch (deg)');
ylim([-10 10]);
legend('Nav440','VectorNav On Pole');


dt = .02;

[pxx,f] = pwelch(Nav440Data.Pitch(Nav440_idx),1/dt)
figure
plot(f,pxx)
xlabel('Freq (hz)')
ylabel('S  [sqrt(deg^2/Hz)]');
xlim([0 .5]);
title('Nav440 Pitch PSD');


ah3 = subplot(4,1,3)
plot(Nav440Data.t(Nav440_idx),Nav440Data.XAngRate(Nav440_idx));
hold all
plot(VNData.t(VN_idx),VNData.XAngRate(VN_idx));
ylim([-30 30]);
hold all
legend('Nav440','VectorNav On Pole');

ylabel('Roll Rate (deg/s)');

ah4 = subplot(4,1,4)
plot(Nav440Data.t(Nav440_idx),Nav440Data.YAngRate(Nav440_idx));
hold all
plot(VNData.t(VN_idx),VNData.YAngRate(VN_idx));
ylabel('Pitch Rate (deg/s)');
ylim([-30 30]);
legend('Nav440','VectorNav On Pole');


linkaxes([ah1 ah2 ah3 ah4],'x');


figure
subplot(2,1,1)
histogram(Nav440Data.Roll(Nav440_idx),20);
xlim([-10 10]);
xlabel('Roll (degrees)');
ylabel('Occurances');
subplot(2,1,2)
histogram(Nav440Data.Pitch(Nav440_idx),20);
xlim([-10 10]);
xlabel('Pitch (degrees)');
ylabel('Occurances');

figure
subplot(2,1,1)
histogram(Nav440Data.XAngRate(Nav440_idx),20);
xlim([-20 20]);
xlabel('Roll Rate (degrees/Sec)');
ylabel('Occurances');

subplot(2,1,2)
histogram(Nav440Data.YAngRate(Nav440_idx),20);
xlim([-20 20]);
xlabel('Pitch Rate (degrees/Sec)');
ylabel('Occurances');

%Compute Alongtrack velocity of Nav440
Vx = Nav440Data.NVel.*cos(Nav440Data.Yaw*pi/180) + ...
     Nav440Data.EVel.*sin(Nav440Data.Yaw*pi/180); 

N = 25;
Vx_Smoothed = filtfilt(ones(1,N)./N,1,Vx);
Vx_Detrended = Vx-Vx_Smoothed;

N = 2000;
Px = cumsum(Vx_Detrended*.02);
Px_Smoothed =  filtfilt(ones(1,N)./N,1,Px);
Px_Detrended = Px-Px_Smoothed;



Ax = [0 diff(Vx_Smoothed')./diff(Nav440Data.t)]

dt = (Nav440Data.t(101)- Nav440Data.t(1))/100;

figure
ah1 = subplot(2,1,1)
plot(Nav440Data.t(Nav440_idx),Vx(Nav440_idx));
xlabel('t');
ylabel('Along-track Velocity (m/s)');
hold all
plot(Nav440Data.t(Nav440_idx),Vx_Smoothed(Nav440_idx));
ah2 = subplot(2,1,2)
plot(Nav440Data.t(Nav440_idx),Ax(Nav440_idx));

[pxx,f] = pwelch(Ax,1/dt)
figure
plot(f,pxx)
xlabel('Freq (hz)')
ylabel('S');
xlim([0 .5]);


Vz = Nav440Data.DVel;
N = 2000;
Vz_Smoothed =  filtfilt(ones(1,N)./N,1,Vz);
Vz_Detrended = Vz-Vz_Smoothed;

Pz = cumsum(Vz_Detrended*.02);
Pz_Smoothed =  filtfilt(ones(1,N)./N,1,Pz);
Pz_Detrended = Pz-Pz_Smoothed;

figure
plot(Nav440Data.t,[Vz Vz_Smoothed]);

figure
plot(Nav440Data.t,Vz_Detrended);
hold all;
plot(Nav440Data.t, [0 diff(Pz_Detrended)'./diff(Nav440Data.t)]);












