xyztolatlonhgt1.m

地心地固坐标系到大地坐标系的转换算法

function [lat, lon, hgt] = XYZToLatLonHgt1(x,y,z)
    eps = 1.0E-13;
    dpi2 = 1.570796326794897E0;

    a = 6378137.0E0;
    b = 6356752.3142E0;
    e =sqrt(1-(b*b)/(a*a));
    pi=3.1415926535898;
    rho = sqrt(x*x+y*y);
    if (rho <= eps)
	    lat = dpi2; 
	    if(z<0.0)
	        lat = -lat;
        end
	    lon = 0.0E0;
	    hgt = fabs(z) - b;
    end
    
    lon = atan2(y,x);
    lat = atan2(z,rho);
    hgt = rho/cos(lat);

    latold = lat + 1.0E0;
    hgtold = hgt + 1.0E0;
    while(abs(lat-latold)>=eps | abs(hgt-hgtold)>=0.01)
	    latold = lat;
	    hgtold = hgt;
	    d = e*sin(latold);
	    n = a/sqrt(1.0E0-d*d);
	    hgt = rho/cos(latold) - n;
	    nph = n + hgt;
	    d = 1.0E0 - e*e*(n/nph);
	    lat = atan2(z,rho*d);
    end