www.gusucode.com > gps卫星位置预报，MATLAB编写，有动画界面源码程序 > code5/gps_final_new/xyz2llh.m

    function pos_llh=xyz2llh(pos_ecef)
%function pos_llh=xyz2llh(pos_ecef)
%
%  Input:   a 3x1 position vector in WGS-84 ECEF Coordinates
%           units for pos_ecef are in meters
%  Output:  a 3x1 position vector in Latitude, Longitude, Height 
%           coordinates, as defined by the WGS-84 ellipsoid.
%           units for latitude and longitude are degrees, height
%           in meters
%  
%  Derived from Table 2.1 of Kaplan, Understanding GPS, p. 28.
%  Author:  G. Lightsey, The Univ. of Texas at Austin
%  Last Modified:  01/25/01
%
k=size(pos_ecef);
if ((k(1)~=3)|(k(2)~=1)),
   s=['vector length error, size(pos_ecef) = [',int2str(k),']'];
   disp(s);
   break;
end
xu=pos_ecef(1);  xu_squared=xu^2;
yu=pos_ecef(2);  yu_squared=yu^2;
zu=pos_ecef(3);  zu_squared=zu^2;
%
if ((xu==0)&(yu==0)),
   s=['range error, lat/lon indeterminate'];
   disp(s);
   break;
end
%
r=sqrt(xu_squared+yu_squared);
r_squared=xu_squared+yu_squared;
a=6378137.0; % semimajor axis of WGS-84 ellipsoid in meters
a_squared=a^2;
b=6356752.3142; % semiminor axis of WGS-84 ellipsoid in meters
b_squared=b^2;
e_squared=1-b_squared/a_squared; % eccentricity of WGS-84 ellipsoid (squared)
E_squared=a_squared-b_squared;
F=54*b_squared*zu_squared;
G=r_squared+(1-e_squared)*zu_squared-e_squared*E_squared;
c=(e_squared)^2*F*r_squared/(G^3);
s=(1+c+sqrt(c^2+2*c))^(1/3);
P=F/(3*(s+1/s+1)^2*G^2);
Q=sqrt(1+2*(e_squared^2)*P);
r0=-P*e_squared*r/(1+Q)+sqrt(1/2*a_squared*(1+1/Q)-P*(1-e_squared)*zu_squared/(Q*(1+Q))-1/2*P*r_squared);
U=sqrt((r-e_squared*r0)^2+zu_squared);
V=sqrt((r-e_squared*r0)^2+(1-e_squared)*zu_squared);
z0=b_squared*zu/(a*V);
%
h=real(U*(1-b_squared/(a*V)));
%
if (h<=-b),
   s=['range error, altitude indeterminate'];
   break;
end
phi=180/pi*real(atan((zu+a_squared/b_squared*e_squared*z0)/r));
lam=180/pi*real(atan2(yu,xu));
%
pos_llh=[phi;lam;h];