%% 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.227132525 16.575194199 355.772]; % 1.5 2020 
Rover =  [49.227132528 16.575194204 355.772]; % 5. 3. 2020  ID1
% 17.5 2020 === [49.227132531, 16.575194211, 355.772];




% TUBO = 49 12 21.21002 16 35 34.20403 324.274  
Base =  [49.205896566, 16.592843063, 324.285]; %  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);
Piksi_Time = Piksi.data(:,2);
Piksi_Lat = Piksi.data(:,3);
Piksi_Lon = Piksi.data(:,4);
Piksi_Height = Piksi.data(:,5);
Piksi_rms_H = Piksi.data(:,6);
Piksi_rms_V = Piksi.data(:,7);
Piksi_N = Piksi.data(:,8);
Piksi_E = Piksi.data(:,9);
% Piksi_U = -Piksi.data(:,8);
% Piksi_U = -(Piksi.data(:,8) + 126.74); %piksi default 
% Piksi_U = (-Piksi.data(:,10)) + 19; %trimble phase center
Piksi_U = (-Piksi.data(:,10)); %trimble phase center
Piksi_GDOP = Piksi.data(:,16);
Piksi_HDOP = Piksi.data(:,17);
Piksi_PDOP = Piksi.data(:,18);
% Piksi_TDOP = Piksi.data(:,19);
% Piksi_VDOP = Piksi.data(:,20);
Piksi_Flags = Piksi.data(:,19);
figure('Outerposition',[3*Screen(3)/4 0 Screen(3)/4 Screen(4)/3]); hold on; 
ax1 = subplot(4,1,1);
plot(Piksi_Time,Piksi_Flags)
title('Flags in time');
ax1.YTick = [1,3,5];
ax1.XTick = [];
Samples(1) = size(Piksi_Time,1);

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

Piksi = importdata(file2,';',2);
PiksiH_Time = Piksi.data(:,2);
PiksiH_Flags = Piksi.data(:,19);
PiksiH_Heading = Piksi.data(:,20);
ax2 = subplot(4,1,2);
plot(PiksiH_Time,PiksiH_Flags)
ax2.YTick = [1,3,5];
ax2.XTick = [];

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

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

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

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


BD992 = importdata(file3,',',2);
BD992_Time = BD992.data(:,1);
BD992_E = round(BD992.data(:,2),3)*1000;
BD992_N = round(BD992.data(:,3),3)*1000;
BD992_U = round(BD992.data(:,4),3)*1000;
BD992_rms_E = BD992.data(:,5)*1000;
BD992_rms_N = BD992.data(:,6)*1000;
BD992_rms_U = BD992.data(:,7)*1000;
BD992_Heading = rad2deg(BD992.data(:,10));
BD992_Lat = BD992.data(:,14);
BD992_Lon = BD992.data(:,15);
BD992_Height = BD992.data(:,16);
BD992_Flags = BD992.data(:,21);
BD992_rms_H = BD992.data(:,23);
ax4 = subplot(4,1,4);
plot((BD992_Time-min(BD992_Time))/3600000,BD992_Flags)
xlabel('Time [Hours]')
ax4.YTick = [1,3,5];

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_E(BD982_N>2362100) = [];
BD982_Time(BD982_N>2362100) = [];
BD982_Flags(BD982_N>2362100) = [];
BD982_U(BD982_N>2362100) = [];
BD982_Heading(BD982_N>2362100) = [];
BD982_Height(BD982_N>2362100) = [];
BD982_N(BD982_N>2362100) = [];

BD982_E(BD982_N<2361800) = [];
BD982_Time(BD982_N<2361800) = [];
BD982_Flags(BD982_N<2361800) = [];
BD982_U(BD982_N<2361800) = [];
BD982_Heading(BD982_N<2361800) = [];
BD982_Height(BD982_N<2361800) = [];
BD982_N(BD982_N<2361800) = [];


BD982_N(BD982_E<-1285600) = [];
BD982_Time(BD982_E<-1285600) = [];
BD982_Flags(BD982_E<-1285600) = [];
BD982_U(BD982_E<-1285600) = [];
BD982_Heading(BD982_E<-1285600) = [];
BD982_Height(BD982_E<-1285600) = [];
BD982_E(BD982_E<-1285600) = [];

BD982_N(BD982_E>-1285500) = [];
BD982_Time(BD982_E>-1285500) = [];
BD982_Flags(BD982_E>-1285500) = [];
BD982_U(BD982_E>-1285500) = [];
BD982_Heading(BD982_E>-1285500) = [];
BD982_Height(BD982_E>-1285500) = [];
BD982_E(BD982_E>-1285500) = [];



% BD982_N(BD982_U<30640) = [];
% BD982_E(BD982_U<30640) = [];
% BD982_Time(BD982_U<30640) = [];
% BD982_Flags(BD982_U<30640) = [];
% BD982_U(BD982_U<30640) = [];

%% 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_E(BD992_N<2361800) = [];
BD992_Time(BD992_N<2361800) = [];
BD992_Flags(BD992_N<2361800) = [];
BD992_U(BD992_N<2361800) = [];
BD992_Heading(BD992_N<2361800) = [];
BD992_Height(BD992_N<2361800) = [];
BD992_N(BD992_N<2361800) = [];

BD992_E(BD992_N>2362100) = [];
BD992_Time(BD992_N>2362100) = [];
BD992_Flags(BD992_N>2362100) = [];
BD992_U(BD992_N>2362100) = [];
BD992_Heading(BD992_N>2362100) = [];
BD992_Height(BD992_N>2362100) = [];
BD992_N(BD992_N>2362100) = [];

BD992_N(BD992_E<-1285600) = [];
BD992_Time(BD992_E<-1285600) = [];
BD992_Flags(BD992_E<-1285600) = [];
BD992_Heading(BD992_E<-1285600) = [];
BD992_U(BD992_E<-1285600) = [];
BD992_Height(BD992_E<-1285600) = [];
BD992_E(BD992_E<-1285600) = [];

BD992_N(BD992_E>-1285500) = [];
BD992_Time(BD992_E>-1285500) = [];
BD992_Flags(BD992_E>-1285500) = [];
BD992_Heading(BD992_E>-1285500) = [];
BD992_U(BD992_E>-1285500) = [];
BD992_Height(BD992_E>-1285500) = [];
BD992_E(BD992_E>-1285500) = [];


% BD992_N(BD992_U<30640) = [];
% BD992_E(BD992_U<30640) = [];
% BD992_Time(BD992_U<30640) = [];
% BD992_Flags(BD992_U<30640) = [];
% BD992_U(BD992_U<30640) = [];


%% 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) = [];

Piksi_E(Piksi_N<2361800) = [];
Piksi_Time(Piksi_N<2361800) = [];
Piksi_Flags(Piksi_N<2361800) = [];
Piksi_U(Piksi_N<2361800) = [];
Piksi_N(Piksi_N<2361800) = [];

Piksi_E(Piksi_N>2362100) = [];
Piksi_Time(Piksi_N>2362100) = [];
Piksi_Flags(Piksi_N>2362100) = [];
Piksi_U(Piksi_N>2362100) = [];
Piksi_N(Piksi_N>2362100) = [];

Piksi_Time(Piksi_E>-1285500) = [];
Piksi_Flags(Piksi_E>-1285500) = [];
Piksi_U(Piksi_E>-1285500) = [];
Piksi_N(Piksi_E>-1285500) = [];
Piksi_E(Piksi_E>-1285500) = [];

Piksi_Time(Piksi_E<-1285600) = [];
Piksi_Flags(Piksi_E<-1285600) = [];
Piksi_U(Piksi_E<-1285600) = [];
Piksi_N(Piksi_E<-1285600) = [];
Piksi_E(Piksi_E<-1285600) = [];

% Piksi_N(Piksi_U<30640) = [];
% Piksi_E(Piksi_U<30640) = [];
% Piksi_Time(Piksi_U<30640) = [];
% Piksi_Flags(Piksi_U<30640) = [];
% Piksi_U(Piksi_U<30640) = [];
%


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(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);
TimeMatch = TimeMatch - min(TimeMatch);
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);
Heading = 334.6;
% figure('Outerposition',[3*Screen(3)/4 2*Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on;
% plot(ref0_2020(2),ref0_2020(1),'ro');
% plot(Ref1_2020(2),Ref1_2020(1),'bx');
% plot(Ref1_2013(2),Ref1_2013(1),'gx');
% plot(Ref6_2020(2),Ref6_2020(1),'bx');
% plot(Ref6_2013(2),Ref6_2013(1),'gx');
% plot(Ref1_2020(2) - (Ref1_2020(2)-Ref1_2013(2))*4.7,Ref1_2020(1) - (Ref1_2020(1)-Ref1_2013(1))*5.7, 'go')
Samples(1,5) = size(TimeMatch,1);
Samples;
Measurement_Length = duration((max(TimeMatch) - min(TimeMatch))/(1000*60*60),0,0);
RTK_Ratio;

Piksi_RMS_s = 1.4 * Piksi_HDOP; % Kaplan : Understanding gnss error table
Piksi_RMS_m = 7.1 * Piksi_HDOP; % Kaplan : Understanding gnss error table
% Piksi_RMS_m = 17.2 * Piksi_HDOP; %Piksi RMS pairing value

% meanRMSc = mean(Piksi_RMS_s);
% medianRMSc = median(Piksi_RMS_s);
% modeRMSc = mode(Piksi_RMS_s);
% meanRMSe = mean(Piksi_RMS_m);
% modeRMSe = mode(Piksi_RMS_m);
% medianRMSe = median(Piksi_RMS_m);
% 
% PiksiE_std = std(Piksi_E(PiksiSection(PiksiMatch)));
% PiksiN_std = std(Piksi_N(PiksiSection(PiksiMatch)));
% Bd982E_std = std(BD982_E(BD982Section(TrimbleMatch)));
% Bd982N_std = std(BD982_N(BD982Section(TrimbleMatch)));
% Heading_corr = corr(PiksiH_Heading(PiksiHSection(PiksiMatch)),BD982_Heading(BD982Section(TrimbleMatch)));
% Piksi_NE_corr = corr(Piksi_E(PiksiSection(PiksiMatch)), Piksi_N(PiksiSection(PiksiMatch)));
% BD982_NE_corr = corr(BD982_E(BD982Section(TrimbleMatch)), BD982_N(BD982Section(TrimbleMatch)));
% Up_corr = corr(Piksi_U(PiksiSection(PiksiMatch)), BD982_U(BD982Section(TrimbleMatch)));
% 
% Heading_cov = cov(PiksiH_Heading(PiksiHSection(PiksiMatch)),BD982_Heading(BD982Section(TrimbleMatch)));
% Piksi_cov = cov(Piksi_E(PiksiSection(PiksiMatch)), Piksi_N(PiksiSection(PiksiMatch)));
% BD982_cov = cov(BD982_E(BD982Section(TrimbleMatch)), BD982_N(BD982Section(TrimbleMatch)));
% Up_cov = cov(Piksi_U(PiksiSection(PiksiMatch)), BD982_U(BD982Section(TrimbleMatch)));
% 
% 
% Timing.data_statistics_time = cputime - time;
% time = cputime;
Piksi_E_rms = zeros(size(TimeMatch,1),1);
Piksi_N_rms = zeros(size(TimeMatch,1),1);
Piksi_U_rms = zeros(size(TimeMatch,1),1);
BD982_E_rms = zeros(size(TimeMatch,1),1);
BD982_N_rms = zeros(size(TimeMatch,1),1);
BD982_U_rms = zeros(size(TimeMatch,1),1);
BD992_E_rms = zeros(size(TimeMatch,1),1);
BD992_N_rms = zeros(size(TimeMatch,1),1);
BD992_U_rms = zeros(size(TimeMatch,1),1);
Piksi_Height_rms = zeros(size(TimeMatch,1),1);
BD982_Height_rms = zeros(size(TimeMatch,1),1);
BD992_Height_rms = zeros(size(TimeMatch,1),1);
for i = 1:1:size(PiksiMatch)
    Piksi_E_rms(i) = rms(Piksi_E(PiksiSection(PiksiMatch(i)))-xEast);
    Piksi_N_rms(i) = rms(Piksi_N(PiksiSection(PiksiMatch(i)))-yNorth);
    Piksi_U_rms(i) = rms(Piksi_U(PiksiSection(PiksiMatch(i)))-zUp);
    BD982_E_rms(i) = rms(BD982_E(BD982Section(TrimbleMatch(i)))-xEast);
    BD982_N_rms(i) = rms(BD982_N(BD982Section(TrimbleMatch(i)))-yNorth);
    BD982_U_rms(i) = rms(BD982_U(BD982Section(TrimbleMatch(i)))-zUp);
    BD992_E_rms(i) = rms(BD992_E(BD992Section(TrimbleMatch(i)))-xEast);
    BD992_N_rms(i) = rms(BD992_N(BD992Section(TrimbleMatch(i)))-yNorth);
    BD992_U_rms(i) = rms(BD992_U(BD992Section(TrimbleMatch(i)))-zUp);
    
    Piksi_Height_rms(i) = rms(Piksi_Height(PiksiSection(PiksiMatch(i)))-Rover(3))*1000;
    BD982_Height_rms(i) = rms(BD982_Height(BD982Section(TrimbleMatch(i)))-Rover(3))*1000;
    BD992_Height_rms(i) = rms(BD992_Height(BD992Section(TrimbleMatch(i)))-Rover(3))*1000;
end

temp = unique(Piksi_rms_H(PiksiSection(PiksiMatch)));
for i = 1:1:size(temp,1)
    RMS_Intersect = ismember(Piksi_rms_H(PiksiSection(PiksiMatch)), temp(i));
    RMS_E_Piksi(i) = rms(Piksi_E(PiksiSection(PiksiMatch(RMS_Intersect)))-xEast);
    RMS_N_Piksi(i) = rms(Piksi_N(PiksiSection(PiksiMatch(RMS_Intersect)))-yNorth);
    drms_piksi(i) = sqrt(RMS_E_Piksi(i)^2 + RMS_N_Piksi(i)^2);
end
temp = unique(Piksi_rms_V(PiksiSection(PiksiMatch)));
for i = 1:1:size(temp,1)
    RMS_Intersect = ismember(Piksi_rms_V(PiksiSection(PiksiMatch)), temp(i));
    RMS_U_Piksi(i) = rms(Piksi_Height(PiksiSection(PiksiMatch(RMS_Intersect)))-mean(Piksi_Height))*1000;
end

temp = unique(BD982_rms_E(BD982Section(TrimbleMatch)));
for i = 1:1:size(temp,1)
    RMS_Intersect = ismember(BD982_rms_E(BD982Section(TrimbleMatch)), temp(i));
    RMS_E_BD982(i) = rms(BD982_E(BD982Section(TrimbleMatch(RMS_Intersect)))-xEast);
end
temp = unique(BD982_rms_N(BD982Section(TrimbleMatch)));
for i = 1:1:size(temp,1)
    RMS_Intersect = ismember(BD982_rms_N(BD982Section(TrimbleMatch)), temp(i));
    RMS_N_BD982(i) = rms(BD982_N(BD982Section(TrimbleMatch(RMS_Intersect)))-yNorth);
end
temp = unique(BD982_rms_U(BD982Section(TrimbleMatch)));
for i = 1:1:size(temp,1)
    RMS_Intersect = ismember(BD982_rms_U(BD982Section(TrimbleMatch)), temp(i));
    RMS_U_BD982(i) = rms(BD982_Height(BD982Section(TrimbleMatch(RMS_Intersect)))-mean(BD982_Height))*1000;
end

temp = unique(BD992_rms_E(BD992Section(TrimbleMatch)));
for i = 1:1:size(temp,1)
    RMS_Intersect = ismember(BD992_rms_E(BD992Section(TrimbleMatch)), temp(i));
    RMS_E_BD992(i) = rms(BD992_E(BD992Section(TrimbleMatch(RMS_Intersect)))-xEast);
end
temp = unique(BD992_rms_N(BD992Section(TrimbleMatch)));
for i = 1:1:size(temp,1)
    RMS_Intersect = ismember(BD992_rms_N(BD992Section(TrimbleMatch)), temp(i));
    RMS_N_BD992(i) = rms(BD992_N(BD992Section(TrimbleMatch(RMS_Intersect)))-yNorth);
end
temp = unique(BD992_rms_U(BD992Section(TrimbleMatch)));
for i = 1:1:size(temp,1)
    RMS_Intersect = ismember(BD992_rms_U(BD992Section(TrimbleMatch)), temp(i));
    RMS_U_BD992(i) = rms(BD992_Height(BD992Section(TrimbleMatch(RMS_Intersect)))-mean(BD992_Height))*1000;
end
temp = unique(BD992_Heading(BD992Section(TrimbleMatch)));
for i = 1:1:size(temp,1)
    RMS_Intersect = ismember(BD992_Heading(BD992Section(TrimbleMatch)), temp(i));
    RMS_H_BD992(i) = rms(BD992_Heading(BD992Section(TrimbleMatch(RMS_Intersect)))-Heading);
end
temp = unique(BD982_Heading(BD982Section(TrimbleMatch)));
for i = 1:1:size(temp,1)
    RMS_Intersect = ismember(BD982_Heading(BD982Section(TrimbleMatch)), temp(i));
    RMS_H_BD982(i) = rms(BD982_Heading(BD982Section(TrimbleMatch(RMS_Intersect)))-Heading);
end
temp = unique(PiksiH_Heading(PiksiHSection(PiksiMatch)));
for i = 1:1:size(temp,1)
    RMS_Intersect = ismember(PiksiH_Heading(PiksiHSection(PiksiMatch)), temp(i));
    RMS_H_Piksi(i) = rms(PiksiH_Heading(PiksiHSection(PiksiMatch(RMS_Intersect)))-Heading);
end
% 
% RMS_E_BD992 = RMS_E_BD992';
% RMS_N_BD992 = RMS_N_BD992';
% RMS_U_BD992 = RMS_U_BD992';
% RMS_E_BD982 = RMS_E_BD982';
% RMS_N_BD982 = RMS_N_BD982';
% RMS_U_BD982 = RMS_U_BD982';
% RMS_U_Piksi = RMS_U_Piksi';
% RMS_E_Piksi = RMS_E_Piksi';
% RMS_N_Piksi = RMS_N_Piksi';
% drms_piksi =  drms_piksi';
%% Data display
% left bottom width height
% figure('Outerposition',[Screen(3)/4 2*Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on; 
% title('Lon / Lat');
% plot(Piksi_Lon(PiksiSection(PiksiMatch)), Piksi_Lat(PiksiSection(PiksiMatch)),'rx');
% plot(BD982_Lon(BD982Section(TrimbleMatch)), BD982_Lat(BD982Section(TrimbleMatch)),'bo');
% plot(BD992_Lon(BD992Section(TrimbleMatch)), BD992_Lat(BD992Section(TrimbleMatch)),'g*');
% plot(Ref1_2013(2),Ref1_2013(1),'kx','Markersize', 12)
% plot(Lon, Lat, 'ko','Markersize', 12)
% plot(mean(Piksi_Lon), mean(Piksi_Lat), 'co','Markersize', 8)
% plot(mean(BD982_Lon), mean(BD982_Lat), 'mo','Markersize', 8)
% plot(mean(BD992_Lon), mean(BD992_Lat), 'ko','Markersize', 8)
% plot(Ref1_2020(2) - (Ref1_2020(2)-Ref1_2013(2))*4.7,Ref1_2020(1) - (Ref1_2020(1)-Ref1_2013(1))*5.7,'kx','Markersize', 12)


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, Piksi_N(PiksiSection(PiksiMatch))-yNorth,'rx');
plot(BD982_E(BD982Section(TrimbleMatch))-xEast, BD982_N(BD982Section(TrimbleMatch))-yNorth,'bo');
plot(BD992_E(BD992Section(TrimbleMatch))-xEast, BD992_N(BD992Section(TrimbleMatch))-yNorth,'g*');
% plot(xEast,yNorth,'kx','Markersize', 20);
plot(0,0,'kx','Markersize', 20);
legend('Piksi 3', 'BD 982', 'BD 992', 'Ref')
xlabel('X [mm]')
ylabel('Y [mm]')

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

figure('Outerposition',[2*Screen(3)/4 2*Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on; 
title('Up Baseline');
plot(TimeMatch/3600000, Piksi_U(PiksiSection(PiksiMatch)),'r');
plot(TimeMatch/3600000, BD982_U(BD982Section(TrimbleMatch)),'b');
plot(TimeMatch/3600000, BD992_U(BD992Section(TrimbleMatch)),'g');
plot(TimeMatch/3600000, zUp*ones(size(TimeMatch)),'k-')
legend('Piksi 3', 'BD 982', 'BD 992', 'Ref')
xlabel('Time [Hours]')
ylabel('Up [mm]')

figure('Outerposition',[2*Screen(3)/4 2*Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on; 
title('Height');
plot(TimeMatch/3600000, Piksi_Height(PiksiSection(PiksiMatch)),'r');
plot(TimeMatch/3600000, BD982_Height(BD982Section(TrimbleMatch)),'b');
plot(TimeMatch/3600000, BD992_Height(BD992Section(TrimbleMatch)),'g');
plot(TimeMatch/3600000,Rover(3)*ones(size(TimeMatch)),'k-')
legend('Piksi 3', 'BD 982', 'BD 992', 'Ref')
xlabel('Time [Hours]')
ylabel('Height [m]')
hA = gca;
resetplotview(hA,'InitializeCurrentView');
set(hA,'ylim',[355.5 356]);

figure('Outerposition',[3*Screen(3)/4 2*Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on; 
title('Heading in time');
plot(TimeMatch/3600000,PiksiH_Heading(PiksiHSection(PiksiMatch)),'r');
plot(TimeMatch/3600000,BD982_Heading(BD982Section(TrimbleMatch)),'b');
plot(TimeMatch/3600000,BD992_Heading(BD992Section(TrimbleMatch)),'g');
hA = gca;
resetplotview(hA,'InitializeCurrentView');
set(hA,'ylim',[333.5 335.5]);
legend('Piksi 3', 'BD 982', 'BD 992')
xlabel('Time [Hours]')
ylabel('Heading [degrees]')


figure('Outerposition',[3*Screen(3)/4 Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on; 
title('Heading histogram');
histogram(PiksiH_Heading(PiksiHSection(PiksiMatch)),unique(BD982_Heading(BD982Section(TrimbleMatch))),'FaceColor','r');
histogram(BD982_Heading(BD982Section(TrimbleMatch)),unique(BD982_Heading(BD982Section(TrimbleMatch))),'FaceColor','b');
histogram(BD992_Heading(BD992Section(TrimbleMatch)),unique(BD992_Heading(BD992Section(TrimbleMatch))),'FaceColor','g');
legend('Piksi 3', 'BD 982', 'BD 992')
xlabel('Heading [degrees]')
ylabel('Count [-]')


% figure('Outerposition',[0 0 Screen(3)/4 Screen(4)/3]); hold on; 
% title('Piksi RMS distribution');
% histogram(Piksi_rms_H(Piksi_rms_H(PiksiSection(PiksiMatch))<20),unique(Piksi_rms_H(Piksi_rms_H(PiksiSection(PiksiMatch))<20)),'FaceColor','r','FaceAlpha',0.5);
% histogram(Piksi_rms_V(Piksi_rms_V(PiksiSection(PiksiMatch))<40),unique(Piksi_rms_V(Piksi_rms_V(PiksiSection(PiksiMatch))<40)),'FaceColor','b','FaceAlpha',0.5);
% % histogram(Piksi_RMS_s(Piksi_RMS_s<20),unique(Piksi_rms_H(Piksi_rms_H<20)),'FaceColor','b','FaceAlpha',0.5); % too accurate
% histogram(Piksi_RMS_m(Piksi_RMS_m(PiksiSection(PiksiMatch))<20),unique(Piksi_RMS_m(Piksi_RMS_m(PiksiSection(PiksiMatch))<30)),'FaceColor','g','FaceAlpha',0.1);
% histogram(Piksi_RMS_m(Piksi_RMS_m(PiksiSection(PiksiMatch))<20),unique(Piksi_rms_H(Piksi_rms_H(PiksiSection(PiksiMatch))<20)),'FaceColor','g','FaceAlpha',0.5);
% % histogram(Piksi_RMS_s(Piksi_RMS_s == mode(Piksi_RMS_s)),unique(Piksi_RMS_s),'FaceColor','b'); % too accurate
% % histogram(Piksi_RMS_s(Piksi_RMS_s == median(Piksi_RMS_s)),unique(Piksi_RMS_s),'FaceColor','g'); % too accurate
% legend('RMSi H','RMSi V', 'RMSc Hc 14.2', 'RMSc Hi') %i - internal, c - calculated
% % annotation('textbox',[0.5 0.6 0.1 0.1],'String','HDOP c Coef = 14.2 ','FitBoxToText','on')



% figure('Outerposition',[Screen(3)/4 0 Screen(3)/4 Screen(4)/3]); hold on; 
% title('BD982 N/E/U RMS distribution');
% histogram(BD982_rms_E(BD982_rms_E<20),unique(BD982_rms_E(BD982_rms_E<20)),'FaceColor','r','FaceAlpha',0.5);
% histogram(BD982_rms_N(BD982_rms_N<20),unique(BD982_rms_N(BD982_rms_U<20)),'FaceColor','b','FaceAlpha',0.5);
% histogram(BD982_rms_U(BD982_rms_U<20),unique(BD982_rms_U(BD982_rms_N<20)),'FaceColor','g','FaceAlpha',0.5);
% % histogram(BD982_rms_E(BD982_rms_E == mode(BD982_rms_E)),unique(BD982_rms_E),'FaceColor','b');
% % histogram(BD982_rms_E(BD982_rms_E == median(BD982_rms_E)),unique(BD982_rms_E),'FaceColor','g');
% 
% 

% figure('Outerposition',[2*Screen(3)/4 0 Screen(3)/4 Screen(4)/3]); hold on; 
% title('BD992 N/E/U RMS distribution');
% histogram(BD992_rms_E(BD992_rms_E<20),unique(BD992_rms_E(BD992_rms_E<20)),'FaceColor','r','FaceAlpha',0.5);
% histogram(BD992_rms_N(BD992_rms_N<20),unique(BD992_rms_N(BD992_rms_U<20)),'FaceColor','b','FaceAlpha',0.5);
% histogram(BD992_rms_U(BD992_rms_U<20),unique(BD992_rms_U(BD992_rms_N<20)),'FaceColor','g','FaceAlpha',0.5);
% % histogram(BD992_rms_E(BD992_rms_E == mode(BD992_rms_E)),unique(BD992_rms_E),'FaceColor','b');
% % histogram(BD992_rms_E(BD992_rms_E == median(BD992_rms_E)),unique(BD992_rms_E),'FaceColor','g');
% 

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

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')
plot(temp2(1:(size(temp,1))), drms_piksi,'c')
Piksi_correct_H(1,:) = interp1(temp2,temp,[50 68 95 98]);
Piksi_correct_H(2,:) = interp1(temp2,drms_piksi,[50 68 95 98]);

xlabel('Bins [%]')
ylabel('Error [mm]')


% temp = unique(drms_piksi);
% temp2 = 0;
% for i = 1:1:size(unique(drms_piksi))
%     temp2(i+1) = temp2(i) + size(drms_piksi(drms_piksi == temp(i)),1);
% end
% temp2 = temp2/max(temp2)*100;
% temp2(1) = [];
% plot(temp2(1:(size(temp,1))), temp,'c')

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')
plot(temp2(1:(size(temp,1))), RMS_U_Piksi,'m')
Piksi_correct_V(1,:) = interp1(temp2,temp,[50 68 95 98]);
Piksi_correct_V(2,:) = interp1(temp2,RMS_U_Piksi,[50 68 95 98]);
legend('Piksi 3 I H', 'Piksi 3 C H','Piksi 3 I V', 'Piksi 3 C V')
xlabel('Bins [%]')
ylabel('Error [mm]')
hA = gca;
resetplotview(hA,'InitializeCurrentView');
set(hA,'xlim',[0 99]);

% temp = unique(RMS_U_Piksi);
% temp2 = 0;
% for i = 1:1:size(unique(RMS_U_Piksi))
%     temp2(i+1) = temp2(i) + size(RMS_U_Piksi(RMS_U_Piksi == temp(i)),1);
% end
% temp2 = temp2/max(temp2)*100;
% temp2(1) = [];
% plot(temp2(1:(size(temp,1))), temp,'c')



% temp = unique(RMS_E_Piksi);
% temp2 = 0;
% for i = 1:1:size(unique(RMS_E_Piksi))
%     temp2(i+1) = temp2(i) + size(RMS_E_Piksi(RMS_E_Piksi == temp(i)),1);
% end
% temp2 = temp2/max(temp2)*100;
% plot(temp2(1:(size(temp,1))), temp,'m')
% 
% temp = unique(RMS_N_Piksi);
% temp2 = 0;
% for i = 1:1:size(unique(RMS_N_Piksi))
%     temp2(i+1) = temp2(i) + size(RMS_N_Piksi(RMS_N_Piksi == temp(i)),1);
% end
% temp2 = temp2/max(temp2)*100;
% plot(temp2(1:(size(temp,1))), temp,'k')




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

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')
plot(temp2(1:(size(temp,1))), RMS_E_BD982,'c')
BD982_correct_E(1,:) = interp1(temp2,temp,[50 68 95 98]);
BD982_correct_E(2,:) = interp1(temp2,RMS_E_BD982,[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')
plot(temp2(1:(size(temp,1))), RMS_N_BD982,'m')
BD982_correct_N(1,:) = interp1(temp2,temp,[50 68 95 98]);
BD982_correct_N(2,:) = interp1(temp2,RMS_N_BD982,[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')
plot(temp2(1:(size(temp,1))), RMS_U_BD982,'k')
BD982_correct_U(1,:) = interp1(temp2,temp,[50 68 95 98]);
BD982_correct_U(2,:) = interp1(temp2,RMS_U_BD982,[50 68 95 98]);
legend('BD982 I E','BD982 C E', 'BD982 I N','BD982 C N','BD982 I U','BD982 C U')
xlabel('Bins [%]')
ylabel('Error [mm]')
hA = gca;
resetplotview(hA,'InitializeCurrentView');
set(hA,'xlim',[0 99]);

% temp = unique(RMS_E_BD982);
% temp2 = 0;
% for i = 1:1:size(unique(RMS_E_BD982))
%     size(RMS_E_BD982(RMS_E_BD982 == temp(i)),1)
%     temp2(i+1) = temp2(i) + size(RMS_E_BD982(RMS_E_BD982 == temp(i)),1);
% end
% temp2(1) = [];
% temp2 = temp2/max(temp2)*100;
% plot(temp2(1:(size(temp,1))), temp,'c')
% 
% temp = unique(RMS_N_BD982);
% temp2 = 0;
% for i = 1:1:size(unique(RMS_N_BD982))
%     temp2(i+1) = temp2(i) + size(RMS_N_BD982(RMS_N_BD982 == temp(i)),1);
% end
% temp2(1) = [];
% temp2 = temp2/max(temp2)*100;
% plot(temp2(1:(size(temp,1))), temp,'m')
% 
% temp = unique(RMS_U_BD982);
% temp2 = 0;
% for i = 1:1:size(unique(RMS_U_BD982))
%     temp2(i+1) = temp2(i) + size(RMS_U_BD982(RMS_U_BD982 == temp(i)),1);
% end
% temp2(1) = [];
% temp2 = temp2/max(temp2)*100;
% plot(temp2(1:(size(temp,1))), temp,'k')


figure('Outerposition',[2*Screen(3)/4 Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on
title('RMS Cumulative BD992 Indicated/Calculated')

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')
plot(temp2(1:(size(temp,1))), RMS_E_BD992,'c')
BD992_correct_E(1,:) = interp1(temp2,temp,[50 68 95 98]);
BD992_correct_E(2,:) = interp1(temp2,RMS_E_BD992,[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')
plot(temp2(1:(size(temp,1))), RMS_N_BD992,'m')
BD992_correct_N(1,:) = interp1(temp2,temp,[50 68 95 98]);
BD992_correct_N(2,:) = interp1(temp2,RMS_N_BD992,[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')
plot(temp2(1:(size(temp,1))), RMS_U_BD992,'k')
BD992_correct_U(1,:) = interp1(temp2,temp,[50 68 95 98]);
BD992_correct_U(2,:) = interp1(temp2,RMS_U_BD992,[50 68 95 98]);
legend('BD992 I E','BD992 C E', 'BD992 I N','BD992 C N','BD992 I U','BD992 C U')
xlabel('Bins [%]')
ylabel('Error [mm]')
hA = gca;
resetplotview(hA,'InitializeCurrentView');
set(hA,'xlim',[0 99]);

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

temp = unique(BD982_rms_H(BD982Section(TrimbleMatch)));
temp2 = 0;
for i = 1:1:size(unique(BD982_rms_H(BD982Section(TrimbleMatch))))
    temp2(i+1) = temp2(i) + size(BD982_rms_H(BD982_rms_H(BD982Section(TrimbleMatch)) == temp(i)),1);
end
temp2(1) = [];
temp2 = temp2/max(temp2)*100;
plot(temp2(1:(size(temp,1))), temp,'b')

temp = unique(RMS_H_BD982);
temp2 = 0;
for i = 1:1:size(unique(RMS_H_BD982),2)
    temp2(i+1) = temp2(i) + size(RMS_H_BD982(RMS_H_BD982 == temp(i)),2);
end
temp2(1) = [];
temp2 = temp2/max(temp2)*100;
plot(temp2(1:(size(temp,2))), temp,'c')
% plot(temp2(1:(size(temp,1))), RMS_H_BD982,'c')
% BD982_correct_Head(1,:) = interp1(temp2,temp,[50 68 95 98]);
% BD982_correct_Head(2,:) = interp1(temp2,RMS_H_BD982,[50 68 95 98]);

temp = unique(BD992_rms_H(BD992Section(TrimbleMatch)));
temp2 = 0;
for i = 1:1:size(unique(BD992_rms_H(BD992Section(TrimbleMatch))))
    temp2(i+1) = temp2(i) + size(BD992_rms_H(BD992_rms_H(BD992Section(TrimbleMatch)) == temp(i)),1);
end
temp2(1) = [];
temp2 = temp2/max(temp2)*100;
plot(temp2(1:(size(temp,1))), temp,'g')

temp = unique(RMS_H_BD992);
temp2 = 0;
for i = 1:1:size(unique(RMS_H_BD992),2)
    temp2(i+1) = temp2(i) + size(RMS_H_BD992(RMS_H_BD992 == temp(i)),2);
end
temp2(1) = [];
temp2 = temp2/max(temp2)*100;
plot(temp2(1:(size(temp,2))), temp,'m')
% plot(temp2(1:(size(temp,1))), RMS_H_BD992,'m')
% BD992_correct_Head(1,:) = interp1(temp2,temp,[50 68 95 98]);
% BD992_correct_Head(2,:) = interp1(temp2,RMS_H_BD992,[50 68 95 98]);

temp = unique(RMS_H_Piksi);
temp2 = 0;
for i = 1:1:size(unique(RMS_H_Piksi),2)
    temp2(i+1) = temp2(i) + size(RMS_H_Piksi(RMS_H_Piksi == temp(i)),2);
end
temp2(1) = [];
temp2 = temp2/max(temp2)*100;
plot(temp2(1:(size(temp,2))), temp,'r')
legend('BD982 I Head','BD982 C Head', 'BD992 I Head','BD992 C Head','Piksi 3 C Head')
xlabel('Bins [%]')
ylabel('Error [degrees]')
hA = gca;
resetplotview(hA,'InitializeCurrentView');
set(hA,'xlim',[0 99]);



% temp = unique(RMS_E_BD992);
% temp2 = 0;
% for i = 1:1:size(unique(RMS_E_BD992))
%     temp2(i+1) = temp2(i) + size(RMS_E_BD992(RMS_E_BD992 == temp(i)),1);
% end
% temp2 = temp2/max(temp2)*100;
% temp2(1) = [];
% plot(temp2(1:(size(temp,1))), temp,'c')
% 
% temp = unique(RMS_N_BD992);
% temp2 = 0;
% for i = 1:1:size(unique(RMS_N_BD992))
%     temp2(i+1) = temp2(i) + size(RMS_N_BD992(RMS_N_BD992 == temp(i)),1);
% end
% temp2 = temp2/max(temp2)*100;
% temp2(1) = [];
% plot(temp2(1:(size(temp,1))), temp,'m')
% 
% temp = unique(RMS_U_BD992);
% temp2 = 0;
% for i = 1:1:size(unique(RMS_U_BD992))
%     temp2(i+1) = temp2(i) + size(RMS_U_BD992(RMS_U_BD992 == temp(i)),1);
% end
% temp2 = temp2/max(temp2)*100;
% temp2(1) = [];
% plot(temp2(1:(size(temp,1))), temp,'k')






% figure('Outerposition',[Screen(3)/4 Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on
% title('RMS Cumulative / Reality')
% 
% temp = unique(Piksi_E_rms);
% temp2 = 0;
% for i = 1:1:size(unique(Piksi_E_rms))
%     temp2(i+1) = temp2(i) + size(Piksi_E_rms(Piksi_E_rms == temp(i)),1);
% end
% temp2 = temp2/max(temp2)*100;
% plot(temp2(1:(size(temp,1))), temp,'r')
% 
% temp = unique(Piksi_N_rms);
% temp2 = 0;
% for i = 1:1:size(unique(Piksi_N_rms))
%     temp2(i+1) = temp2(i) + size(Piksi_N_rms(Piksi_N_rms == temp(i)),1);
% end
% temp2 = temp2/max(temp2)*100;
% plot(temp2(1:(size(temp,1))), temp,'r')


% temp = unique(BD982_E_rms);
% temp2 = 0;
% for i = 1:1:size(unique(BD982_E_rms))
%     temp2(i+1) = temp2(i) + size(BD982_E_rms(BD982_E_rms == temp(i)),1);
% end
% temp2 = temp2/max(temp2)*100;
% plot(temp2(1:(size(temp,1))), temp,'b')
% 
% temp = unique(BD982_N_rms);
% temp2 = 0;
% for i = 1:1:size(unique(BD982_N_rms))
%     temp2(i+1) = temp2(i) + size(BD982_N_rms(BD982_N_rms == temp(i)),1);
% end
% temp2 = temp2/max(temp2)*100;
% plot(temp2(1:(size(temp,1))), temp,'b')
% 
% 
% temp = unique(BD992_E_rms);
% temp2 = 0;
% for i = 1:1:size(unique(BD992_E_rms))
%     temp2(i+1) = temp2(i) + size(BD992_E_rms(BD992_E_rms == temp(i)),1);
% end
% temp2 = temp2/max(temp2)*100;
% plot(temp2(1:(size(temp,1))), temp,'g')
% 
% temp = unique(BD992_N_rms);
% temp2 = 0;
% for i = 1:1:size(unique(BD992_N_rms))
%     temp2(i+1) = temp2(i) + size(BD992_N_rms(BD992_N_rms == temp(i)),1);
% end
% temp2 = temp2/max(temp2)*100;
% plot(temp2(1:(size(temp,1))), temp,'g')


% figure('Outerposition',[2*Screen(3)/4 Screen(4)/3 Screen(3)/4 Screen(4)/3]); hold on
% title('RMS Cumulative / Reality - Height')


% 
% temp = unique(BD982_U_rms);
% temp2 = 0;
% for i = 1:1:size(unique(BD982_U_rms))
%     temp2(i+1) = temp2(i) + size(BD982_U_rms(BD982_U_rms == temp(i)),1);
% end
% temp2 = temp2/max(temp2)*100;
% plot(temp2(1:(size(temp,1))), temp,'b')
% 
% temp = unique(BD992_U_rms);
% temp2 = 0;
% for i = 1:1:size(unique(BD992_U_rms))
%     temp2(i+1) = temp2(i) + size(BD992_U_rms(BD992_U_rms == temp(i)),1);
% end
% temp2 = temp2/max(temp2)*100;
% plot(temp2(1:(size(temp,1))), temp,'g')



% temp = unique(BD982_Height_rms);
% temp2 = 0;
% for i = 1:1:size(unique(BD982_Height_rms))
%     temp2(i+1) = temp2(i) + size(BD982_Height_rms(BD982_Height_rms == temp(i)),1);
% end
% temp2 = temp2/max(temp2)*100;
% plot(temp2(1:(size(temp,1))), temp,'b')
% 
% temp = unique(BD992_Height_rms);
% temp2 = 0;
% for i = 1:1:size(unique(BD992_Height_rms))
%     temp2(i+1) = temp2(i) + size(BD992_Height_rms(BD992_Height_rms == temp(i)),1);
% end
% temp2 = temp2/max(temp2)*100;
% plot(temp2(1:(size(temp,1))), temp,'g')


    
  

%% Bivariate histogram
figure('Outerposition',[0 0 Screen(3)/4 Screen(4)/3]); hold on; 
title('N/E histogram Piksi');
hist3([Piksi_E(PiksiSection(PiksiMatch))-xEast Piksi_N(PiksiSection(PiksiMatch))-yNorth],[size(unique(Piksi_E(PiksiSection(PiksiMatch))-xEast),1) size(unique(Piksi_N(PiksiSection(PiksiMatch))-yNorth),1)],'CDataMode','auto','FaceAlpha',.85);
% colorbar;
s = findobj(gca,'Type','Surface');
% s.FaceAlpha = 0.85;
set(gca,'view',[0 90])
plot([0 0], 0, 'rx', 'LineWidth', 2, 'MarkerSize', 30);
% s.FaceAlpha = 1;
xlabel('X [mm]')
ylabel('Y [mm]')
zlabel('Count [-]')

figure('Outerposition',[Screen(3)/4 0 Screen(3)/4 Screen(4)/3]); hold on; title('N/E histogram BD982');
hist3(gca,[BD982_E(BD982Section(TrimbleMatch))-xEast BD982_N(BD982Section(TrimbleMatch))-yNorth],[size(unique(BD982_E(BD982Section(TrimbleMatch))-xEast),1) size(unique(BD982_N(BD982Section(TrimbleMatch))-yNorth),1)],'CDataMode','auto','FaceAlpha',.85);
% colorbar;
s = findobj(gca,'Type','Surface');
% s.FaceAlpha = 0.85;
set(gca,'view',[0 90])
plot([0 0], 0, 'rx', 'LineWidth', 2, 'MarkerSize', 30);
% s.FaceAlpha = 1;
xlabel('X [mm]')
ylabel('Y [mm]')
zlabel('Count [-]')

figure('Outerposition',[2*Screen(3)/4 0 Screen(3)/4 Screen(4)/3]); hold on; title('N/E histogram BD992');
hist3(gca,[BD992_E(BD992Section(TrimbleMatch))-xEast BD992_N(BD992Section(TrimbleMatch))-yNorth],[size(unique(BD992_E(BD992Section(TrimbleMatch))-xEast),1) size(unique(BD992_N(BD992Section(TrimbleMatch))-yNorth),1)],'CDataMode','auto','FaceAlpha',.85);
% colorbar;
s = findobj(gca,'Type','Surface');
% s.FaceAlpha = 0.85;
set(gca,'view',[0 90])
plot([0 0], 0, 'rx', 'LineWidth', 2, 'MarkerSize', 30);
% s.FaceAlpha = 1;
xlabel('X [mm]')
ylabel('Y [mm]')
zlabel('Count [-]')

min_x = min([min(Piksi_E(PiksiSection(PiksiMatch))-xEast) min(BD982_E(BD982Section(TrimbleMatch))-xEast) min(BD992_E(BD992Section(TrimbleMatch))-xEast)]);
max_x = max([max(Piksi_E(PiksiSection(PiksiMatch))-xEast) max(BD982_E(BD982Section(TrimbleMatch))-xEast) max(BD992_E(BD992Section(TrimbleMatch))-xEast)]);
min_y = min([min(Piksi_N(PiksiSection(PiksiMatch))-yNorth) min(BD982_N(BD982Section(TrimbleMatch))-yNorth) min(BD992_N(BD992Section(TrimbleMatch))-yNorth)]);
max_y = max([max(Piksi_N(PiksiSection(PiksiMatch))-yNorth) max(BD982_N(BD982Section(TrimbleMatch))-yNorth) max(BD992_N(BD992Section(TrimbleMatch))-yNorth)]);

figure(12)
hA = gca;
resetplotview(hA,'InitializeCurrentView');
set(hA,'xlim',[min_x max_x], 'ylim', [min_y max_y]);
figure(13)
hA = gca;
resetplotview(hA,'InitializeCurrentView');
set(hA,'xlim',[min_x max_x], 'ylim', [min_y max_y]);
figure(14)
hA = gca;
resetplotview(hA,'InitializeCurrentView');
set(hA,'xlim',[min_x max_x], 'ylim', [min_y max_y]);


pause(10);
dlmwrite('Stats.csv', Samples)
dlmwrite('Stats.csv',hours(Measurement_Length),'-append')
dlmwrite('Stats.csv',RTK_Ratio,'-append')
dlmwrite('Stats.csv',Piksi_correct_H,'-append')
dlmwrite('Stats.csv',Piksi_correct_V,'-append')
dlmwrite('Stats.csv',BD982_correct_H,'-append')
dlmwrite('Stats.csv',BD982_correct_U,'-append')
dlmwrite('Stats.csv',BD992_correct_H,'-append')
dlmwrite('Stats.csv',BD992_correct_U,'-append')
figure(1); savefig('Flags');pause(5);
figure(2); savefig('EN_B'); pause(5);
saveas(figure(2),'EN_B.png'); pause(5);
figure(3); savefig('EN_T'); pause(5);
saveas(figure(3),'EN_T.png'); pause(5);
figure(4); savefig('Up'); pause(5);
saveas(figure(4),'Up.png'); pause(5);
figure(5); savefig('Height'); pause(5);
saveas(figure(5),'Height.png'); pause(5);
figure(6); savefig('Heading_Time'); pause(5);
saveas(figure(6),'Heading_Time.png'); pause(5);
figure(7); savefig('Heading_Hist'); pause(5);
saveas(figure(7),'Heading_Hist.png'); pause(5);
figure(8); savefig('RMS_Piksi'); pause(5);
saveas(figure(8),'RMS_Piksi.png'); pause(5);
figure(9); savefig('RMS_BD982'); pause(5);
saveas(figure(9),'RMS_BD982.png'); pause(5);
figure(10); savefig('RMS_BD992'); pause(5);
saveas(figure(10),'RMS_BD992.png'); pause(5);
figure(11); savefig('RMS_Heading'); pause(5);
saveas(figure(11),'RMS_Heading.png'); pause(5);
figure(12); savefig('Hist_3D_Piksi'); pause(5);
saveas(figure(12),'Hist_3D_Piksi.png'); pause(5);
figure(13); savefig('Hist_3D_BD982'); pause(5);
saveas(figure(13),'Hist_3D_BD982.png'); pause(5);
figure(14); savefig('Hist_3D_BD992'); pause(5);
saveas(figure(14),'Hist_3D_BD992.png'); pause(5);

close all;