topocent.m

《 software Defined GPS and Galilleo Receiver》源码

function [Az, El, D] = topocent(X, dx)
%TOPOCENT  Transformation of vector dx into topocentric coordinate
%          system with origin at X.
%          Both parameters are 3 by 1 vectors.
%
%[Az, El, D] = topocent(X, dx);
%
%   Inputs:
%       X           - vector origin corrdinates (in ECEF system [X; Y; Z;]) 
%       dx          - vector ([dX; dY; dZ;]). 
%
%   Outputs:
%       D           - vector length. Units like units of the input
%       Az          - azimuth from north positive clockwise, degrees
%       El          - elevation angle, degrees

%Kai Borre 11-24-96
%Copyright (c) by Kai Borre
%
% CVS record:
% $Id: topocent.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $
%==========================================================================

dtr = pi/180;

[phi, lambda, h] = togeod(6378137, 298.257223563, X(1), X(2), X(3));

cl  = cos(lambda * dtr);
sl  = sin(lambda * dtr);
cb  = cos(phi * dtr); 
sb  = sin(phi * dtr);

F   = [-sl -sb*cl cb*cl;
        cl -sb*sl cb*sl;
        0    cb   sb];

local_vector = F' * dx;
E   = local_vector(1);
N   = local_vector(2);
U   = local_vector(3);

hor_dis = sqrt(E^2 + N^2);

if hor_dis < 1.e-20
    Az = 0;
    El = 90;
else
    Az = atan2(E, N)/dtr;
    El = atan2(U, hor_dis)/dtr;
end

if Az < 0
    Az = Az + 360;
end

D   = sqrt(dx(1)^2 + dx(2)^2 + dx(3)^2);
%%%%%%%%% end topocent.m %%%%%%%%%