los.m

此功能包用于各种GPS坐标和时间的转换

  num_prn1 = length(prn_unique_1);
  num_t1 = length(t_unique_1);
  num_prn2 = length(prn_unique_2);
  num_t2 = length(t_unique_2);  
  
  I_t1_keep_temp = I_t1_keep;
  I_t1_keep = I_t2_keep;
  I_t2_keep = I_t1_keep_temp;

  swap_12 = 1;                       % set the swap flag
end % if num_prn1 > num_prn2

% Remap the PRN 2 data to be indexed as 1, 2, ... num_prn2
[prn2_index] = remap(x2(:,1));

% Create a matrix filled with inf the size of the satellite 2 data
% assuming that each colunm is a time and the rows are satellite numbers
d2_prn = ones(num_prn2,num_t2) * inf;      % vehicle number
d2_x = ones(num_prn2,num_t2) * inf;        % X-component
d2_y = ones(num_prn2,num_t2) * inf;        % Y-component
d2_z = ones(num_prn2,num_t2) * inf;        % Z-component
d2_t = ones(num_prn2,num_t2) * inf;        % time
  
% Fill each of the times and available PRN data with ones for satellite 2.
% This index is used to fill in the data for satellite/constellation #2.
I_d2 = sub2ind(size(d2_x),prn2_index',t2_index);

% Fill in the X, Y, and Z data for the group #2 assets
d2_prn(I_d2) = x2(:,1);
d2_x(I_d2) = x2(:,2);
d2_y(I_d2) = x2(:,3);
d2_z(I_d2) = x2(:,4);
d2_t(I_d2) = t2;

% Check that the matrix dimension for the x2 data match.  This can cause
% a problem when there is repeated time data points without the appropriate
% satellite numbers.
if size(I_t2_keep,1) ~= size(d2_x,2) *  size(d2_x,1)
  fprintf('Problem detected in LOS.\n')
  fprintf('Verify that the any constellation position data has the satellite\n')
  fprintf('numbers in addition to the position information.  Also verify that\n')
  fprintf('there are no repeated time/position/satellite number combinations.\n')
  fprintf('Returning from LOS with empty output matrices.\n\n');
  dbstack
  fprintf('\n\n')                                            
  return
end % if size(I_t2_keep,1) ~= size(d2_x,2)

% Initialize the output variable 
los_vect = [];
t_los_sec = [];
veh_num1 = [];
veh_num2 = [];
x1_out = [];
x2_out = [];
x1_ind = [];
x2_ind = [];

% Loop over the vehicle numbers in group #1
for i = 1:num_prn1
  % Find all of the time #1 where this PRN is used.  This will have a single
  % index for each time.
  this_prn = prn_unique_1(i);
  I_times_1 = find(x1(:,1) == this_prn);
                                       
  % Get the indices to the time when this PRN is available
  d1_index = t1_index(I_times_1)';                         
  
  % Create a vector with infinity the size of the whole common time series
  d1_row_one_x = ones(1,size(d2_x,2)) * inf;
  d1_row_one_y = ones(1,size(d2_x,2)) * inf;
  d1_row_one_z = ones(1,size(d2_x,2)) * inf;
  
  % Fill in x, y, and z position data at time when this PRN is used
  d1_row_one_prn(d1_index) = x1(I_times_1,1);
  d1_row_one_x(d1_index) = x1(I_times_1,2);
  d1_row_one_y(d1_index) = x1(I_times_1,3);
  d1_row_one_z(d1_index) = x1(I_times_1,4);
  
  % Make this the same size as the d1 (group #2) data
  d1_prn = ones(size(d2_x,1),1) * d1_row_one_prn;
  d1_x(i,:,:) = ones(size(d2_x,1),1) * d1_row_one_x;
  d1_y(i,:,:) = ones(size(d2_x,1),1) * d1_row_one_y;
  d1_z(i,:,:) = ones(size(d2_x,1),1) * d1_row_one_z;   

  d2_x_all(i,:,:) = d2_x;
  d2_y_all(i,:,:) = d2_y;
  d2_z_all(i,:,:) = d2_z;   
  t_los_sec(i,:,:) = d2_t;

  veh_num1(i,:,:) = d1_prn;
  veh_num2(i,:,:) = d2_prn;
  x1_ind(i,:,:) = ones(size(d2_x,1),1) * I_t1_keep(I_times_1)'; 
  x2_ind(i,:,:) = reshape(I_t2_keep,size(d2_x)); 
  
end

% Compute the LOS for this group #1 vehicle
los_x = d2_x_all - d1_x;
los_y = d2_y_all - d1_y;
los_z = d2_z_all - d1_z;

% Remove the LOS that are INF (no group #2 or group #1 data)
I_valid_los = find(isinf(los_x) == 0);

num_los = length(I_valid_los);    

% Reshape the output to be vectors while eliminating the invalid points
los_x = reshape(los_x(I_valid_los),num_los,1);
los_y = reshape(los_y(I_valid_los),num_los,1);
los_z = reshape(los_z(I_valid_los),num_los,1);
t_los_sec = reshape(t_los_sec(I_valid_los),num_los,1);
veh_num1 = reshape(veh_num1(I_valid_los),num_los,1);  
veh_num2 = reshape(veh_num2(I_valid_los),num_los,1);  
  
x1_ind = reshape(x1_ind(I_valid_los),num_los,1);
x2_ind = reshape(x2_ind(I_valid_los),num_los,1);

d1_x = reshape(d1_x(I_valid_los),num_los,1);
d1_y = reshape(d1_y(I_valid_los),num_los,1);
d1_z = reshape(d1_z(I_valid_los),num_los,1);

d2_x_all = reshape(d2_x_all(I_valid_los),num_los,1);
d2_y_all = reshape(d2_y_all(I_valid_los),num_los,1);
d2_z_all = reshape(d2_z_all(I_valid_los),num_los,1);

los_vect = [los_x los_y los_z];
x1_out = [d1_x d1_y d1_z];
x2_out = [d2_x_all d2_y_all d2_z_all];
  
% See if the group #1 and group #2 data were swapped, if so swap them back
if swap_12 == 1                                                           
  los_vect = -los_vect;
  veh_num1_temp = veh_num1;
  veh_num1 = veh_num2;
  veh_num2 = veh_num1_temp;
  x1_out_temp = x1_out;
  x1_out = x2_out;
  x2_out = x1_out_temp;
  x1_ind_temp = x1_ind;
  x1_ind = x2_ind;
  x2_ind = x1_ind_temp;

end % if swap_12 == 1

% Clean up x1 and x2 indices if single ground stations time/positions were input
I_x1 = find(ind_x1 ~= 0);
if any(I_x1)
  for i = 1:length(I_x1)
    this_sta_num = sta_num_x1(I_x1(i));
  
    I_replace = find(this_sta_num == veh_num1);
    
    % Replace the corresponding indices in the output 
    x1_ind(I_replace) = ones(size(I_replace)) * ind_x1(i);
  end % for i = 1:length(I_x1)
end % if any(I_x1)

% Clean up x1 and x2 indices if single ground stations time/positions were input
I_x2 = find(ind_x2 ~= 0);
if any(I_x2)
  
  for i = 1:length(I_x2)
    this_sta_num = sta_num_x2(I_x2(i));
  
    I_replace = find(this_sta_num == veh_num2);
    
    % Replace the corresponding indices in the output 
    x2_ind(I_replace) = ones(size(I_replace)) * ind_x2(i);
  end % for i = 1:length(I_x2)
end % if any(I_x2)

% Get time back into full GPS time
t_los = sec2gpst(t_los_sec);

% Fill indices
los_indices = [x1_ind, x2_ind];

if nargout==4,  % Compute the Earth obscuring information if requested
  % Call tanellip to compute obscuring info
  obscure_info = tanellip(earth_model_flag,x1_out,x2_out,los_vect);
end;  % if nargout==4,

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

% end LOS

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%  TANELLIP function within LOS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [obscure_info] = tanellip(earth_model_flag,x1_out,x2_out,los_vect);
% Compute tangent to ellipsoid
RE = 6378137.0;
seminor = 6356752.314;         % ellipsiod semi-minor axis


%%%%% BEGIN ALGORITHM CODE %%%%%
if earth_model_flag==0,  % Use spherical earth
  % Compute the radius to the tangent point to the Earth's surface
  % First compute the angle between the los vector and x1_out
  [x1_norm, x1_mag] = normvect(x1_out);
  [los_norm, los_mag] = normvect(los_vect);
  [x2_norm, x2_mag] = normvect(x2_out);
  
  % Set the initial values for the tangent altitudes to the target vehicles
  tangent_altitude = x2_mag - RE;
  
  delta = pi - acos(dot(x1_out',los_vect')'./(x1_mag.*los_mag));
  beta = zeros(size(delta));
  below_90 = find(delta < pi/2);
  
  if any(below_90),
    beta(below_90) = (pi/2 - delta(below_90));
    tangent_altitude(below_90) = cos(beta(below_90)).*x1_mag(below_90) - RE;
  end;
  % Cases with delta above pi/2 are clearly not obscured by the earth,
  % so set beta to pi so that alpha is never greater than beta
  above_90 = find(delta > pi/2);
  if any(above_90),
    beta(above_90) = ones(length(above_90),1)*pi;
  end;
  equal_90 = find(delta == pi/2);
  if any(equal_90),
    beta(equal_90) = zeros(length(equal_90),1);
  end;

  alpha = acos(dot(x1_out',x2_out')'./(x1_mag.*x2_mag));
  obscure_info = [tangent_altitude, alpha, beta];

else,  % Oblate Earth model
  % Iterate to compute the lat and lon of the tangent altitude.
  % Use FMIN function to compute the minimum

  % First compute the angle between the los vector and x1_out
  [x1_norm, x1_mag] = normvect(x1_out);
  [los_norm, los_mag] = normvect(los_vect);
  [x2_norm, x2_mag] = normvect(x2_out);

  % Set the initial tangent altitude to be the altitude of the x1_out positions.
  % The tangent altitude that correspond to delta angles less than 90
  % will be recomputed below.
  x1_lla = ecef2lla(x1_out,1);
  tangent_altitude = x1_lla(:,3);

  % Compute the angle, delta, between the x1 position and the LOS vector
  delta = pi - acos(dot(x1_out',los_vect')'./(x1_mag.*los_mag));
  
  % Find the delta angles less than 90 degrees
%  below_90 = find(delta < pi/2);

%  if any(below_90),
    % these are potentially cases which my be obscured by the oblate earth
    % use FMIN to find the minimum altitude above the ellipsoid
%    [los_tangent_length] = fminv('ellipalt',zeros(size(los_mag(below_90))),...
%                               los_mag(below_90),[0,1.e-12], ...
%                               x1_out(below_90,1), x1_out(below_90,2),...
%                               x1_out(below_90,3), los_norm(below_90,1),...
%                               los_norm(below_90,2),los_norm(below_90,3));
%    rt(:,1) = x1_out(below_90,1) + los_norm(below_90,1).*los_tangent_length;
%    rt(:,2) = x1_out(below_90,2) + los_norm(below_90,2).*los_tangent_length;
%    rt(:,3) = x1_out(below_90,3) + los_norm(below_90,3).*los_tangent_length;
    
%    [rt_norm, rt_mag] = normvect(rt);
%    [lla] = ecef2lla(rt);
    
%    tangent_altitude(below_90) = lla(:,3);
%    beta(below_90,1) = asin(sin(delta(below_90)).*los_tangent_length./rt_mag);
%  end;
  
  % Cases with delta above pi/2 are clearly not obscured by the earth,
  % so set beta to pi so that alpha is never greater than beta
%  above_90 = find(delta > pi/2);
%  if any(above_90),
%    beta(above_90,1) = ones(length(above_90),1)*pi;
%  end;
%  equal_90 = find(delta == pi/2);
%  if any(equal_90),
%    beta(equal_90,1) = zeros(length(equal_90),1);
%  end;

  [los_tangent_length] = fminv('ellipalt',zeros(size(los_mag)),...
                             los_mag,[0,1.e-12], ...
                             x1_out(:,1), x1_out(:,2),...
                             x1_out(:,3), los_norm(:,1),...
                             los_norm(:,2),los_norm(:,3));
  rt(:,1) = x1_out(:,1) + los_norm(:,1) .* los_tangent_length;
  rt(:,2) = x1_out(:,2) + los_norm(:,2) .* los_tangent_length;
  rt(:,3) = x1_out(:,3) + los_norm(:,3) .* los_tangent_length;
    
  [rt_norm, rt_mag] = normvect(rt);
  [lla] = ecef2lla(rt);
    
  tangent_altitude(:) = lla(:,3);
  beta = asin(sin(delta).*los_tangent_length ./ rt_mag);

  alpha = acos(dot(x1_out',x2_out')'./(x1_mag.*x2_mag));

  obscure_info = [tangent_altitude, alpha, beta];
end;

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

% end TANELLIP

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%  REMAP function within LOS 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function d_remap = remap(d);

% d_remap = remap(d);
%
% Remaps a vector of data with repeated indices to the values 1 through
% the number of unique entires in d.  This function can be used to take
% a set of satellite numbers and reindex them to be satellites 1 through the
% total number of unique satellites without skipping numbers.  This is a 
% support function for COMP_LOS.