los.m

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

function [t_los,los_vect,los_indices,obscure_info] = los(t1,x1,t2,x2, ...
                                                      earth_model_flag);

% [t_los,los_vect,los_indices,obscure_info] = los(t1,x1,t2,x2,earth_model_flag)
% 
% Compute line-of-sight (LOS) vectors from a group of objects to another
% group of objects, each identified by object number within the group.  For 
% example, this function allows computation of LOS vectors from a satellite
% (group #1) to the GPS constellation of satellites (group #2).  Group #2
% could also be a group of ground stations.  This function is used for LOS
% computation for all ground assets, satellites, and constellation combinations.
%
% Input:
%   t1  - GPS time vector for group #1 objects (mx2) [GPS_week GPS_sec]
%          valid GPS_week values are 1-3640 (years 1980-2050)
%          valid GPS_sec values are 0-604799
%   x1  - positions for group #1 vehicles (mx3) [x y z] or (mx4) [veh_num1 x y z]
%   t2  - GPS time vector for group #2 objects (nx2) [GPS_week GPS_sec]
%          valid GPS_week values are 1-3640 (years 1980-2050)
%          valid GPS_sec values are 0-604799
%   x2  - positions for group #2 vehicles (nx3) [x y z] or (nx4) [veh_num2 x y z], 
%          veh_num is a vehicle identification number, not required for either
%          group #1 or group #2.
%   earth_model_flag - 0 for spherical earth, 1 for oblate earth (1x1).
%                      Default = spherical earth. (Optional). Used only if
%                      obscure_info is requested as output. The tangent of the los_vect
%                      to the earth is computed. Note: For oblate earth, input vectors
%                      x1 and x2 must be in ECEF coordinates.                    
%                      Not used for visibility from a ground-based site.
%
%   Note: If a single time for a given object number is given for either input set
%         (e.g. x1 or x2 data), it is assumed to be an object fixed to the surface
%         of the Earth.  This objects position will be fixed and LOS computed
%         for each of the times in the other object (e.g. if the ground station
%         is entered in x1 data, then the times from x2 will be used for the
%         LOS computations).  The Earth fixed objects are assumed to be in ECEF
%         coordinates.  Care should be taken when inputing Earth fixed objects
%         in ECI or other non-ECEF coordinate frames.
%   Note: Group #1 and group #2 positions must be in the same reference system 
%          (e.g. ECEF or ECI).
% Output:
%   t_los        - GPS time vector corresponding to the LOS (kx2) [GPS_week GPS_sec]
%   los_vect          - line of sight vector at t_los from group #1 object to 
%                  group #2 object (kx3).  LOS are computed only 
%                  for matching times in t1 and t2.
%   los_indices  - indices to obtain relationship between output LOS vectors and
%                  the input positions [x1_ind, x2_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. 
%
% Note: The los_indices can be used to track which velocities or other
%       data associated with the vehicle trajectory go with each line of sight.
%       For example, to rotate the ECEF LOS vector to a satellite local
%       level coordinate system via ECEF2LL, the position and velocity of the
%       vehicle are required.  The ECEF2LL function call would be the following
%       assuming the original vehicle position and velocity matrices were x1 & v1
%         [x_ll] = ecef2ll(los_vect, x1(los_indices(:,1),:), v1(los_indices(:,1),:));
%       See help on ECEF2LL for details about these inputs.
%
% See also PROPGEPH

% Written by: Jimmy LaMance 10/24/96
% Modified by: Maria Evans, May 98
% Copyright (c) 1998 by Constell, Inc.

% functions called: ERR_CHK, GPST2SEC, REMAP, SEC2GPST, TANELLIP

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

% Initialize the output variables
t_los = []; los_vect = []; veh_num1 = []; veh_num2 = []; x1_out = []; x2_out = [];
x1_ind = []; x2_ind = [];

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

estruct.func_name = 'LOS';

% Develop the error checking structure with required dimension, matching
% dimension flags, and input dimensions.
estruct.variable(1).name = 't1';
estruct.variable(1).req_dim = [901 2];
estruct.variable(1).var = t1;
estruct.variable(1).type = 'GPS_TIME';
  
estruct.variable(2).name = 'x1';
estruct.variable(2).req_dim = [901 3; 901 4];
estruct.variable(2).var = x1;
  
estruct.variable(3).name = 't2';
struct.variable(3).req_dim = [902 2];
estruct.variable(3).var = t2;
estruct.variable(3).type = 'GPS_TIME';
  
estruct.variable(4).name = 'x2';
estruct.variable(4).req_dim = [902 3; 902 4];
estruct.variable(4).var = x2;
  
if nargin==5,
  estruct.variable(4).name = 'earth_model_flag';
  estruct.variable(4).req_dim = [1 1];
  estruct.variable(4).var = earth_model_flag;
else,
  earth_model_flag = 0;
end;
 
% 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 VARIABLE CHECKING CODE %%%%%

%%%%% BEGIN ALGORITHM CODE %%%%%
RE = 6378137.0;

% If x1 is nx3, resize to be nx4 with 1's in the satellite number column
if size(x1,2) == 3 
  x1(:,2:4) = x1;
  x1(:,1) = ones(size(x1(:,1)));
end % size(x1,2) == 3

if size(x2,2) == 3 
  x2(:,2:4) = x2;
  x2(:,1) = ones(size(x2(:,1)));
end % size(x2,2) == 3

% Convert to linear time in seconds past the GPS epoch                                      
t1 = gpst2sec(t1);
t2 = gpst2sec(t2);

% Find all of the single times/position for the group #1 and group #2 input data.
% These are assumed to be fixed to the Earth and the ECEF positions (assumed ECEF)
% will be duplicated for all of the times in the other group set.
% Check the size of the t1 input, if it is 1x1, then expand it to be
% the same times as t2 and assume the position is constant
% Find the unique input times for the combined x1 and x2 data
t_all = unique([t1; t2]);

% Find the unique vehicle numbers in data set 1
vn1 = unique(x1(:,1));

% Set up indices indicating if this is a ground station in the group.  Used later
% in re-establishing the output indices.
ind_x1 = zeros(length(vn1),1);
sta_num_x1 = zeros(length(vn1),1);

x1_org = x1;
x2_org = x2;

% Loop over the vehicle #1 data to find the unique times
for i = 1:length(vn1)
  I = find(x1_org(:,1) == vn1(i));
  
  % Find the unique times for this data
  tu = unique(t1(I));
  
  % If this is a single time, expand it to be the size of t_all
  if length(tu) == 1
    % Start by removing the single time point.  Prevents repetition of times
    I_t1 = find(x1(:,1) ~= vn1(i));
    t1 = t1(I_t1);
    x_to_duplicate = x1_org(I(1),:);
    x1 = x1(I_t1,:);
    
    % Add all of the t_all times
    t1 = [t1; t_all];
    x1 = [x1; ones(length(t_all),1) * x_to_duplicate];
    ind_x1(i) = I(1);  
    sta_num_x1(i) = x1_org(I(1),1);

  end % if length(tu) == 1
  
%  clear I tu x_to_duplicate
end % for i = 1:length(vn1)

% Repeat the process for the group #2 data
% Find the unique vehicle numbers in data set 1
vn2 = unique(x2(:,1));
ind_x2 = zeros(length(vn2),1);
sta_num_x2 = zeros(length(vn2),1);

% Loop over the vehicle #1 data to find the unique times
for i = 1:length(vn2)
  I = find(x2_org(:,1) == vn2(i));
  
  % Find the unique times for this data
  tu = unique(t2(I));
  
  % If this is a single time, expand it to be the size of t_all
  if length(tu) == 1
    % Start by removing the single time point.  Prevents repetition of times
    I_t2 = find(x2(:,1) ~= vn2(i));
    t2 = t2(I_t2);
    x_to_duplicate = x2_org(I(1),:);
    x2 = x2(I_t2,:);
    
    % Add all of the t_all times
    t2 = [t2; t_all];
    x2 = [x2; ones(length(t_all),1) * x_to_duplicate];
    ind_x2(i) = I(1);
    sta_num_x2(i) = x2_org(I(1),1);
  end % if length(tu) == 1
  
  clear I tu x_to_duplicate
end % for i = 1:length(vn2)

% Check to see if the data need to be time sorted  
dt1 = diff(t1);
dt2 = diff(t2);

if any(dt1 < 0);
  [t1, I_sort] = sort(t1);
  x1 = x1(I_sort,:);
end
if any(dt2 < 0);
  [t2, I_sort] = sort(t2);
  x2 = x2(I_sort,:);
end

% Find the intersection of the 2 time sets.  These are the unique
% intersection points with duplicates deleted.  The indices point to 
% the last of a run of common points (eg t1 = [1 1 1] -> I_t1 = 3)
[t_intersect, I_t1, I_t2] = intersect(t1,t2);

% Make sure that there are some common time intersections
if isempty(t_intersect)
  fprintf('No common times in t1 and t2 data in LOS.\n')
  return
end

% Set up indices to all the common times.  This will also include
% the non-common times at this point.  The non-common times will
% be removed later.
t1_all_index = zeros(size(t1));
t2_all_index = zeros(size(t2));

num_t1_intersect = length(I_t1);
num_t2_intersect = length(I_t2);

% Remove the last intersection point from both index data sets
I_t1 = I_t1(1:end-1);
I_t2 = I_t2(1:end-1);

% Insert a 1 at all of the intersection time changes.
t1_all_index(I_t1+1) = ones(size(I_t1));
t2_all_index(I_t2+1) = ones(size(I_t2));

% Put a 1 at the starting index
t1_all_index(1) = 1;
t2_all_index(1) = 1;           

% Use the cumsum function to create the indices to the intersection of the 
% times.  This will results in matrices like [1 1 1 1 2 2 2 2... n n n].
t1_all_index = cumsum(t1_all_index);
t2_all_index = cumsum(t2_all_index); 

% Set up another set of time matrices that are the same size as the initial
% matrices, but only contain times that are the intersection of the two time
% matrices.  This will be used to eliminate the non-intersection times.
t1_new = t_intersect(t1_all_index);
t2_new = t_intersect(t2_all_index);

% Find all of the valid
I_t1_keep = find(t1 == t1_new);
I_t2_keep = find(t2 == t2_new);

% Eliminate time tags for data not used in the comparison
t1 = t1(I_t1_keep);
x1 = x1(I_t1_keep,:);
t2 = t2(I_t2_keep);
x2 = x2(I_t2_keep,:);
t1_index = t1_all_index(I_t1_keep);
t2_index = t2_all_index(I_t2_keep);

% Find all of the unique satellite/station numbers for both data sets.  There will
% be a loop over the smaller of the two sets.
prn_unique_1 = unique(x1(:,1));
t_unique_1 = unique(t1);
prn_unique_2 = unique(x2(:,1));
t_unique_2 = unique(t2);

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

% Switch group #1 and group #2 if required to make group #1 have the fewest 
% number of vehicles.
swap_12 = 0;               % flag indicating if the swap has occured
if num_prn1 > num_prn2
  t2_temp = t2;
  x2_temp = x2;
  t2_index_temp = t2_index;
  t2 = t1;
  x2 = x1;
  t2_index = t1_index;
  t1 = t2_temp;
  x1 = x2_temp;       
  t1_index = t2_index_temp;
  
  prn_unique_1 = unique(x1(:,1));
  t_unique_1 = unique(t1);
  prn_unique_2 = unique(x2(:,1));
  t_unique_2 = unique(t2);