pseudo_r.m

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

function [t_pr,prn,pr,pr_errors,pr_ndex,obscure_info]=...
               pseudo_r(t_user,x_user,v_user,t_gps,x_gps,v_gps,model,...
                        seed,code_noise,carrier_noise,ephem,model_data)

% [t_pr,prn,pr] = pseudo_r(t_user,x_user,v_user,t_gps,x_gps,v_gps);
%                      or
% [t_pr,prn,pr,pr_errors,obscure_info] = ...
%   pseudo_r(t_user,x_user,v_user,t_gps,x_gps,v_gpsmodel,seed,code_noise, ...
%            carrier_noise,ephem,model_data);
%
% Computes pseduo-range (pr) and accumulated carrier phase (cph) measurements
% given a user trajectory and the GPS/GLONASS satellite positions. 
% Errors that can be included in the measurements include S/A (epislon and 
% dither), troposphere, ionosphere, receiver clock bias and clock drift,  
% receiver code and carrier tracking noise, satellite motion, GPS satellite
% clock, and relativity.  Pseudorange and phase data are generated for all
% satellites and user positions.  Editing and masking of the measurement
% data can be accomplished using the function VIS_DATA.
%
% Input:
%   t_user - GPS time vector for user trajectory [GPS_week, GPS_sec] (nx2)
%             There must be coincide times with the times in the GPS time vector. 
%             If there are no coincident times, no pseudo-range or phase data
%             will be generated and the output matrices will be empty.
%             valid GPS_week values are 1-3640 (years 1980-2050)
%             valid GPS_sec values are 0-604799
%   x_user - ECEF/ECI position vectors for the user vehicle [x,y,z] (nx3) (m) or
%             if more than 1 receiver is being used, [receiver_num x y z] (nx4) or
%             if multiple antenna are used, [receiver_num ant_num x y z] (nx5)
%   v_user - ECEF/ECI velocity vectors for the user vehicle [x,y,z] (nx3) (m/s)
%   t_gps  - GPS time vector for GPS positions [GPS_week, GPS_sec] (mx2)
%             valid GPS_week values are 1-3640 (years 1980-2050)
%             valid GPS_sec values are 0-604799
%   x_gps  - ECEF/ECI position vectors for GPS satellites [prn,x,y,z] (mx4) (m)
%   v_gps  - ECEF/ECI velocity vectors for GPS satellites [prn,x,y,z] (mx3) (m/s)
%   model  - flags controlling which contributions to the 
%             PR errors are modeled (optional).  (1x11)
%              [sa_eps dither troposphere ionosphere receiver_clock 
%               receiver_noise line_bias sat_motion sat_clock earth_rotation
%               relativity]
%              a value of 1 (or 2 for the tropo) indicates useage of the model
%              and a value of zero indicates no use of that model.  Use values
%              of 2 to implement user supplied models.  See the code for where
%              to insert the user models.  A warning is given if a user model
%              is selected and none is supplied.
%              Default = [1 1 1 1 1 1 1 0 0 0 0 0].
%   seed   - seed value for random number generator (optional)
%             Default value is 0. 
%   code_noise    - 1 sigma estimate of the receiver code noise (optional) (1x1)
%                    (m), default = 1
%   carrier_noise - 1 sigma estimate of the receiver carrier noise (optional) 
%                    (1x1) (m), default = 0.01  
%   ephem         - ephemeris matrix for all satellites (nx24). (optional)
%                    Used to compute satellite clock. If not provided, no 
%                    GPS satellite clock effects will be computed
%                    The columns of ephemeris matrix are ...
%                    [prn,M0,delta_n,e,sqrt_a,long. of asc_node at GPS week epoch,
%                    i,perigee,ra_rate,i_rate,Cuc,Cus,Crc,Crs,Cic,Cis,Toe,IODE,
%                    GPS_week,Toc,Af0,Af1,Af2,perigee_rate] 
%                    Ephemeris parameters are from ICD-GPS-200 with the 
%                    exception of perigee_rate. 
%   model_data    - structure with data for each of the models.  (optional)
%                   If not provided, model defaults will be used.  Valid model
%                   structure elements are ...
%                    model_data.sa_eps (1x1) see SA_EPS for details
%                    model_data.sa_dither (1x3) see SA_CLOCK for details
%                    model_data.tropo (1x3) see TROPDLAY for details
%                    model_data.iono (1x8) see IONODLAY for details
%                    model_data.rec_clock (1x2) see CLOCKERR for details
%                    model_data.line_bias (nx3) [rec_num ant_num line_bias_sigma] 
%                                default = 1 meter for all line biases
%
% Note:  For the troposphere model, a value of 1 = modified Hopfield model
%                                   a value of 2 = UNB1 model (altitude dependent)
%
% Note:  If a single user time and position set is input with multiple GPS
%        satellite time/positions, the user is considered to be stationary and
%        PR and CPH measurements will be generated for all GPS times.  For
%        this special case, no obscure_info will be returned (obscure_info = []).
%
% Output:
%   t_pr   - GPS time associated with the PR, [GPS_week GPS_sec] (kx2)
%              k = num_time_steps x number of visible satellites
%   prn    - satellite number for this pseudo-range (kx1)
%   pr     - pseudo-range and accumulated carrier phase measurements associated 
%             with the corresponding t_pr and prn (kx2) [pr cph] (m and cycles)
%   pr_errors - modeled errors added to the geometric range to obtain
%                a pseudorange and carrier phase measurement (m or m/s) (kx13)
%                [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]    
%   pr_ndex   - indices to obtain relationship between output PR matrix and
%                the input positions and velocity [x_user_ind, x_gps_ind] (kx2)
%   obscure_info - contains information needed to determine whether the earth
%                  obscures the line-of-sight (kx3) [tangent_altitude, alpha, beta].
%                  Alpha is the angle between the x1 and x2 vectors. 
%                  Beta is the angle between the x1 vectors and the radius to the
%                  tangent point of the los vectors.
%                  An observation is obscured if the tangent vector magnitude is
%                  below the users tolerance, and alpha is greater than beta. 
%
% See also DOPPLER, SA_EPS, SA_CLOCK, CLOCKERR, TROPDLAY, IONODLAY, VIS_DATA

% References: 
% ICD-GPS-200C-002, 25 Sept, 1997, pages 88-89, 102.
% 
% Global Positioning System: Theory and Applications
% Volume 2, Parkinson and Spilker, pages 8, 28.
%
% 'GPS Theory and Practice", Hoffman-Wellenhoff, pages 89-90.

% Written by: Jimmy LaMance 1/15/97
% Copyright (c) 1998 by Constell, Inc.

% functions called: ERR_CHK, LOS, NORMVECT, SA_EPS, SA_CLOCK, ADDAMBIG,
%                   CLOCKERR, ECEF2LLA, ECEF2NED, NED2AZEL, TROPDLAY, 
%                   IONODLAY, GPST2SEC, KEPLR_EQ
%
%JB variable los renamed to lofs due to variable & function name restriction in Matlab 7


% WGS-84 constants
LIGHT_SPEED = 299792458;       % WGS-84 value in m / s
EARTH_RATE = 7.2921151467e-5;  % WGS-84 value in rad/s 
MU_EARTH = 3.986005e14;        % WGS-84 value in m^3/s^2

% GPS constants
L1_FREQ = 1575.42e6;                      % Hz (1575.42 MHz)
L1_WAVELENGTH = LIGHT_SPEED / L1_FREQ;

%%%%% BEGIN VARIABLE CHECKING CODE %%%%%
% declare the global debug mode
global DEBUG_MODE

% Initialize the output variables
t_pr=[]; prn=[]; pr=[]; pr_orb=[]; pr_errors=[]; obscure_info=[];

% Check the number of input arguments and issues a message if invalid
msg = nargchk(6,12,nargin);
if ~isempty(msg)
  fprintf('%s  See help on PSEUDO_R for details.\n',msg);
  fprintf('Returning with empty outputs.\n\n');
  return
end

if nargin < 7
  model = [1 1 1 1 1 1 1 0 0 0 0 0];     % set to the default value
end % if nargin > 5

if nargin < 8
  seed = 0;     % set to the default value
end % if nargin > 6

if nargin < 9  
  code_noise = 1;
  carrier_noise = .01;
end % if nargin < 8

if nargin < 10  
  carrier_noise = .01;
end % if nargin < 9

% Get the current Matlab version
matlab_version = version;
matlab_version = str2num(matlab_version(1));

% If the Matlab version is 5.x and the DEBUG_MODE flag is not set
% then set up the error checking structure and call the error routine.
if matlab_version >= 5.0                        
  estruct.func_name = 'PSEUDO_R';

  % Develop the error checking structure with required dimension, matching
  % dimension flags, and input dimensions.
  estruct.variable(1).name = 't_user';
  estruct.variable(1).req_dim = [901 2];
  estruct.variable(1).var = t_user;
  estruct.variable(1).type = 'GPS_TIME';
  
  estruct.variable(2).name = 'x_user';
  estruct.variable(2).req_dim = [901 3; 901 4; 901 5; 1 3; 1 4; 1 5];
  estruct.variable(2).var = x_user;
  
  estruct.variable(3).name = 'v_user';
  estruct.variable(3).req_dim = [901 3; 1 3];
  estruct.variable(3).var = v_user;
  
  estruct.variable(4).name = 't_gps';
  estruct.variable(4).req_dim = [902 2];
  estruct.variable(4).var = t_gps;
  estruct.variable(4).type = 'GPS_TIME';
  
  estruct.variable(5).name = 'x_gps';
  estruct.variable(5).req_dim = [902 4];
  estruct.variable(5).var = x_gps;
  
  estruct.variable(6).name = 'v_gps';
  estruct.variable(6).req_dim = [902 3];
  estruct.variable(6).var = v_gps;
  
  estruct.variable(7).name = 'model';
  estruct.variable(7).req_dim = [1 11];
  estruct.variable(7).var = model;
  
  estruct.variable(8).name = 'seed';
  estruct.variable(8).req_dim = [1 1];
  estruct.variable(8).var = seed;
  
  estruct.variable(9).name = 'code_noise';
  estruct.variable(9).req_dim = [1 1];
  estruct.variable(9).var = code_noise;
  
  estruct.variable(10).name = 'carrier_noise';
  estruct.variable(10).req_dim = [1 1];
  estruct.variable(10).var = carrier_noise;
  
  % Call the error checking function
  stop_flag = err_chk(estruct);
  
  if stop_flag == 1           
    fprintf('Invalid inputs to %s.  Returning with empty outputs.\n\n', ...
             estruct.func_name);
    return
  end % if stop_flag == 1
end % if matlab_version >= 5.0 & isempty(DEBUG_MODE) 

clear estruct
%%%%% END VARIABLE CHECKING CODE %%%%%

%%%%% BEGIN ALGORITHM CODE %%%%%

% Determine how many receivers and how many antenna per receiver are being used
insert_comb_nums = 0;     % no combination numbers
if size(x_user,2) == 4              % more than one receiver is in use
  rec_nums = x_user(:,1); 
  ant_nums = ones(size(x_user,1),1);

% If there are numtiple antenna in use, store the antenna numbers in a separate
% matrix and remove that information from the x_user matrix.  Also we must 
% compute a unique ID number for each receiver/antenna for use in LOS.
% Initialize the flag indicating if combination receiver/ant numbers are used,
elseif size(x_user,2) == 5
  rec_nums = x_user(:,1); 
  ant_nums = x_user(:,2);                                             
  
  % Compute unique receiver/antenna numbers for LOS.  This is limited to 10000 
  % receivers at a time.  If you need to do more than that increase this
  % to something larger and call technical support to let them know.  Please.
  comb_num = x_user(:,1) * 10000 + ant_nums;
  insert_comb_nums = 1;     % combination numbers
    
  x_user = [comb_num x_user(:,3:5)];
else      % input is nx3
  rec_nums = ones(size(x_user,1),1);
  ant_nums = ones(size(x_user,1),1);
  x_user = [rec_nums x_user];      % this sets x_user to nx4 from here on
end % if size(x_user,2) > 3
  
% Compute line-of-sight vectors.  With the vectorized LOS routine, it will
% accept mutiple receiver numbers without an external loop.  If the x_user and
% x_gps matrices are the same length, they are assumed to already be aligned
% and only those PR measurements are computed.
if size(x_user,1) == size(x_gps,1)
  lofs = x_gps(:,2:4) - x_user(:,2:4);
  t_pr = t_gps;
  pr_ndex = [1:size(x_user,1); 1:size(x_user,1)]';
else
  [t_pr,lofs,pr_ndex,obscure_info] = los(t_user,x_user,t_gps,x_gps);  
end % if size(x_user,1) == size(x_gps,1)

% Compute the magnitude and direction of the LOS vector.  The magnitude is
% the starting point for the pseudorange (pr) measurement.
[los_norm, pr] = normvect(lofs); 

% Model Earth rotation
if model(10) == 1
  % Compute the pseudorange in seconds
  pr_seconds = pr ./ LIGHT_SPEED;

  % Correct the user position for Earth rotation, per ICD-GPS-200C-002
  % 25 Sept, 1997, section 20.3.3.4.3.4, page 102.
  earth_rot_corr(:,1) = -EARTH_RATE * x_user(pr_ndex(:,1),2) .* pr_seconds;
  earth_rot_corr(:,2) = EARTH_RATE * x_user(pr_ndex(:,1),1) .* pr_seconds;

  % Use the indices from LOS to obtain which x_user are associated with the LOS,
  % and therefore, PR measurement.
  x_new(:,1:2) = x_user(pr_ndex(:,1),2:3) + earth_rot_corr;
  x_new(:,3) = x_user(pr_ndex(:,1),4);

  % Recompute the LOS and PR with the new user positions
  lofs = x_gps(pr_ndex(:,2),2:4) - x_new;
  [los_norm, pr] = normvect(lofs); 

end % if model(10) == 1

% Model satellite motion.  Reference: 'GPS Theory and Practice", 
% Hoffman-Wellenhoff, pages 89-90.
if model(8) == 1
  % Compute the pseudorange in seconds
  pr_seconds = pr / LIGHT_SPEED;

  % compute the relative velocity of the user trajectory and the GPS satellite
  vel = v_gps(pr_ndex(:,2),1:3) - v_user(pr_ndex(:,1),1:3);

  % compute the Doppler shift as the dot product of the LOS with the velocity
  doppler = dot(los_norm',vel')';

  % Compute the correction for satellite motion
  sat_motion = doppler .* pr_seconds;
  
  % Correct the PR measurements
  pr = pr + sat_motion;

else
  sat_motion = zeros(size(pr));  
end % if model(8) == 1