pseudo_r.m

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

% If a combination was used, replace the combination number with the receiver
% number and regain the original input (less the antenna number)
if insert_comb_nums == 1
  x_user = [rec_nums x_user(:,2:4)]
end % if insert_comb_nums == 1

% rename the GPS satellite number (prn), receiver number, and antenna number
% variable for easier reading later in the code.  Do the same thing for the
% x_user data so it can be easily used in the tropo and iono models where
% azimuth and elevation relative to local level are required.
prn = x_gps(pr_ndex(:,2),1); 
ant_nums = ant_nums(pr_ndex(:,1));
x_user = x_user(pr_ndex(:,1),:);
rec_nums = x_user(:,1);

% Initialize the SA epsilon and dither error matrices and the receiver clocks
sa_eps_err = zeros(size(pr,1),1);
sa_clk_err = zeros(size(pr,1),1);
sa_clk_drift = zeros(size(pr,1),1);
clk_bias = zeros(size(pr,1),1);
clk_drift = zeros(size(pr,1),1);
line_bias = zeros(size(pr,1),1);

% Compute the range portion of the accumulated carrier phase measurement.
% Keep the phase in meters until all computations are complete, then
% convert to phase using the L1 wavelength.
cph = pr;

% Loop over the receivers to generate S/A and receiver clock errors
rec_num_unique = unique(rec_nums);
num_receivers = length(rec_num_unique);

for rec_loops = 1:num_receivers
  % Find all of the receivers with this receiver number
  I = find(x_user(:,1) == rec_num_unique(rec_loops));
  
  % Loop over the number of antenna for this receiver and add the line biases
  % See Parkinson, Vol. 2, page 28, eq 11.  The line bias is applied to each
  % antenna and would appear as a clock bias for that antenna.  However, when
  % multiple antenna are in use, the line bias is not common between the two 
  % antenna unless the cable length is the same.  For this case, set the lin
  % bias sigma to 0 to apply no line bias.
  ant_unique = unique(ant_nums(I));
  for ant_n = 1:length(ant_unique)
    I_this_ant = find(ant_nums == ant_unique(ant_n) & ...
                      x_user(:,1) == rec_num_unique(rec_loops));
    
    % Add the line bias if modeled
    if model(7) == 1 
      % Seed the random number generator such that is is repeatable,
      % but such that all of the line biases are not the same.
      if num_receivers > 1
        if seed == 0
          seed = 1;
        end % if num_receivers > 1
        seed = seed * rec_num_unique(rec_loops) * 1000 + ant_unique(ant_n);
      end % if num_receivers > 1                
      
      rand('seed',seed);                               
      
      % Get the line bias for this receiver and antenna
      if exist('model_data')
        if isfield(model_data,'line_bias')
          % Make sure that this is a nx3 using the error checking function
          estruct.variable(1).name = 'model_data.line_bias';
          estruct.variable(1).req_dim = [901 3];
          estruct.variable(1).var = model_data.line_bias;

          % Call the error checking function
          stop_flag = err_chk(estruct);
  
          if stop_flag == 1           
            fprintf('Invalid line bias inputs to %s.  Using default = 1 meter.\n\n', ...
                     estruct.func_name);
            line_bias_sigma = 1;
          else
         
            I = find(model_data.line_bias(:,1) == rec_num_unique(rec_loops) & ...
                     model_data.line_bias(:,2) == ant_unique(ant_n));
                 
            if ~isempty(I)
              line_bias_sigma = model_data.line_bias(I(1),3);
            else
              line_bias_sigma = 1;
            end % if ~isempty(I) 
          
          end % if stop_flag == 1
        
        else % if isfield(model_data,'line_bias')
          line_bias_sigma = 1;
      
        end % if isfield(model_data,'line_bias')
      
      else
        line_bias_sigma = 1;
      end % if exist('model_data')
        
      line_bias(I_this_ant) = ones(length(I_this_ant),1) * rand(1,1) * line_bias_sigma;  
      pr(I_this_ant) = pr(I_this_ant) + line_bias(I_this_ant);
      cph(I_this_ant) = cph(I_this_ant) + line_bias(I_this_ant);
    else
      line_bias(I_this_ant) = zeros(size(I_this_ant));
    end % if model(7) == 1  
          
  end % for ant_n = 1:length(ant_unique)
  
  % add sa epsilon effect
  if model(1) == 1       
    if exist('model_data')
      if isfield(model_data,'sa_eps')
        % Make sure that this is a 1x1 using the error checking function
        estruct.variable(1).name = 'model_data.sa_eps';
        estruct.variable(1).req_dim = [1 1];
        estruct.variable(1).var = model_data.sa_eps;

        % Call the error checking function
        stop_flag = err_chk(estruct);
  
        if stop_flag == 1           
          fprintf('Invalid SA epsilon inputs to %s.  Using default = 1 meter.\n\n', ...
                   estruct.func_name);
          % Set the sigma value for the SA epsilon model to the RTCM default of 23
          sa_eps_sigma = 23;    % meters                                          
        else
          % Set the sigma value for the SA epsilon model to the RTCM default of 23
          sa_eps_sigma = 23;    % meters                                          
        end % if stop_flag == 1
    
      else
        % Set the sigma value for the SA epsilon model to the RTCM default of 23
        sa_eps_sigma = 23;    % meters                                          
      end % if isfield(model_data,'sa_eps')
    
    else
      % Set the sigma value for the SA epsilon model to the RTCM default of 23
      sa_eps_sigma = 23;    % meters                                          
    end % if exist('model_data')

    % compute the SA sa_eps errors and apply them to the PRs 
    % all done in the sa_eps function
    [pr_1, sa_eps_err_1] = sa_eps([prn(I) pr(I)], sa_eps_sigma, seed);
    
    pr(I) = pr_1;
    sa_eps_err(I) = sa_eps_err_1;

    % apply the SA epsilon error to the phase data also
    cph(I) = cph(I) + sa_eps_err(I);
  
  % Insert user supplied epsilon model here
  elseif model(1) == 2
    dbstack
    fprintf('Warning from PSEUDO_R.\n');
    fprintf('No user supplied epsilon supplied.  \n');
    fprintf('Setting epsilon contribution to PR to zero (0).\n');
    sa_eps_err(I) = zeros(size(pr(I),1),1);  

  else
    sa_eps_err(I) = zeros(size(pr(I),1),1);  
  end % model(1) == 1  

  % add dither effect
  if model(2) == 1 
    if exist('model_data')
      if isfield(model_data,'sa_clock')
        % Make sure that this is a 1x1 using the error checking function
        estruct.variable(1).name = 'model_data.sa_clock';
        estruct.variable(1).req_dim = [1 3; 3 1];
        estruct.variable(1).var = model_data.sa_clock;

        % Call the error checking function
        stop_flag = err_chk(estruct);
  
        if stop_flag == 1           
          fprintf('Invalid SA dither (clock) inputs to %s.  Using default model.\n\n', ...
                   estruct.func_name);
          % set the sigma value for the SA clock model to the RTCM default values
          sa_model_data = [23 .28 118];    % sigma PR, sigma PR-rate, tau 
                                         % (decorrelation time) (m, m/s, dimensionless)                                          
        else
          % set the sigma value for the SA clock model to the RTCM default values
          sa_model_data = [23 .28 118];    % sigma PR, sigma PR-rate, tau 
                                         % (decorrelation time) (m, m/s, dimensionless)                                          
        end % if stop_flag == 1
    
      else
        % set the sigma value for the SA clock model to the RTCM default values
        sa_model_data = [23 .28 118];    % sigma PR, sigma PR-rate, tau 
                                       % (decorrelation time) (m, m/s, dimensionless)                                          
      end % if isfield(model_data,'sa_clock')
    
    else
      % set the sigma value for the SA clock model to the RTCM default values
      sa_model_data = [23 .28 118];    % sigma PR, sigma PR-rate, tau 
                                       % (decorrelation time) (m, m/s, dimensionless)                                          
    end % if exist('model_data')
    
    % compute the dither effects using the default model (RTCA parameters)
    [sa_clk_err(I), sa_clk_drift(I)] = sa_clock(t_pr(I,:), prn(I), sa_model_data, seed);

    pr(I) = pr(I) + sa_clk_err(I); 
    cph(I) = cph(I) + sa_clk_err(I);                 
  
  % Insert user supplied SA model here
  elseif model(2) == 2
    dbstack
    fprintf('Warning from PSEUDO_R.\n');
    fprintf('No user supplied SA model supplied.  \n');
    fprintf('Setting SA contribution to PR to zero (0).\n');
    sa_clk_err(I) = zeros(size(pr(I),1),1);  

  else
    sa_clk_err(I) = zeros(size(pr(I),1),1);  
  end % model(2) == 1  

  % Add random ambiguities to the phase data (parameter N in Parkinson, page 8,
  % equation 4).
  [cph(I), ambig(I)] = addambig([prn(I) cph(I)]);

  % compute user clock bias
  if model(5) == 1
    if exist('model_data')
      if isfield(model_data,'rec_clock')
        % Make sure that this is a 1x2 using the error checking function
        estruct.variable(1).name = 'model_data.rec_clock';
        estruct.variable(1).req_dim = [1 2];
        estruct.variable(1).var = model_data.rec_clock;

        % Call the error checking function
        stop_flag = err_chk(estruct);
  
        if stop_flag == 1           
          fprintf('Invalid receiver clock inputs to %s.  Using default model.\n\n', ...
                   estruct.func_name);
          % Set the value for the clock model to the default
          clk_model = [4e-19 1.58e-18];     % bias and frequency noise values
        else
          % Set the value for the clock model to the default
          clk_model = [4e-19 1.58e-18];     % bias and frequency noise values
        end % if stop_flag == 1
    
      else
        % Set the value for the clock model to the default
        clk_model = [4e-19 1.58e-18];     % bias and frequency noise values
      end % if isfield(model_data,'rec_clock')

    else
      % Set the value for the clock model to the default
      clk_model = [4e-19 1.58e-18];     % bias and frequency noise values
    end % exist('model_data')
    
    % 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
    
    [clk_bias(I) clk_drift(I)] = clockerr(t_pr(I,:), clk_model, seed);

    % add the clock bias to the pseudo-range and accumulated phase measurements
    pr(I) = pr(I) + clk_bias;
    cph(I) = cph(I) + clk_bias;

  % Insert user supplied receiver clock model here
  elseif model(5) == 2
    dbstack
    fprintf('Warning from PSEUDO_R.\n');
    fprintf('No user supplied receiver clock model supplied.  \n');
    fprintf('Setting receiver clock contribution to PR to zero (0).\n');
    clk_bias(I) = zeros(size(pr(I),1),1);  
    clk_drift(I) = zeros(size(pr(I),1),1);  
    
  else
    clk_bias(I) = zeros(size(pr(I),1),1);  
    clk_drift(I) = zeros(size(pr(I),1),1);  
  end % if model(5) == 1

end % for rec_loops = 1:num_receivers

% Begin the processing for the atmospheric models (troposphere and ionosphere).
% Compute azimuth and elevation in the NED frame.  This is how the tropo and iono
% models want the data.
ref_lla = ecef2lla(x_user(:,2:4));
[x_gps_ned] = ecef2ned(lofs, ref_lla);
[az, el] = ned2azel(x_gps_ned);     
  
% add troposphere
if model(3) == 1
  
  % Check if there is troposphere model data supplied
  if exist('model_data')
    if isfield(model_data,'tropo')
      % Make sure that this is a 1x3 using the error checking function
      estruct.variable(1).name = 'model_data.tropo';
      estruct.variable(1).req_dim = [1 3];
      estruct.variable(1).var = model_data.tropo;

      % Call the error checking function
      stop_flag = err_chk(estruct);
  
      if stop_flag == 1           
        fprintf('Invalid tropo model inputs to %s.  Using default model.\n\n', ...
                 estruct.func_name);
        % Set the value for the tropo model to the default
        trop_model = [1013.25 288.15 11.691];    
      else
        % Set the value for the tropo model to the default
        trop_model = [1013.25 288.15 11.691];    
      end % if stop_flag == 1
    
    else
      % Set the value for the tropo model to the default