%% GNSS compare
clc; clear; close all; format compact;
pause on;
time = cputime;
Timing = struct('Piksi_loadtime',0,'PiksiH_loadtime',0,'BD982_loadtime',0,'BD992_loadtime',0,'Data_sorting_time',0,'Data_statistics_time',0);
Screen = get(0,'Screensize');
Piksistring = {'P_Invalid',-1,'P_SPP',-1,'P_DGNSS',-1,'P_Float',-1,'P_Fixed',-1,'P_DeadReckoning',-1,'P_SBAS',-1};

%% Static data
% %REF1 == 49 13 37.65947 16 34 30.66812 355.761
 Rover =  [49.227132535 16.575194219 355.772]; % 1.5 2020 
% % 17.5 2020 === [49.227132531, 16.575194211, 355.772];
% 
% % TUBO = 49 12 21.21002 16 35 34.20403 324.274  
Base =  [49.205896576 16.592843083 324.28]; %  1.5 2020 
% %  17.5 2020 === [49.205896572, 16.592843075, 324.283];


% ref1 = [4000143.357	1190608.371	4807361.930 ];
% %  49.227131683 16.57519236 355.762
% TUBO3 = [4001470.607 1192345.310 4805795.318 49.205891672222222 16.592834452777776];

% % 49.205896628 16.592843032 324.283

% N = [-sin(TUBO3(5)) cos(TUBO3(5)) 0; -sin(TUBO3(4))*cos(TUBO3(5)) -sin(TUBO3(4))*sin(TUBO3(5)) cos(TUBO3(5)); cos(TUBO3(4))*cos(TUBO3(5)) cos(TUBO3(4))*sin(TUBO3(5)) sin(TUBO3(4))]*[ref1(1) - TUBO3(1);ref1(2) - TUBO3(2);ref1(3) - TUBO3(3)]
% 
wgs84 = wgs84Ellipsoid;
[xEast,yNorth,zUp] = geodetic2enu(Rover(1), Rover(2), Rover(3), Base(1), Base(2), Base(3),wgs84);
xEast = xEast*1000; yNorth = yNorth*1000; zUp = zUp*1000;

%% Load Piksi and Piksi_Heading
file1 = uigetfile('*.csv');
file2 = uigetfile('*.csv');
file3 = uigetfile('*.csv');
file4 = uigetfile('*.csv');

Piksi = importdata(file1,';',2);
GPS_Week = Piksi.data(:,1);
Piksi_Time = Piksi.data(GPS_Week~=0,2);
Piksi_Lat = Piksi.data(GPS_Week~=0,3);
Piksi_Lon = Piksi.data(GPS_Week~=0,4);
Piksi_Height = Piksi.data(GPS_Week~=0,5);
Piksi_rms_H = Piksi.data(GPS_Week~=0,6);
Piksi_rms_V = Piksi.data(GPS_Week~=0,7);
Piksi_N = Piksi.data(GPS_Week~=0,8);
Piksi_E = Piksi.data(GPS_Week~=0,9);
% Piksi_U = -Piksi.data(:,8);
% Piksi_U = -(Piksi.data(:,8) + 126.74); %piksi default 
Piksi_U = (-Piksi.data(GPS_Week~=0,10)) + 19; %trimble phase center
Piksi_GDOP = Piksi.data(GPS_Week~=0,16);
Piksi_HDOP = Piksi.data(GPS_Week~=0,17);
Piksi_PDOP = Piksi.data(GPS_Week~=0,18);
% Piksi_TDOP = Piksi.data(:,19);
% Piksi_VDOP = Piksi.data(:,20);
Piksi_Flags = Piksi.data(GPS_Week~=0,19);
figure('Outerposition',[3*Screen(3)/4 Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on;
ax1 = subplot(4,1,1);
plot((Piksi_Time-min(Piksi_Time))/60000,Piksi_Flags)
title('Flags in time');
ax1.YTick = 4;

Samples(1) = size(Piksi_Time,1);

Timing.Piksi_loadtime = cputime - time;
time = cputime;

Piksi = importdata(file2,';',2);
GPS_Week = Piksi.data(:,1);
PiksiH_Time = Piksi.data(GPS_Week~=0,2);
PiksiH_Flags = Piksi.data(GPS_Week~=0,19);
PiksiH_Heading = Piksi.data(GPS_Week~=0,20);
ax2 = subplot(4,1,2);
plot((PiksiH_Time-min(PiksiH_Time))/60000,PiksiH_Flags)
ax2.YTick = 4;


Samples(2) = size(PiksiH_Time,1);

clear('Piksi');
Timing.PiksiH_loadtime = cputime - time;
time = cputime;
%% Load Trimble
BD982 = importdata(file4,',',2);
BD982_Week = 600000;
BD982_Time = BD982.data(:,1);
beg = min(find(BD982_Time>BD982_Week));
BD982_Time(BD982_Time<BD982_Week) = [];
BD982_E = round(BD982.data(beg:end,2),3)*1000;
BD982_N = round(BD982.data(beg:end,3),3)*1000;
BD982_U = round(BD982.data(beg:end,4),3)*1000;
BD982_rms_E = BD982.data(beg:end,5)*1000;
BD982_rms_N = BD982.data(beg:end,6)*1000;
BD982_rms_U = BD982.data(beg:end,7)*1000;
BD982_Heading = rad2deg(BD982.data(beg:end,10));
BD982_Lat = BD982.data(beg:end,14);
BD982_Lon = BD982.data(beg:end,15);
BD982_Height = BD982.data(beg:end,16);
BD982_Flags = BD982.data(beg:end,21);
ax3 = subplot(4,1,3);
plot(BD982_Time,BD982_Flags)
ax3.YTick = [2 3];
plot((BD982_Time-min(BD982_Time))/60000,BD982_Flags)

Samples(3) = size(BD982_Time,1);

clear('BD982');
Timing.BD982_loadtime = cputime - time;
time = cputime;


BD992 = importdata(file3,',',2);
BD992_Week = 600000;
BD992_Time = BD992.data(:,1);
beg = min(find(BD992_Time>BD992_Week));
BD992_Time(BD992_Time<BD992_Week) = [];
BD992_E = round(BD992.data(beg:end,2),3)*1000;
BD992_N = round(BD992.data(beg:end,3),3)*1000;
BD992_U = round(BD992.data(beg:end,4),3)*1000;
BD992_rms_E = BD992.data(beg:end,5)*1000;
BD992_rms_N = BD992.data(beg:end,6)*1000;
BD992_rms_U = BD992.data(beg:end,7)*1000;
BD992_Heading = rad2deg(BD992.data(beg:end,10));
BD992_Lat = BD992.data(beg:end,14);
BD992_Lon = BD992.data(beg:end,15);
BD992_Height = BD992.data(beg:end,16);
BD992_Flags = BD992.data(beg:end,21);
ax4 = subplot(4,1,4);
plot((BD992_Time-min(BD992_Time))/60000,BD992_Flags)
ax4.YTick = [2 3];
xlabel('Time [Minutes]')
Samples(4) =  size(BD992_Time,1);

clear('BD992');
Timing.BD992_loadtime = cputime - time;
time = cputime;

%% Data filtering BD982
temp = isnan(BD982_Heading);
BD982_Heading(temp) = [];
BD982_Time(temp) = [];
BD982_Flags(temp) = [];
BD982_N(temp) = [];
BD982_E(temp) = [];
BD982_U(temp) = [];

temp = isnan(BD982_N);
BD982_N(temp) = [];
BD982_E(temp) = [];
BD982_U(temp) = [];
BD982_Time(temp) = [];
BD982_Heading(temp) = [];
BD982_Flags(temp) = [];

temp = isnan(BD982_E);
BD982_N(temp) = [];
BD982_E(temp) = [];
BD982_U(temp) = [];
BD982_Time(temp) = [];
BD982_Heading(temp) = [];
BD982_Flags(temp) = [];

temp = isnan(BD982_U);
BD982_N(temp) = [];
BD982_E(temp) = [];
BD982_U(temp) = [];
BD982_Time(temp) = [];
BD982_Heading(temp) = [];
BD982_Flags(temp) = [];

BD982_N(~BD982_E) = [];
BD982_U(~BD982_E) = [];
BD982_Flags(~BD982_E) = [];
BD982_Time(~BD982_E) = [];
BD982_E(~BD982_E) = [];

%% BD992

temp = isnan(BD992_Heading);
BD992_Heading(temp) = [];
BD992_Time(temp) = [];
BD992_Flags(temp) = [];
BD992_N(temp) = [];
BD992_E(temp) = [];
BD992_U(temp) = [];

temp = isnan(BD992_N);
BD992_N(temp) = [];
BD992_E(temp) = [];
BD992_U(temp) = [];
BD992_Time(temp) = [];
BD992_Heading(temp) = [];
BD992_Flags(temp) = [];

temp = isnan(BD992_E);
BD992_N(temp) = [];
BD992_E(temp) = [];
BD992_U(temp) = [];
BD992_Time(temp) = [];
BD992_Heading(temp) = [];
BD992_Flags(temp) = [];

temp = isnan(BD992_U);
BD992_N(temp) = [];
BD992_E(temp) = [];
BD992_U(temp) = [];
BD992_Time(temp) = [];
BD992_Heading(temp) = [];
BD992_Flags(temp) = [];

BD992_N(~BD992_E) = [];
BD992_U(~BD992_E) = [];
BD992_Flags(~BD992_E) = [];
BD992_Time(~BD992_E) = [];
BD992_E(~BD992_E) = [];




%% Piksi3
temp = isnan(PiksiH_Heading);
PiksiH_Flags(temp) = [];
PiksiH_Time(temp) = [];
PiksiH_Heading(temp) = [];

temp = isnan(Piksi_N);
Piksi_N(temp) = [];
Piksi_E(temp) = [];
Piksi_U(temp) = [];
Piksi_Time(temp) = [];
Piksi_Flags(temp) = [];

temp = isnan(Piksi_E);
Piksi_N(temp) = [];
Piksi_E(temp) = [];
Piksi_U(temp) = [];
Piksi_Time(temp) = [];
Piksi_Flags(temp) = [];

temp = isnan(Piksi_U);
Piksi_N(temp) = [];
Piksi_E(temp) = [];
Piksi_U(temp) = [];
Piksi_Time(temp) = [];
Piksi_Flags(temp) = [];

Piksi_E(~Piksi_N) = [];
Piksi_U(~Piksi_N) = [];
Piksi_Flags(~Piksi_N) = [];
Piksi_Time(~Piksi_N) = [];
Piksi_N(~Piksi_N) = [];

Piksi_N(~Piksi_E) = [];
Piksi_U(~Piksi_E) = [];
Piksi_Flags(~Piksi_E) = [];
Piksi_Time(~Piksi_E) = [];
Piksi_E(~Piksi_E) = [];

temp = struct('Invalid',-1,'SPP',-1,'DGNSS',-1,'Float',-1,'Fixed',-1,'DeadReckoning',-1,'SBAS',-1);
Flags.Piksi = temp;
Flags.PiksiH = temp;
temp = struct('NoPosition',-1,'Autonomous',-1,'Float',-1,'Fixed',-1,'DGNSS',-1);
Flags.BD982 = temp;
Flags.BD992 = temp;
clear('temp');

fields = fieldnames(Flags.Piksi);

for i = 1:1:(numel(fields))
    Flags.Piksi.(fields{i}) = find(Piksi_Flags == i - 1);
    Flags.PiksiH.(fields{i}) = find(PiksiH_Flags == i - 1);
end

fields = fieldnames(Flags.BD982);

for i = 1:1:(numel(fields))
    Flags.BD982.(fields{i}) = find(BD982_Flags == i - 1);
    Flags.BD992.(fields{i}) = find(BD992_Flags == i - 1);
end

clear('fields')
Timing.data_sorting_time = cputime - time;
time = cputime;
%% Data statistics
[PiksiTime, PiksiSection, PiksiHSection] = intersect(Piksi_Time, PiksiH_Time);
[TrimbleTime, BD982Section, BD992Section] = intersect(BD982_Time, BD992_Time);
[TimeMatch, PiksiMatch, TrimbleMatch] = intersect(PiksiTime, TrimbleTime);

figure('Outerposition',[3*Screen(3)/4 2*Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on; 
title('Heading in time');
plot((TimeMatch-min(TimeMatch))/60000,PiksiH_Heading(PiksiHSection(PiksiMatch)),'rx');
plot((TimeMatch-min(TimeMatch))/60000,BD982_Heading(BD982Section(TrimbleMatch)),'bo');
plot((TimeMatch-min(TimeMatch))/60000,BD992_Heading(BD992Section(TrimbleMatch)),'g*');
legend('Piksi 3', 'BD 982', 'BD 992')
xlabel('Time [Minutes]')
ylabel('Heading [degrees]')

figure('Outerposition',[2*Screen(3)/4 2*Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on; 
title('Height');
plot((TimeMatch-min(TimeMatch))/60000, Piksi_Height(PiksiHSection(PiksiMatch)),'rx');
plot((TimeMatch-min(TimeMatch))/60000, BD982_Height(BD982Section(TrimbleMatch)),'bo');
plot((TimeMatch-min(TimeMatch))/60000, BD992_Height(BD992Section(TrimbleMatch)),'g*');
CORR_U(1) = corr(Piksi_Height(PiksiSection(PiksiMatch)),BD982_Height(BD982Section(TrimbleMatch)));
CORR_U(2) = corr(Piksi_Height(PiksiSection(PiksiMatch)),BD992_Height(BD992Section(TrimbleMatch)));
CORR_U(3) = corr(BD982_Height(BD982Section(TrimbleMatch)),BD992_Height(BD992Section(TrimbleMatch)));
CORR_U
legend('Piksi 3', 'BD 982', 'BD 992','Location','southeast')
xlabel('Time [Minutes]')
ylabel('Height [m]')
hA = gca;
resetplotview(hA,'InitializeCurrentView');
set(hA,'ylim',[310 335]);

figure('Outerposition',[Screen(3)/4 2*Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on; 
subplot(2,1,1)
plot((TimeMatch-min(TimeMatch))/60000, (Piksi_E(PiksiSection(PiksiMatch))-xEast)/1000,'rx');hold on; 
plot((TimeMatch-min(TimeMatch))/60000, (BD982_E(BD982Section(TrimbleMatch))-xEast)/1000,'bo');hold on; 
plot((TimeMatch-min(TimeMatch))/60000, (BD992_E(BD992Section(TrimbleMatch))-xEast)/1000,'g*');hold on; 
title('North(Y) - East(X) Baseline in time')
% legend('Piksi 3', 'BD 982', 'BD 992')
ylabel('E [m]')
subplot(2,1,2)
plot((TimeMatch-min(TimeMatch))/60000, (Piksi_N(PiksiSection(PiksiMatch))-yNorth)/1000,'rx');hold on; 
plot((TimeMatch-min(TimeMatch))/60000, (BD982_N(BD982Section(TrimbleMatch))-yNorth)/1000,'bo');hold on; 
plot((TimeMatch-min(TimeMatch))/60000, (BD992_N(BD992Section(TrimbleMatch))-yNorth)/1000,'g*');hold on; 
ylabel('N [m]')
xlabel('Time [Minutes]')




% hA = gca;
% resetplotview(hA,'InitializeCurrentView');
% set(hA,'ylim',[333.5 335.5]);
CORR_H(1) = corr(PiksiH_Heading(PiksiHSection(PiksiMatch)),BD982_Heading(BD982Section(TrimbleMatch)));
CORR_H(2) = corr(PiksiH_Heading(PiksiHSection(PiksiMatch)),BD992_Heading(BD992Section(TrimbleMatch)));
CORR_H(3) = corr(BD982_Heading(BD982Section(TrimbleMatch)),BD992_Heading(BD992Section(TrimbleMatch)));
CORR_H


[PiksiTime, PiksiSection, PiksiHSection] = intersect(Piksi_Time(Flags.Piksi.Fixed), PiksiH_Time);
[TrimbleTime, BD982Section, BD992Section] = intersect(BD982_Time(Flags.BD982.Fixed), BD992_Time(Flags.BD992.Fixed));
[TimeMatch, PiksiMatch, TrimbleMatch] = intersect(PiksiTime, TrimbleTime);
Samples(2,1) = size(Piksi_Time(Flags.Piksi.Fixed),1);
Samples(2,3) = size(BD982_Time(Flags.BD982.Fixed),1);
Samples(2,4) = size(BD992_Time(Flags.BD992.Fixed),1);
RTK_Ratio(1) = size(Piksi_Time(Flags.Piksi.Fixed))/size(Piksi_Time);
RTK_Ratio(2) = size(BD982_Time(Flags.BD982.Fixed))/size(BD982_Time);
RTK_Ratio(3) = size(BD992_Time(Flags.BD992.Fixed))/size(BD992_Time);
Samples;
common_samples = size(TimeMatch,1);
Measurement_Lenght = duration((max(TimeMatch) - min(TimeMatch))/(1000*60*60),0,0)
RTK_Ratio

%% RMS
figure('Outerposition',[0 Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on
title('RMS Cumulative Piksi Indicated')

temp = unique(Piksi_rms_H(PiksiSection(PiksiMatch)));
temp2 = 0;
for i = 1:1:size(unique(Piksi_rms_H(PiksiSection(PiksiMatch))))
    temp2(i+1) = temp2(i) + size(Piksi_rms_H(Piksi_rms_H(PiksiSection(PiksiMatch)) == temp(i)),1);
end
temp2 = temp2/max(temp2)*100;
temp2(1) = [];
plot(temp2(1:(size(temp,1))), temp,'b')
Piksi_correct_H(1,:) = interp1(temp2,temp,[50 68 95 98]);
xlabel('Bins [%]')
ylabel('Error [mm]')
temp = unique(Piksi_rms_V(PiksiSection(PiksiMatch)));
temp2 = 0;
for i = 1:1:size(unique(Piksi_rms_V(PiksiSection(PiksiMatch))))
    temp2(i+1) = temp2(i) + size(Piksi_rms_V(Piksi_rms_V(PiksiSection(PiksiMatch)) == temp(i)),1);
end
temp2 = temp2/max(temp2)*100;
temp2(1) = [];
plot(temp2(1:(size(temp,1))), temp,'g')
Piksi_correct_V(1,:) = interp1(temp2,temp,[50 68 95 98]);
legend('Piksi 3 I H', 'Piksi 3 I V','Location','northwest')
xlabel('Bins [%]')
ylabel('Error [mm]')
hA = gca;
resetplotview(hA,'InitializeCurrentView');
set(hA,'ylim',[0 100]);

figure('Outerposition',[Screen(3)/4 Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on
title('RMS Cumulative BD982 Indicated')

temp = unique(BD982_rms_E(BD982Section(TrimbleMatch)));
temp2 = 0;
for i = 1:1:size(unique(BD982_rms_E(BD982Section(TrimbleMatch))))
    temp2(i+1) = temp2(i) + size(BD982_rms_E(BD982_rms_E(BD982Section(TrimbleMatch)) == temp(i)),1);
end
temp2(1) = [];
temp2 = temp2/max(temp2)*100;
plot(temp2(1:(size(temp,1))), temp,'b')
BD982_correct_E(1,:) = interp1(temp2,temp,[50 68 95 98]);
temp = unique(BD982_rms_N(BD982Section(TrimbleMatch)));
temp2 = 0;
for i = 1:1:size(unique(BD982_rms_N(BD982Section(TrimbleMatch))))
    temp2(i+1) = temp2(i) + size(BD982_rms_N(BD982_rms_N(BD982Section(TrimbleMatch)) == temp(i)),1);
end
temp2(1) = [];
temp2 = temp2/max(temp2)*100;
plot(temp2(1:(size(temp,1))), temp,'g')
BD982_correct_N(1,:) = interp1(temp2,temp,[50 68 95 98]);
BD982_correct_H = sqrt(BD982_correct_E.^2 + BD982_correct_N.^2);
temp = unique(BD982_rms_U(BD982Section(TrimbleMatch)));
temp2 = 0;
for i = 1:1:size(unique(BD982_rms_U(BD982Section(TrimbleMatch))))
    temp2(i+1) = temp2(i) + size(BD982_rms_U(BD982_rms_U(BD982Section(TrimbleMatch)) == temp(i)),1);
end
temp2(1) = [];
temp2 = temp2/max(temp2)*100;
plot(temp2(1:(size(temp,1))), temp,'r')
BD982_correct_U(1,:) = interp1(temp2,temp,[50 68 95 98]);
legend('BD982 I E','BD982 I N','BD982 I U','Location','northwest')
xlabel('Bins [%]')
ylabel('Error [mm]')
hA = gca;
resetplotview(hA,'InitializeCurrentView');
set(hA,'ylim',[0 100]);

figure('Outerposition',[2*Screen(3)/4 Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on
title('RMS Cumulative BD992 Indicated')
temp = unique(BD992_rms_E(BD992Section(TrimbleMatch)));
temp2 = 0;
for i = 1:1:size(unique(BD992_rms_E(BD992Section(TrimbleMatch))))
    temp2(i+1) = temp2(i) + size(BD992_rms_E(BD992_rms_E(BD992Section(TrimbleMatch)) == temp(i)),1);
end
temp2(1) = [];
temp2 = temp2/max(temp2)*100;
plot(temp2(1:(size(temp,1))), temp,'b')
BD992_correct_E(1,:) = interp1(temp2,temp,[50 68 95 98]);
temp = unique(BD992_rms_N(BD992Section(TrimbleMatch)));
temp2 = 0;
for i = 1:1:size(unique(BD992_rms_N(BD992Section(TrimbleMatch))))
    temp2(i+1) = temp2(i) + size(BD992_rms_N(BD992_rms_N(BD992Section(TrimbleMatch)) == temp(i)),1);
end
temp2(1) = [];
temp2 = temp2/max(temp2)*100;
plot(temp2(1:(size(temp,1))), temp,'g')
BD992_correct_N(1,:) = interp1(temp2,temp,[50 68 95 98]);
BD992_correct_H = sqrt(BD992_correct_E.^2 + BD992_correct_N.^2);
temp = unique(BD992_rms_U(BD992Section(TrimbleMatch)));
temp2 = 0;
for i = 1:1:size(unique(BD992_rms_U(BD992Section(TrimbleMatch))))
    temp2(i+1) = temp2(i) + size(BD992_rms_U(BD992_rms_U(BD992Section(TrimbleMatch)) == temp(i)),1);
end
temp2(1) = [];
temp2 = temp2/max(temp2)*100;
plot(temp2(1:(size(temp,1))), temp,'r')
BD992_correct_U(1,:) = interp1(temp2,temp,[50 68 95 98]);
legend('BD992 I E','BD992 I N','BD992 I U','Location','northwest')
xlabel('Bins [%]')
ylabel('Error [mm]')
hA = gca;
resetplotview(hA,'InitializeCurrentView');
set(hA,'ylim',[0 100]);


%% Data display

figure('Outerposition',[0 2*Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on; 
title('North(Y) - East(X) Baseline');
plot((Piksi_E(PiksiSection(PiksiMatch)) -xEast)/1000, (Piksi_N(PiksiSection(PiksiMatch))-yNorth)/1000,'rx');
plot((BD982_E(BD982Section(TrimbleMatch))-xEast)/1000, (BD982_N(BD982Section(TrimbleMatch))-yNorth)/1000,'bo');
plot((BD992_E(BD992Section(TrimbleMatch))-xEast)/1000, (BD992_N(BD992Section(TrimbleMatch))-yNorth)/1000,'g*');
plot(0,0,'kx','Markersize', 10);
legend('Piksi 3', 'BD 982', 'BD 992', 'Ref')
xlabel('X [m]')
ylabel('Y [m]')


% figure('Outerposition',[0 Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on; 
% title('N / E');
% plot((Piksi_E-xEast)/1000, (Piksi_N -yNorth)/1000,'rx');
% 
% figure('Outerposition',[Screen(3)/4 Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on; 
% title('N / E');
% plot((BD982_E-xEast)/1000, (BD982_N-yNorth)/1000,'bo');
% 
% figure('Outerposition',[2*Screen(3)/4 Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on; 
% title('N / E');
% plot((BD992_E(BD992Section(TrimbleMatch))-xEast)/1000, (BD992_N(BD992Section(TrimbleMatch))-yNorth)/1000,'g*');

% plot(xEast,yNorth,'kx','Markersize', 20);
CORR_E(1) = corr(Piksi_E(PiksiSection(PiksiMatch)),BD982_E(BD982Section(TrimbleMatch)));
CORR_E(2) = corr(Piksi_E(PiksiSection(PiksiMatch)),BD992_E(BD992Section(TrimbleMatch)));
CORR_E(3) = corr(BD982_E(BD982Section(TrimbleMatch)),BD992_E(BD992Section(TrimbleMatch)));
CORR_E

CORR_N(1) = corr(Piksi_N(PiksiSection(PiksiMatch)),BD982_N(BD982Section(TrimbleMatch)));
CORR_N(2) = corr(Piksi_N(PiksiSection(PiksiMatch)),BD992_N(BD992Section(TrimbleMatch)));
CORR_N(3) = corr(BD982_N(BD982Section(TrimbleMatch)),BD992_N(BD992Section(TrimbleMatch)));
CORR_N


% figure('Outerposition',[Screen(3)/4 2*Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on; 
% title('Up');
% plot(TimeMatch, Piksi_U(PiksiSection(PiksiMatch)),'rx');
% plot(TimeMatch, BD982_U(BD982Section(TrimbleMatch)),'bo');
% plot(TimeMatch, BD992_U(BD992Section(TrimbleMatch)),'g*');
% % plot(TimeMatch, zUp*ones(size(TimeMatch)),'k-')



   
  


% pause(30);
% figure(1); savefig('Flags');
% figure(2); savefig('ENU'); pause(10);
% figure(3); savefig('Up'); pause(10);
% figure(4); savefig('Height'); pause(10);
% figure(5); savefig('Heading_Time'); pause(10);
% figure(6); savefig('Heading_Hist'); pause(10);
% figure(7); savefig('RMS_theory'); pause(10);
% figure(8); savefig('RMS_EN'); pause(10);
% figure(9); savefig('RMS_Up_Height'); pause(10);
% figure(10); savefig('Hist_3D_Piksi'); pause(10);
% figure(11); savefig('Hist_3D_BD982'); pause(10);
% figure(12); savefig('Hist_3D_BD992'); pause(10);




% [PiksiTime, PiksiSection, PiksiHSection] = intersect(Piksi_Time(Flags.Piksi.Float), PiksiH_Time);
% [TrimbleTime, BD982Section, BD992Section] = intersect(BD982_Time(Flags.BD982.Float), BD992_Time(Flags.BD992.Float));
% [TimeMatch, PiksiMatch, TrimbleMatch] = intersect(PiksiTime, TrimbleTime);
% Samples(2,1) = size(Piksi_Time(Flags.Piksi.Float),1);
% Samples(2,3) = size(BD982_Time(Flags.BD982.Float),1);
% Samples(2,4) = size(BD992_Time(Flags.BD992.Float),1);
% RTK_Ratio(2,1) = size(Piksi_Time(Flags.Piksi.Float))/size(Piksi_Time);
% RTK_Ratio(2,2) = size(BD982_Time(Flags.BD982.Float))/size(BD982_Time);
% RTK_Ratio(2,3) = size(BD992_Time(Flags.BD992.Float))/size(BD992_Time);


% figure(7); hold on; 
% plot((Piksi_E(PiksiSection(PiksiMatch))-xEast)/1000, (Piksi_N(PiksiSection(PiksiMatch))-yNorth)/1000,'rx');
% plot((BD982_E(BD982Section(TrimbleMatch))-xEast)/1000, (BD982_N(BD982Section(TrimbleMatch))-yNorth)/1000,'bo');
% plot((BD992_E(BD992Section(TrimbleMatch))-xEast)/1000, (BD992_N(BD992Section(TrimbleMatch))-yNorth)/1000,'g*');

% figure(4); hold on; 
% title('N / E');
% plot((Piksi_E(PiksiSection(PiksiMatch))-xEast)/1000, (Piksi_N(PiksiSection(PiksiMatch))-yNorth)/1000,'mx');
%  
% figure(5); hold on; 
% title('N / E');
% plot((BD982_E(BD982Section(TrimbleMatch))-xEast)/1000, (BD982_N(BD982Section(TrimbleMatch))-yNorth)/1000,'co');
% 
% figure(6); hold on; 
% title('N / E');
% plot((BD992_E(BD992Section(TrimbleMatch))-xEast)/1000, (BD992_N(BD992Section(TrimbleMatch))-yNorth)/1000,'k*');
% 
% plot(xEast,yNorth,'kx','Markersize', 20);
% CORR_E(1) = corr(Piksi_E(PiksiSection(PiksiMatch)),BD982_E(BD982Section(TrimbleMatch)));
% CORR_E(2) = corr(Piksi_E(PiksiSection(PiksiMatch)),BD992_E(BD992Section(TrimbleMatch)));
% CORR_E(3) = corr(BD982_E(BD982Section(TrimbleMatch)),BD992_E(BD992Section(TrimbleMatch)));
% CORR_E
% 
% CORR_N(1) = corr(Piksi_N(PiksiSection(PiksiMatch)),BD982_N(BD982Section(TrimbleMatch)));
% CORR_N(2) = corr(Piksi_N(PiksiSection(PiksiMatch)),BD992_N(BD992Section(TrimbleMatch)));
% CORR_N(3) = corr(BD982_N(BD982Section(TrimbleMatch)),BD992_N(BD992Section(TrimbleMatch)));
% CORR_N


% figure('Outerposition',[Screen(3)/4 2*Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on; 
% title('Up');
% plot(TimeMatch, Piksi_U(PiksiSection(PiksiMatch)),'rx');
% plot(TimeMatch, BD982_U(BD982Section(TrimbleMatch)),'bo');
% plot(TimeMatch, BD992_U(BD992Section(TrimbleMatch)),'g*');
% % plot(TimeMatch, zUp*ones(size(TimeMatch)),'k-')

% figure(8); hold on; 
% plot((TimeMatch-min(TimeMatch))/60000, Piksi_Height(PiksiSection(PiksiMatch)),'rx');
% plot((TimeMatch-min(TimeMatch))/60000, BD982_Height(BD982Section(TrimbleMatch)),'bo');
% plot((TimeMatch-min(TimeMatch))/60000, BD992_Height(BD992Section(TrimbleMatch)),'g*');
% plot(TimeMatch,Rover(3)*ones(size(TimeMatch)),'k-')
% CORR_U(1) = corr(Piksi_Height(PiksiSection(PiksiMatch)),BD982_Height(BD982Section(TrimbleMatch)));
% CORR_U(2) = corr(Piksi_Height(PiksiSection(PiksiMatch)),BD992_Height(BD982Section(TrimbleMatch)));
% CORR_U(3) = corr(BD982_Height(BD982Section(TrimbleMatch)),BD992_Height(BD982Section(TrimbleMatch)));
% CORR_U

% figure(8); hold on; 
% title('Heading in time');
% plot(TimeMatch,PiksiH_Heading(PiksiHSection(PiksiMatch)),'mx');
% plot(TimeMatch,BD982_Heading(BD982Section(TrimbleMatch)),'co');
% plot(TimeMatch,BD992_Heading(BD992Section(TrimbleMatch)),'k*');
% hA = gca;
% resetplotview(hA,'InitializeCurrentView');
% set(hA,'ylim',[333.5 335.5]);
% CORR_H(1) = corr(PiksiH_Heading(PiksiHSection(PiksiMatch)),BD982_Heading(BD982Section(TrimbleMatch)));
% CORR_H(2) = corr(PiksiH_Heading(PiksiHSection(PiksiMatch)),BD992_Heading(BD982Section(TrimbleMatch)));
% CORR_H(3) = corr(BD982_Heading(BD982Section(TrimbleMatch)),BD992_Heading(BD982Section(TrimbleMatch)));
% CORR_H

pause(5);
dlmwrite('Ex6/Stats.csv', Samples)
dlmwrite('Ex6/Stats.csv',minutes(Measurement_Lenght),'-append')
dlmwrite('Ex6/Stats.csv',RTK_Ratio,'-append')
dlmwrite('Ex6/Stats.csv',Piksi_correct_H,'-append')
dlmwrite('Ex6/Stats.csv',Piksi_correct_V,'-append')
dlmwrite('Ex6/Stats.csv',BD982_correct_H,'-append')
dlmwrite('Ex6/Stats.csv',BD982_correct_U,'-append')
dlmwrite('Ex6/Stats.csv',BD992_correct_H,'-append')
dlmwrite('Ex6/Stats.csv',BD992_correct_U,'-append')
dlmwrite('Ex6/Stats.csv',CORR_E,'-append')
dlmwrite('Ex6/Stats.csv',CORR_N,'-append')
dlmwrite('Ex6/Stats.csv',CORR_U,'-append')
dlmwrite('Ex6/Stats.csv',CORR_H,'-append')

figure(1); savefig('Ex6/Flags');
saveas(figure(1),'Ex6/Flags.png'); 
figure(2); savefig('Ex6/Heading_Time');
saveas(figure(2),'Ex6/Heading_Time.png'); 
figure(3); savefig('Ex6/EN_T'); 
saveas(figure(3),'Ex6/EN_T.png');
figure(4); savefig('Ex6/RMS_Piksi'); 
saveas(figure(4),'Ex6/RMS_Piksi.png');
figure(5); savefig('Ex6/RMS_BD982'); 
saveas(figure(5),'Ex6/RMS_BD982.png'); 
figure(6); savefig('Ex6/RMS_BD992'); 
saveas(figure(6),'Ex6/RMS_BD992.png');
figure(7); savefig('Ex6/EN_B');
saveas(figure(7),'Ex6/EN_B.png');
figure(8); savefig('Ex6/Height');
saveas(figure(8),'Ex6/Height.png');
