los.m

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

%
% Input: d - vector of numbers to be indexed as as 1, 2, ...n where n is
%            the number of unique inputs.
% 
% Output: d_remap - indexed version of the input, d, where the unique numbers
%                   have been replace with 1 through the maximum number of 
%                   unique numbers

% Written by: Jimmy LaMance 4/14/98
% Copyright (c) 1998 by Constell, Inc.

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

% Initialize the output variables
d_remap=[];

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

estruct.func_name = 'REMAP';

% Develop the error checking structure with required dimension, matching
% dimension flags, and input dimensions.
estruct.variable(1).name = 'd';
estruct.variable(1).req_dim = [901 1];
estruct.variable(1).var = d;

% 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 %%%%%

% Start by sorting the input data
[d_sort, ndx_sort] = sort(d);

% Find all of the unique data points
[d_unique, ndx_unique] = unique(d_sort);

% Remove the last unique index
ndx_unique = ndx_unique(1:end-1);

% Build a matrix with zeros the length of d and put 1's at the changes
z = zeros(size(d));
z(ndx_unique+1) = ones(size(ndx_unique));

% Add a one to start the z vector
z(1) = 1;

% Use the cumsum function to get to 1, 2, 3, ...
z_cum = cumsum(z);     

% Resort the remapped data to the original indices
d_remap(ndx_sort) = z_cum;

%%%%% End of REMAP %%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%  FMINV function within LOS 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [xf,options] = fminv(funfcn,ax,bx,options,varargin)
%FMINV   Minimize function of one variable, vectorized.
%   X = FMINV('F',x1,x2) attempts to return a value of x which is a local 
%   minimizer of F(x) in the interval x1 < x < x2.  'F' is a string 
%   containing the name of the objective function to be minimized.
%
%   X = FMINV('F',x1,x2,OPTIONS) uses a vector of control parameters.
%   If OPTIONS(1) is positive, intermediate steps in the solution are
%   displayed; the default is OPTIONS(1) = 0.  OPTIONS(2) is the termination
%   tolerance for x; the default is 1.e-4.  OPTIONS(14) is the maximum
%   number of function evaluations; the default is OPTIONS(14) = 500.  
%   The other components of OPTIONS are not used as input control 
%   parameters by FMIN.  For more information, see FOPTIONS.
%
%   X = FMINV('F',x1,x2,OPTIONS,P1,P2,...) provides for additional
%   arguments which are passed to the objective function, F(X,P1,P2,...)
%
%   [X,OPTIONS] = FMINV(...) returns a count of the number of steps
%   taken in OPTIONS(10).
%
%   Examples
%       fmin('cos',3,4) computes pi to a few decimal places.
%       fmin('cos',3,4,[1,1.e-12]) displays the steps taken
%       to compute pi to about 12 decimal places.
%
%   See also FMIN, FMINS.

%   Reference: "Computer Methods for Mathematical Computations",
%   Forsythe, Malcolm, and Moler, Prentice-Hall, 1976.

% Taken from original MATLAB function FMIN.
% Vectorized by Jimmy LaMance, June 1998
% Constell, Inc. 

% initialization
if nargin<4, options = []; end
options = foptions(options);
tol = options(2);
if (~options(14))
    options(14) = 500; 
  end
if nargin < 3
  error('fmin requires three input arguments.')
end

header = ' Func evals     x           f(x)         Procedure';
step='       initial';

% Convert to inline function as needed.
funfcn = fcnchk(funfcn,length(varargin));

num = 1;
seps = sqrt(eps);
c = 0.5*(3.0 - sqrt(5.0));
a = ax; b = bx;
v = a + c*(b-a);
w = v; xf = v;
d = 0.0; e = 0.0;
x= xf; fx = feval(funfcn,x,varargin{:});  
% fmin_data = [ 1 xf fx ];               % original code
fmin_data = [ ones(size(xf)) xf fx ];    % vectorized code

fv = fx; fw = fx;
xm = 0.5*(a+b);
tol1 = seps*abs(xf) + tol/3.0;   tol2 = 2.0*tol1;

d = ones(size(xf)) * -inf;
e = ones(size(xf)) * -inf;
    
% Main loop

options(10) = 0; 
% while ( abs(xf-xm) > (tol2 - 0.5*(b-a)) )         % original version
while ( any(abs(xf-xm) > (tol2 - 0.5*(b-a))) )      % vectorized version

    num = num+1;
    gs = 1;
    I_gs = [1:size(xf,1)];
    
    % Is a parabolic fit possible
    I_poss = find(abs(e) > tol1);
    if ~isempty(I_poss)
        % Yes, so fit parabola
        gs = 0;
        r = (xf-w).*(fx-fv);           % vectorized
        q = (xf-v).*(fx-fw);           % vectorized
        p = (xf-v).*q-(xf-w).*r;       % vectorized
        q = 2.0*(q-r);                 
        
        I_q_pos = find(q > 0.0);
        if ~isempty(I_q_pos)
          p(I_q_pos) = -p(I_q_pos); 
        end
        
        q = abs(q);
        r = e;  
        e = d;

        % Is the parabola acceptable
        I_par = find((abs(p)<abs(0.5.*q.*r)) & (p>q.*(a-xf)) & (p<q.*(b-xf)) );
        I_gs = find((abs(p)>=abs(0.5.*q.*r)) | (p<=q.*(a-xf)) | (p>=q.*(b-xf)) ); 

        if ~isempty(I_par)

            % Yes, parabolic interpolation step
            d(I_par) = p(I_par)./q(I_par);
            x(I_par) = xf(I_par)+d(I_par);
            step = '       parabolic';
     
            % f must not be evaluated too close to ax or bx
            I_close = find(x(I_par)-a(I_par) < tol2(I_par) | ...
                           b(I_par)-x(I_par) < tol2(I_par));
            if ~isempty(I_close)                           
                si = sign(xm(I_par(I_close))-xf(I_par(I_close))) + ...
                         ((xm(I_par(I_close))-xf(I_par(I_close))) == 0);
                d(I_par(I_close)) = tol1(I_par(I_close)).*si;
            end
        end % if ~isempty(I_par)
          
        if ~isempty(I_gs)
            % Not acceptable, must do a golden section step
            gs=1;      
            
        end % if ~isempty(I_gs)
        
    end
    if gs
        % A golden-section step is required
        I_big = find(xf(I_gs) >= xm(I_gs));
        I_small = find(xf(I_gs) < xm(I_gs));

        if ~isempty(I_big)
          e(I_gs(I_big)) = a(I_gs(I_big))-xf(I_gs(I_big));    
        end
        
        if ~isempty(I_small)
          e(I_gs(I_small)) = b(I_gs(I_small))-xf(I_gs(I_small));  
        end
        
        d(I_gs) = c*e(I_gs);
        step = '       golden';
    end

    % The function must not be evaluated too close to xf
    si = sign(d) + (d == 0);
    
    x = xf + si .* max( [abs(d'); tol1'] )';       % vectorized version

    fu = feval(funfcn,x,varargin{:});  
    fmin_data = [num*ones(size(x)) x fu];       % vectorized version

    % Update a, b, v, w, x, xm, tol1, tol2
    I_low = find(fu <= fx);
    I_high = find(fu > fx);

    if ~isempty(I_low)                          % vectorized version 
        I_low2 = find(x(I_low) >= xf(I_low));
        I_not_low2 = find(x(I_low) < xf(I_low));

        if ~isempty(I_low2), 
          a(I_low(I_low2)) = xf(I_low(I_low2));
        end
        if ~isempty(I_not_low2) 
          b(I_low(I_not_low2)) = xf(I_low(I_not_low2)); 
        end
        
        v(I_low) = w(I_low); fv(I_low) = fw(I_low);
        w(I_low) = xf(I_low); fw(I_low) = fx(I_low);
        xf(I_low) = x(I_low); fx(I_low) = fu(I_low); 
    end

    if ~isempty(I_high)                          % vectorized version 
        I_high2 = find(x(I_high) < xf(I_high));
        I_not_high2 = find(x(I_high) >= xf(I_high));

        if ~isempty(I_high2)
          a(I_high(I_high2)) = x(I_high(I_high2));            
        end
        
        if ~isempty(I_not_high2)
        % else,
          b(I_high(I_not_high2)) = x(I_high(I_not_high2)); 
        end
        
        I_1 = find((fu(I_high) <= fw(I_high)) | (w(I_high) == xf(I_high)) );
        if ~isempty(I_1)
            v(I_high(I_1)) = w(I_high(I_1)); 
            fv(I_high(I_1)) = fw(I_high(I_1));
            w(I_high(I_1)) = x(I_high(I_1)); 
            fw(I_high(I_1)) = fu(I_high(I_1));
        end
        
        I_1 = find((fu(I_high) <= fv(I_high)) | (v(I_high) == xf(I_high)) | ...
                   (v(I_high) == w(I_high)) );
        % elseif ( any(fu <= fv) | any(v == xf) | any(v == w) ) 
        if ~isempty(I_1)
            v(I_high(I_1)) = x(I_high(I_1)); 
            fv(I_high(I_1)) = fu(I_high(I_1));
        end
    end

    xm = 0.5*(a+b);
    
    tol1 = seps*abs(xf) + tol/3.0; 
    tol2 = 2.0*tol1;
    
    if num > options(14)
        if options(1) >= 0
        disp('Warning: Maximum number of function evaluations has been');
        disp('         exceeded - increase options(14).')
        options(10)=num;
        return
        end
    end
end
%options(8) = feval(funfcn,x,varargin{:});
options(10)=num;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%  ELLIPALT function within LOS 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function fx = ellipalt(los_length, x1, x2, x3, xlos_unit1, xlos_unit2, xlos_unit3);
% Provided a length of the los vector, compute the corresponding tangent altitude
% above WGS 84 ellipsoid.

rt(:,1) = x1 + xlos_unit1 .* los_length;
rt(:,2) = x2 + xlos_unit2 .* los_length;
rt(:,3) = x3 + xlos_unit3 .* los_length;

[lla] = ecef2lla(rt,1);
fx = lla(:,3);

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

% end ELLIP_ALT

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

% end REMAP