%%Program for confidence regions creation from NMEA log.
%Autor: David Jurajda
%Date: 5.5.2013

%***************************** REFERENCES *********************************
%http://www.personal.psu.edu/sqm110/GPS/GPS_Prelab.pdf

%http://www.mathworks.com/matlabcentral/fileexchange/15010-article-using-ma
    %tlab-with-external-hardware-garmin-gps-example

%http://www.mathworks.com/matlabcentral/fileexchange/24100-nmea-0183-senten
    %ce-reader
 
%http://ohioline.osu.edu/aex-fact/0560.html

%checksum

%wgs2utm

%crr

%% ********************* Section 1 - initialization ***********************

clc
close all
clear all

%Shift between known ref. point and real position of Test board 
l = sqrt(0.04^2+0.34^2);
fi = rad2deg(asin(0.04/0.34));
d_pos(1) = l*cos(deg2rad(-148.78+fi));
d_pos(2) = l*sin(deg2rad(-148.78+fi));
d_pos(3) = 0.05;

%shift for each receiver, because of diferent location on main board.
NVS_ant_pos = [-0.015 0.022]; 
l = sqrt(NVS_ant_pos(1)^2 + NVS_ant_pos(2)^2);
d_NVS(1) = l*cos(deg2rad(-148.78));
d_NVS(2) = l*sin(deg2rad(-148.78));
d_NVS(3) = 0;

UC530_ant_pos = [0.037 -0.004]; 
l = sqrt(UC530_ant_pos(1)^2 + UC530_ant_pos(2)^2);
d_UC530(1) = l*cos(deg2rad(-148.78));
d_UC530(2) = l*sin(deg2rad(-148.78));
d_UC530(3) = 0; 

UP500_ant_pos = [0.17 0.26]; 
l = sqrt(UP500_ant_pos(1)^2 + UP500_ant_pos(2)^2);
d_UP500(1) = l*cos(deg2rad(-148.78));
d_UP500(2) = l*sin(deg2rad(-148.78));
d_UP500(3) = 0;

%Roof reference point                             dd mm ss.sssss
REF_ETRS89 = struct('lat', [], ... %double    Lat 49 13 53.23748 N  Jilek
                    'lon', [], ... %double    Lon 16 34 21.82488 E   
                    'alt', []);    %double    Alt 340.079
REF_ETRS89.lat = 49 + (13 + (53.23748/60))/60.0; % degrees               
REF_ETRS89.lon = 16 + (34 + (21.82488/60))/60.0; % degrees 
REF_ETRS89.alt = 340.079; % degrees 

%Transformated point by Transdat 16.26 (No additional datum corections)             
REF_WGS84 = struct('lat',  [], ... %double    Lat  49 13 53.251386	
                   'lon', [], ... %double    Lon  16 34 21.848185 
                   'alt', []);    %double    Alt 340.079 ?
REF_WGS84.lat = 49 + (13 + (53.251386/60))/60.0; % degrees               
REF_WGS84.lon = 16 + (34 + (21.848185/60))/60.0; % degrees 
REF_WGS84.alt = 340.079; % degrees 

%UTM projection of REF_ETRS89
REF_ETRS89_UTM = struct('x',      [], ... 
                       'y',       [], ... 
                       'utmzone', [], ...       
                       'utmhemi', []);  
[REF_ETRS89_UTM.x,...
 REF_ETRS89_UTM.y,...
 REF_ETRS89_UTM.utmzone,...
 REF_ETRS89_UTM.utmhemi]= wgs2utm(REF_ETRS89.lat,REF_ETRS89.lon);                       
                   
%UTM projection of REF_WGS84
REF_WGS84_UTM = struct('x',       [], ... 
                       'y',       [], ... 
                       'utmzone', [], ...       
                       'utmhemi', []);  
[REF_WGS84_UTM.x,...
 REF_WGS84_UTM.y,...
 REF_WGS84_UTM.utmzone,...
 REF_WGS84_UTM.utmhemi]= wgs2utm(REF_WGS84.lat,REF_WGS84.lon);      

%Position corection
REF_WGS84_UTM.x = REF_WGS84_UTM.x + d_pos(1);
REF_WGS84_UTM.y = REF_WGS84_UTM.y + d_pos(2);


% Structures declaration
GGA = struct('utc',       [], ... %string
             'time',      [], ... %double <0,1> day part
             'lat',       [], ... %double minutes S -
             'n_s',       [], ... %string
             'lon',       [], ... %double minutes W-
             'e_w',       [], ... %string
             'fix_flag',  [], ... %uint8
             'sv_num',    [], ... %double
             'hdop',      [], ... %double
             'altitude',  [], ... %double
             'geo_separ', [], ... %double 
             'dgps_age',  [], ... %double
             'dgps_id',   [], ... %double
             'checksum',  []);    %%uint8
 
         
Info = struct('type',                  [], ... %string NVS08, UP500, UC530
              'corections',            [], ... %string SBAS, RTCM, SBAS+RTCM, Bez korekcí
              'fix_frequency',         [], ... %double 1, 5, 10 Hz
              'h_accuracy',            [], ... %string from datasheet [m]
              'accuracy_type',         [], ... %string R95 RMS CEP...
              'graph_shift_x',         [], ... %double 
              'graph_shift_y',         [], ... %double 
              'graph_loc',             [], ... %double 
              'GGA_checksum_error',    [], ... %double
              'GGA_structure_error',   [], ... %double 
              'GGA_zero_fix',          [],... %double
              'info_note',             []); ...%double 
                  

UTM = struct('x',       [], ... 
             'y',       [], ... 
             'utmzone', [], ...       
             'utmhemi', []);        
    
% **************** Required user input start  **********************************         
Info.type =            'UC530';       % 'UP500' 'NVS08' 'UC530'
Info.corections =      'bez korekcí'  ;% 'SBAS' 'RTCM' 'SBAS+RTCM' 'bez korekcí' 
Info.system =          'GPS'; % 'GPS' 'GLONASS' 'GPS+GLONASS'
Info.graph_shift_x =      0;         %graph to legend adjustment
Info.graph_shift_y =      0;          %graph to legend adjustment 
Info.graph_loc =       'SouthEast';   %NorthEast 'NorthWest' 'SouthWest''SouthEast'
Info.note=             '-';  
% **************** Required user input stop  ************************************


%set proper title line from user input
Info.fix_frequency =    1;       % 1 2 5 10
if    (strcmp(Info.type, 'UP500'))
    Info.accuracy_type = 'R 95%';  %alowed 'R 95%' 'R 67%' 'RMS' 'CEP'
    Info.h_accuracy =     3;       %UP500: 3m(95% ), NVS08: 1,5m(autonomous)  1m(SBAS, RTCM) RMS, UC530M: 3m(67%)  
elseif(strcmp(Info.type, 'UC530'))
    Info.accuracy_type = 'RMS';  %alowed 'R 95%' 'R 67%' 'RMS' 'CEP'
    Info.h_accuracy =     3;       %UP500: 3m(95% ), NVS08: 1,5m(autonomous)  1m(SBAS, RTCM) RMS, UC530M: 3m(67%)  
elseif(strcmp(Info.type, 'NVS08'))
    if  (strcmp(Info.corections, 'bez korekcí'))
      Info.accuracy_type = 'RMS';  %alowed 'R 95%' 'R 67%' 'RMS' 'CEP'
      Info.h_accuracy =     1.5;       %UP500: 3m(95% ), NVS08: 1,5m(autonomous)  1m(SBAS, RTCM) RMS, UC530M: 3m(67%)  
    else
      Info.accuracy_type = 'RMS';  %alowed 'R 95%' 'R 67%' 'RMS' 'CEP'
      Info.h_accuracy =     1;       %UP500: 3m(95% ), NVS08: 1,5m(autonomous)  1m(SBAS, RTCM) RMS, UC530M: 3m(67%)    
    end
end    

 

%% ******************** Section 2: Set input NMEA log file ****************


%Raw NMEA data input paths:
%fid = fopen('d:\Škola\M4_semestr\Dioplomka\data z mereni\kolejni\A_9800.ubx','r');
%fid = fopen('test_data_NVS08CSM .txt','r');

%fid = fopen('d:\Škola\M4_semestr\Dioplomka\data z mereni\kolejni_strecha\1.mereni_3.5.2013\UP500_SBAS_COM7_130503_140640.ubx','r');
%fid = fopen('d:\Škola\M4_semestr\Dioplomka\data z mereni\kolejni_strecha\1.mereni_3.5.2013\UC530_SBAS_COM12_130503_140711.ubx','r');
%fid = fopen('d:\Škola\M4_semestr\Dioplomka\data z mereni\kolejni_strecha\1.mereni_3.5.2013\NVS08_RTCM_COM13_130503_140248.ubx','r');

%fid = fopen('d:\Škola\M4_semestr\Dioplomka\data z mereni\kolejni_strecha\2.mereni_4.5.2013\UP500_no_corections_COM7_130504_185125.ubx','r');
%fid = fopen('d:\Škola\M4_semestr\Dioplomka\data z mereni\kolejni_strecha\2.mereni_4.5.2013\UC530M_RTCM_COM13_130504_185201.ubx','r');
%fid = fopen('d:\Škola\M4_semestr\Dioplomka\data z mereni\kolejni_strecha\2.mereni_4.5.2013\NVS08_SBAS_COM12_130504_185142.ubx','r');

%fid = fopen('d:\Škola\M4_semestr\Dioplomka\data z mereni\kolejni_strecha\3.mereni_6.5.2013\UP500_no_corections_COM7_130506_175554.ubx','r');
%fid = fopen('d:\Škola\M4_semestr\Dioplomka\data z mereni\kolejni_strecha\3.mereni_6.5.2013\UC530_no_corections_COM8_130506_175831.ubx','r');
%fid = fopen('d:\Škola\M4_semestr\Dioplomka\data z mereni\kolejni_strecha\3.mereni_6.5.2013\NVS08_SBAS+RTMC_COM9_130506_180046.ubx','r');

%fid = fopen('d:\Škola\M4_semestr\Dioplomka\data z mereni\kolejni_strecha\4.mereni_7.5.2013\UP500_no_corections_COM3_130507_193341.ubx','r');
%fid = fopen('d:\Škola\M4_semestr\Dioplomka\data z mereni\kolejni_strecha\4.mereni_7.5.2013\UC530M_OnlyGlonass_COM8_130507_193656.ubx','r');
%fid = fopen('d:\Škola\M4_semestr\Dioplomka\data z mereni\kolejni_strecha\4.mereni_7.5.2013\NVS_nezname_nastaveni_chybna_odpoved_COM9_130507_193614.ubx','r');

%fid = fopen('d:\Škola\M4_semestr\Dioplomka\data z mereni\kolejni_strecha\5.mereni_9.5.2013\UP500_GPS_SBAS__COM7_130509_130742.ubx','r');
%fid = fopen('d:\Škola\M4_semestr\Dioplomka\data z mereni\kolejni_strecha\5.mereni_9.5.2013\UC530M_GPS_NoCor_COM8_130509_130708.ubx','r');
%fid = fopen('d:\Škola\M4_semestr\Dioplomka\data z mereni\kolejni_strecha\5.mereni_9.5.2013\NVS08_GP_GL_NoCor_COM9_130509_130606.ubx','r');

%************************************************
%fid = fopen('d:\Škola\M4_semestr\Dioplomka\Diplomka_Ultra_final_version\Jednotlivé přílohy\TEX\A_grafy_pos_alt_hdop_snum_cov_cor\5.mereni_9.5.2013\NVS08_GPS_GLONASS_BEZ_130509_130606.ubx','r');
%fid = fopen('d:\Škola\M4_semestr\Dioplomka\Diplomka_Ultra_final_version\Jednotlivé přílohy\TEX\A_grafy_pos_alt_hdop_snum_cov_cor\5.mereni_9.5.2013\UC530M_GPS_BEZ_130509_130708.ubx','r');
%fid = fopen('d:\Škola\M4_semestr\Dioplomka\Diplomka_Ultra_final_version\Jednotlivé přílohy\TEX\A_grafy_pos_alt_hdop_snum_cov_cor\65.mereni_9.5.2013\UP500_GPS_SBAS_130509_130742.ubx','r');

%fid = fopen('d:\Škola\M4_semestr\Dioplomka\Diplomka_Ultra_final_version\Jednotlivé přílohy\TEX\A_grafy_pos_alt_hdop_snum_cov_cor\6.mereni_13.5.2013\NVS08_GLONASS_BEZ_130510_171023.ubx','r');
fid = fopen('d:\Škola\M4_semestr\Dioplomka\Diplomka_Ultra_final_version\Jednotlivé přílohy\TEX\A_grafy_pos_alt_hdop_snum_cov_cor\6.mereni_13.5.2013\UC530_GPS_SBAS_130510_170936.ubx','r');
%fid = fopen('d:\Škola\M4_semestr\Dioplomka\Diplomka_Ultra_final_version\Jednotlivé přílohy\TEX\A_grafy_pos_alt_hdop_snum_cov_cor\6.mereni_13.5.2013\UP500_GPS_SBAS_130510_170905.ubx','r');


%% ******** Section 3: Parse all GGAs in file *****************************

tline=fgetl(fid);
index = 1;
radek = 1;
wrong_checksum = 0;
disintegrated_lines = 0;

while (~feof(fid)) %until text lines in file or feof(obj1)
  % parse GGA (Global Positioning System Fix Data)
  if ((~isempty(strmatch('$GPGGA',tline)))&&(~isempty(tline)))
  
  comma = findstr(tline,','); 
  asterisk = findstr(tline,'*');
 
  if ((length(comma) == 14)&&(length(asterisk) == 1)&&((length(tline)-asterisk)==2)) %line integrity control
  checksum_nmea  = tline(asterisk+1:asterisk+2);
  checksum_count = nmeachecksum(tline);
  if (checksum_nmea == checksum_count)             %checksum control
      
%time  
    t_time = tline(comma(1)+1:comma(2)-4);
    if(isempty(t_time))
     t_time  =  NaN;
    else
     GGA.utc(index,:) = char(t_time);
     GGA.time(index,:) =  datenum(t_time,'HHMMSS') - ...
                          floor(datenum(t_time,'HHMMSS'));   
     clear t_time;   
    end
%lat
    t_deg = tline(comma(2)+1:comma(2)+2);
    t_min = tline(comma(2)+3:comma(3)-1);
    if(isempty(tline(comma(2)+1:comma(2)+2)) || isempty(tline(comma(2)+3:comma(3)-1)))
      GGA.lat(index,:)  =  NaN;
    else
     GGA.lat(index,:)       = str2double(t_deg) + (str2double(t_min)/60.0); % tline(comma(2)+1:comma(3)-1)
    end 
%n_s   
    if(isempty(tline(comma(3)+1:comma(4)-1)))
      GGA.n_s(index,:)  =  NaN;
    else
      GGA.n_s(index,:) = tline(comma(3)+1:comma(4)-1);
    end
    if ((GGA.n_s(index,:)  ==  'S') || (GGA.n_s(index,:) == 's'))
      GGA.lat(index,:) = GGA.lat(index) * -1;
    end
%lon  
    t_deg = tline(comma(4)+1:comma(4)+3);
    t_min = tline(comma(4)+4:comma(5)-1);
    if(isempty(tline(comma(4)+1:comma(4)+3)) || isempty(tline(comma(4)+4:comma(5)-1)))
      GGA.lon(index,:)  =  NaN;
    else
     GGA.lon(index,:) = str2double(t_deg) + (str2double(t_min)/60.0); % tline(comma(4)+1:comma(5)-1)
    end     
    clear t_deg;
    clear t_min;
%e_w   
    if(isempty(tline(comma(5)+1:comma(6)-1)))
      GGA.e_w(index,:)  =  NaN;
    else
      GGA.e_w(index,:) = tline(comma(5)+1:comma(6)-1);
    end
    if((GGA.e_w(index,:)  ==  'W') || (GGA.e_w(index,:)  ==  'w'))
      GGA.lon(index) = GGA.lon(index) * -1;
    end
%fix_flag  
    if(isempty(tline(comma(6)+1:comma(7)-1)))
      GGA.fix_flag(index,:)  =  NaN;
    else
      GGA.fix_flag(index,:)  = uint8(str2double(tline(comma(6)+1:comma(7)-1)));
    end
  GGA.sv_num(index,:)    = str2double(tline(comma(7)+1:comma(8)-1));
  GGA.hdop(index,:)      = str2double(tline(comma(8)+1:comma(9)-1));
  GGA.altitude(index,:)  = str2double(tline(comma(9)+1:comma(10)-1)); %!M Altitude (mean-sea-level, geoid)
  GGA.geo_separ(index,:) = str2double(tline(comma(11)+1:comma(12)-1)); %!M Difference between the WGS-84 reference ellipsoid surface and the mean-sea-level altitude.
  GGA.dgps_age(index,:)  = str2double(tline(comma(13)+1:comma(14)-1));
  GGA.dgps_id(index,:)   = str2double(tline(comma(14)+1:asterisk-1));
  GGA.checksum(index,:)  = uint8(str2double(tline(asterisk+1:asterisk+2)));
    clear comma;
    clear asterisk;
   
    index = index + 1;
    disp(['GGA veta: ', num2str(index),' -> Data OK. ', 'Radek: ', num2str(radek)]);
  else
    wrong_checksum = wrong_checksum + 1;
    disp(['GGA veta: ', num2str(index), ' -> Warning: Data corupted, checksum not valid! ', 'Radek: ', num2str(radek)])
  end
  else %poskozeny radek
    disintegrated_lines =  disintegrated_lines + 1; 
  end  %rozpadly radek
  end  %neni GGA
  
  tline = fgetl(fid); 
  radek = radek + 1;

end %while
disp(['Error in GGA sentence (checksum): ', num2str(wrong_checksum)])
disp(['Error in GGA sentence (wrong asterisk sum, checksum missing ): ', num2str(disintegrated_lines)])

 clear tline;
 fclose(fid);
 
 Info.GGA_checksum_error = wrong_checksum;
 Info.GGA_structure_error = disintegrated_lines;           
 Info.GGA_zero_fix = length(find(GGA.fix_flag == 0));         
           
 
%% ************** Section 4: Lat, Lon to UTM  *****************************
 
for index=1:1:length(GGA.lon)
    index
    [UTM.x(index),UTM.y(index),UTM.utmzone(index),UTM.utmhemi(index,:)] = wgs2utm(GGA.lat(index),GGA.lon(index));
end

%% *******  Section 5: Confidential intervals, graphs   *******************

%Input data matrix A, first 24 h. selection, flag validity control
fix_flag_zero = 0;
fix_flag_ok = 1;
A = zeros(86400,3);
if (length(UTM.x) > 86400)
 for ii=1:1:86400
   if (GGA.fix_flag(ii) > 0)  
   A(fix_flag_ok,:) = [UTM.x(ii) UTM.y(ii) GGA.altitude(ii)];
   fix_flag_ok = fix_flag_ok + 1;
   else
   disp(['Neplatný fix: ', num2str(ii)])    
   fix_flag_zero = fix_flag_zero + 1;
   end
 end
else
 disp('Not enough samples for 24 h  .')
end    

A = A(1:fix_flag_ok-1,:);

disp(['Neplatnych fixu: ', num2str(fix_flag_zero)])   

%Move origin to ref. point
for ii=1:1:length(A)
A(ii,1) = A(ii,1) - REF_WGS84_UTM.x;
A(ii,2) = A(ii,2) - REF_WGS84_UTM.y;
end

A_mean = mean(A);
C = cov(A);       %covariance matrix [x,y,z]
Cxy = C(1:2,1:2); %covariance matrix [x,y]
[R,P,RLO,RUP] = corrcoef(A);  %correlation matrix [x,y,z]

BIAS = sqrt(A_mean(1)^2 + A_mean(2)^2);
%'R 95%' 'R 67%' 'RMS' 'CEP'
if    (strcmp(Info.accuracy_type, 'R 95%'))
RR = crr(Cxy,0.95);  % R95  95% confidence
elseif(strcmp(Info.accuracy_type, 'R 67%'))
RR = crr(Cxy,0.67);  % R67
elseif(strcmp(Info.accuracy_type, 'RMS'))
RR = crr(Cxy,0.682); % R682 Root mean square (68.2), 1 sigma error
elseif(strcmp(Info.accuracy_type, 'CEP'))
RR = crr(Cxy,0.5);   % R50  Circular Error Probable (CEP)
end

% Reference point REF_WGS84_UTM
hFig = figure;
scatter(A(:,1), A(:,2),'+') %Print all samples
hold on
scatter(0, 0,'x','r') %Origin = ref.point
hold on
% text(0,0,'Reference','HorizontalAlignment','right')%WGS origin
% hold on
% scatter(REF_ETRS89_UTM.x-REF_WGS84_UTM.x,...
%         REF_ETRS89_UTM.y-REF_WGS84_UTM.y,'+','r') %ETRS origin
% text(REF_ETRS89_UTM.x-REF_WGS84_UTM.x,...
%      REF_ETRS89_UTM.y-REF_WGS84_UTM.y,...
%      'ETRS89','HorizontalAlignment','right')
% hold on
scatter(A_mean(1),A_mean(2),'*','g') %elipses center
hold on
line([A_mean(1),0],...  %bias distance line
     [A_mean(2),0],...
     'Color','k');
hold on
%h1 = error_ellipse(Cxy,A_mean(1:2),'conf', 0.5,'style','m');
%set(h1, 'LineWidth',2)
%hold on
[el_ax1 el_ax2 h2] = error_ellipse(Cxy,A_mean(1:2),'conf', 0.95,'style','r');
set(h2,'LineWidth',2)
hold on
%UP500: 3m(95% ), NVS08: 1,5m(autonomous)  1m(SBAS, RTCM) RMS, UC530M: 3m(67%) 
h3 = circle(A_mean(1),A_mean(2),Info.h_accuracy); %manufacturer Error:
set(h3,'Color','g','LineWidth',2)
hold on 
h4 = circle(A_mean(1),A_mean(2),RR); %Computed Error
set(h4,'Color','c','LineWidth',2)
% Title('UP500, 1 Hz, 24 h, GPS + SBAS, počátek v ref. bodu') %'UP500, 1 Hz, 24 h, GPS + SBAS, počátek v ref. bodu'
hold on 


% % ****************START*****test PORZE, load NVS_PORZE.mat****START********
%  B = zeros(70000,3);
%  for ii=1:1:70000
%  B(ii,:) = [PORZE.x(ii) PORZE.y(ii)  340];
%  end
% 
%  B_mean = mean(B);
%  
%  for ii=1:1:length(B)
% B(ii,1) = B(ii,1) -  B_mean(1);
% B(ii,2) = B(ii,2) -  B_mean(2);
%  end
%  
% scatter(B(:,1), B(:,2),'+','y') %Print all samples
% %****************STOP****test test PORZE, load NVS_PORZE.mat***STOP********


title([Info.type,', ',num2str(Info.fix_frequency),' Hz',', 24 h, ',...
    Info.system, ', ' Info.corections])
% Title('UP500, 1 Hz, 24 h, GPS + SBAS, počátek v ref. bodu')

% legend('Vzorky', 'Ref. bod WGS-84 (počátek)', 'Ref. bod ETRS-84',...
%     'Střední hodnota výběru',['Odchylka:
%     ',num2str(sprintf('%.2f',BIAS)),' m'],...
%     'Elipsa 95%','R 95% výrobce: 3 m',['R 95% z dat: ',num2str(sprintf('%.2f',R95)),' m']...
%     ); % pridani legendy ke grafu'Location','NorthEastOutside'

legend('Vzorky','Reference',... % 'Ref. bod WGS-84 (počátek)', 'Ref. bod ETRS-84',
    'Střední hodnota',['Odchylka: ',num2str(sprintf('%.2f',BIAS)),' m'],...
    ['95% elipsa, ',sprintf('%.2f m, %.2f m',el_ax1, el_ax2)],...
    [Info.accuracy_type,' výrobce: ',num2str(Info.h_accuracy),' m'],...
    [Info.accuracy_type,' z dat: ',sprintf('%.2f',RR),' m'],...
    'Location',Info.graph_loc); % pridani legendy ke grafu'Location','NorthEastOutside'

axis equal
set(hFig, 'Position', [104   135   786   568])
ax = axis; %x min, xmax ymin ymax
shift_x = Info.graph_shift_x;
shift_y = Info.graph_shift_y;
frame = 0.5;

axis([ax(1)+shift_x-frame ax(2)+shift_x+frame ax(3)+shift_y-frame ax(4)+shift_y+frame])
xlabel('x [m], UTM')
ylabel('y [m], UTM')

%Test distance between 
% xx = REF_WGS84_UTM.x - REF_ETRS89_UTM.x
% yy = REF_WGS84_UTM.y - REF_ETRS89_UTM.y
%kk = find(GGA.fix_flag == 2);

%% poznamky pak smazat

%udělat test RTK_lib, čt
 

               