ecef2llh.m

利用gps多天线载波相位技术

%%========================================
%%     Toolbox for attitude determination
%%     Zhen Dai
%%     [e-mail]
%%     ZESS, University of Siegen, Germany
%%     Last Modified  : 1.Sep.2008
%%========================================
%% Functions:
%%      Convert the Cartesian coordinate to ellipsoidal coordinate 
%% Input parameters:
%%      vXYZ -> Cartesian coordinate   
%% Output:
%%     vLLH ->  ellipsoidal coordinate of Latitude,longitude and height
%%  References:
%%       B.Hofmann-Wellenhof, H.Lichtenegger and J.Collins: GPS Theory
%%       and practice. 2001. Fifth revised edition. Springer, Wien, New York.
%%       pp.280-282 

function vLLH = ECEF2LLH(vXYZ)

%% Earth radius in meters
a = 6378137.0000;
%% Earth semiminor in meters
b = 6356752.3142;	
%% Following the procedure proposed by the text book 
e2 =  (a*a-b*b )/ (a*a);
X=vXYZ(1);
Y=vXYZ(2);
Z=vXYZ(3);
p=sqrt(X*X+Y*Y);
e_2=(a*a-b*b)/(b*b);
theta=atan(Z*a/p/b);
lat=atan((Z+e_2*b*sin(theta)^3)/(p-e2*a*cos(theta)^3));
lon=atan(Y/X);
N=a*a/sqrt(a*a*cos(lat)^2+b*b*sin(lat)^2);
h=p/cos(lat)-N;

vLLH=[lat lon h];