pseudo_r.m

来自「此功能包用于各种GPS坐标和时间的转换」· M 代码 · 共 947 行 · 第 1/3 页

      trop_model = [1013.25 288.15 11.691];    
    end % if isfield(model_data,'tropo')
  
  else
    % Set the value for the tropo model to the default
    trop_model = [1013.25 288.15 11.691];    
  end % exist('model_data')
 
  % Find all of the elevation angles above to to compute iono for
  trop_dry = zeros(size(pr));
  trop_wet = zeros(size(pr));
  
  I = find(el > -pi/2);
  
  if ~isempty(I)
    % Compute the dry and wet troposphere components
    [trop_dry_mdl, trop_wet_mdl] = tropdlay(el(I),trop_model);           
    trop_dry(I) = trop_dry_mdl;
    trop_wet(I) = trop_wet_mdl;
  end % if ~isepmty(I)
    
  % add the troposphere error to the pr
  pr = pr + trop_dry + trop_wet;

  % add the troposphere error to the cph
  cph = cph + trop_dry + trop_wet;
  
% UNB1 troposphere model here, altitude dependent
elseif model(3) == 2
  % Find all of the elevation angles above to to compute iono for
  trop_dry = zeros(size(pr));
  trop_wet = zeros(size(pr));
  
  I = find(el > -pi/2);
  
  if ~isempty(I)
     [trop_dry_mdl, trop_wet_mdl] = tropunb1(ref_lla,t_pr,el(I));  
     trop_dry(I) = trop_dry_mdl;
     trop_wet(I) = trop_wet_mdl;
end
  
  % add the troposphere error to the pr
  
  pr = pr + trop_dry + trop_wet;

  % add the troposphere error to the cph
  cph = cph + trop_dry + trop_wet;
   
% Insert user supplied troposphere model here
elseif model(3) == 3
  dbstack
  fprintf('Warning from PSEUDO_R.\n');
  fprintf('No user supplied troposphere model supplied.  \n');
  fprintf('Setting ionosphere contribution to PR to zero (0).\n');
  trop_wet = zeros(size(az,1),1);
  trop_dry = zeros(size(az,1),1);

else
  trop_wet = zeros(size(az,1),1);
  trop_dry = zeros(size(az,1),1);

end % if model(3) == 1  

% add ionosphere
if model(4) == 1            
  % Check if there is ionosphere model data supplied
  if exist('model_data')
    if isfield(model_data,'iono')
      % Make sure that this is a 1x8 using the error checking function
      estruct.variable(1).name = 'model_data.iono';
      estruct.variable(1).req_dim = [1 8];
      estruct.variable(1).var = model_data.iono;

      % Call the error checking function
      stop_flag = err_chk(estruct);
  
      if stop_flag == 1           
        fprintf('Invalid iono model inputs to %s.  Using default model.\n\n', ...
                 estruct.func_name);
        % Set the value for the tropo model to the default
        iono_params = [0.1211D-07  0.1490D-07 -0.5960D-07 -0.1192D-06 ...
                       0.9626D+05  0.8192D+05 -0.1966D+06 -0.3932D+06]; 
      else
        % Set the value for the tropo model to the default
        iono_params = [0.1211D-07  0.1490D-07 -0.5960D-07 -0.1192D-06 ...
                       0.9626D+05  0.8192D+05 -0.1966D+06 -0.3932D+06]; 
      end % if stop_flag == 1
    
    else
      % Set the value for the tropo model to the default
      iono_params = [0.1211D-07  0.1490D-07 -0.5960D-07 -0.1192D-06 ...
                     0.9626D+05  0.8192D+05 -0.1966D+06 -0.3932D+06]; 
    end % if isfield(model_data,'iono')
  else
    % Set the value for the tropo model to the default
    iono_params = [0.1211D-07  0.1490D-07 -0.5960D-07 -0.1192D-06 ...
                   0.9626D+05  0.8192D+05 -0.1966D+06 -0.3932D+06]; 
  end % if exist('model_data')
  
  % Find all of the elevation angles above to to compute iono for
  iono = zeros(size(pr));
  
  I = find(el > -pi/2);
  if ~isempty(I)
    iono_dlay = ionodlay(t_pr(I,:),ref_lla(I,:),az(I),el(I),iono_params);
    iono(I) = iono_dlay;
  end % if ~isepmty(I)
  
  % add the ionosphere error to the pr
  pr = pr + iono;
  
  % add the ionosphere error to the cph (the sign is opposite from
  % the pr because the ionosphere delays the code (pr) and advances
  % the carrier (cph))
  cph = cph - iono;

% Insert user supplied ionosphere model here
elseif model(4) == 2                  
  dbstack
  fprintf('Warning from PSEUDO_R.\n');
  fprintf('No user supplied ionosphere model supplied.  \n');
  fprintf('Setting ionosphere contribution to PR to zero (0).\n');
  iono = zeros(size(az,1),1);

else
  iono = zeros(size(az,1),1);

end % if model(4) == 1

% Add GPS satellite clock model from ephemeris
if model(9) == 1            
  % Check if there is ephemeris data supplied
  if exist('ephem')
    % Make sure that this is a nx24 using the error checking function
    estruct.variable(1).name = 'ephem';
    estruct.variable(1).req_dim = [901 24];
    estruct.variable(1).var = ephem;

    % Call the error checking function
    stop_flag = err_chk(estruct);
  
    if stop_flag == 1           
      fprintf('Invalid ephemeris inputs to %s.  ',estruct.func_name);
      fprintf('No GPS satellite clock correction applied.\n\n');
      % Set the value for the tropo model to the default
      sat_clock = zeros(size(az,1),1);
    else
      % Compute the satellite clock correction for each GPS satellite
      % based on the ephemeris data.
      % Start by find the unique satellite IDs and initialing the sat_clock
      sat_ids = unique(ephem(:,1));
      sat_clock = zeros(size(az,1),1);
      
      % Compute the time past the Toc (time of clock) parameter.
      pr_time = gpst2sec(t_pr);
      
      % Loop over the valid satellite IDs and compute the clock correction
      % for that satellite.
      for i = 1:length(sat_ids)
        % Find all of the PR and ephem with this satellite ID
        I_pr = find(prn == sat_ids(i));
        I_ephem = find(ephem(:,1) == sat_ids(i));
        
        % Find the time past the Toc (time of clock)
        toc = gpst2sec([ephem(I_ephem(1),19) ephem(I_ephem(1),20)]);
        
        % Compute the time between toc and the PR times
        delta_t = pr_time(I_pr) - toc;
        
        % Compute the satellite clock correction to the PR in meters
        sat_clock(I_pr) = (ephem(I_ephem(1),21) + ...
                           ephem(I_ephem(1),22) * delta_t + ...
                           ephem(I_ephem(1),23) * delta_t.^2) * LIGHT_SPEED;
      end % for i = 1:length(sat_ids)
    end % if stop_flag == 1
    
  else
    sat_clock = zeros(size(az,1),1);
  end % if exist('ephem')

  % add the satellite clock error to the pr
  pr = pr + sat_clock;
  
  % add the ionosphere error to the cph (the sign is opposite from
  % the pr because the ionosphere delays the code (pr) and advances
  % the carrier (cph))
  cph = cph + sat_clock;

else
  sat_clock = zeros(size(az,1),1);

end % if model(9) == 1

% Add relativity correction from ephemeris data per IC-GPS-200C, 10 Oct 1993
% pages 88-89 section 20.3.3.3.3.1, User Algorithm for the SV Clock Correction.
if model(11) == 1            
  % Check if there is ephemeris data supplied
  if exist('ephem')
    % Make sure that this is a nx24 using the error checking function
    estruct.variable(1).name = 'ephem';
    estruct.variable(1).req_dim = [901 24];
    estruct.variable(1).var = ephem;

    % Call the error checking function
    stop_flag = err_chk(estruct);
  
    if stop_flag == 1           
      fprintf('Invalid ephemeris inputs to %s.  ',estruct.func_name);
      fprintf('No relativistic corrections applied.\n\n');
      % Set the value for the relativistic effects to zero
      relative = zeros(size(az,1),1);
    else                     
      % Define the constant F, per ICD-GPS-200, section 20.3.3.3.3.1
      F = -4.442807633e-10;
      
      % Compute the PR times in seconds past GPS epoch
      pr_time = gpst2sec(t_pr);

      % Start by find the unique satellite IDs and initialing the sat_clock
      sat_ids = unique(ephem(:,1));
      relative = zeros(size(az,1),1);
      
      % Loop over the valid satellite IDs and compute the relativistic 
      % correction for that satellite.
      for i = 1:length(sat_ids)
        % Find all of the PR and ephem with this satellite ID
        I_pr = find(prn == sat_ids(i));
        I_ephem = find(ephem(:,1) == sat_ids(i));
        
        % Find the Toe (time of ephemeris) in GPS seconds
        toe = gpst2sec([ephem(I_ephem(1),19) ephem(I_ephem(1),17)]);
        
        % Compute the time between toc and the PR times
        delta_t = pr_time(I_pr) - toc;
         
        % Compute some orbit terms
        a12 = ephem(I_ephem(1),5);  % get sqrt(a) = ephem(:,5) (m)
        a = ephem(I_ephem(1),5).^2; % get a = ephem(:,5)^2 (m)
        e = ephem(I_ephem(1),4);    % load eccentricity to a local variable
        n0 = sqrt(MU_EARTH / a.^3);    % compute the nominal mean motion (rad/s)
        n = n0 + ephem(I_ephem(1),3);     % corrected mean montion
                                              % delta_n = ephem(:,3) 
        % Compute the mean anomaly (M)
        M = ephem(I_ephem(1),2) + n .* delta_t; % M0 = ephem(:,2)   
        
        % Compute the eccentric anomaly
        [E] = keplr_eq(M,e);
        
        % Compute the relativistic correction to the PR in meters
        relative(I_pr) = (F .* e .* a12 .* sin(E)) * LIGHT_SPEED;
        
      end % for i = 1:length(sat_ids)
    end % if stop_flag == 1
    
  else
    % Set the value for the relativistic effects to zero
    relative = zeros(size(az,1),1);
  end % if exist('ephem')
    
  % add the satellite clock error to the pr
  pr = pr + relative;
  
  % add the ionosphere error to the cph (the sign is opposite from
  % the pr because the ionosphere delays the code (pr) and advances
  % the carrier (cph))
  cph = cph + relative;

else
  relative = zeros(size(az,1),1);

end % if model(11) == 1

% add receiver white noise 
if model(6) == 1
  % Compute the clock bias using the model. Use the receiver number in
  % conjunction with the provided seed to generate a new seed value.  
  % This will keep all of the clocks from the different receivers from
  % being the same.    
  if num_receivers > 1
    if seed == 0
      seed = 1;
    end % if num_receivers > 1
    seed = seed * rec_num_unique(rec_loops);
  end % if num_receivers > 1

  % set the seed value of the random number generator 
  randn('seed',seed);
  
  % compute white noise for the code and carrier
  code_white = randn(size(pr,1),1) * code_noise;
  carrier_white = randn(size(pr,1),1) * carrier_noise;

  pr = pr + code_white; 
  cph = cph + carrier_white;
   
else           

  code_white = zeros(size(pr,1),1);
  carrier_white = zeros(size(pr,1),1);
end % if model(6) == 1

% convert the phase measurements from meters to phase
cph = cph / L1_WAVELENGTH;

% build up the PR error matrix for output
pr_errors = [sa_eps_err sa_clk_err trop_wet trop_dry iono clk_bias clk_drift ...
             code_white carrier_white line_bias sat_motion sat_clock relative];

% add the phase data to the output variable pr
pr = [pr cph];

%%%%% END ALGORITHM CODE %%%%%

% end of PSEUDO_R