enu转wgs_matlab_matlab中enu转换_经纬_ENU坐标_经纬度_ENU转wgs坐标

function [lat0, lon0, h0]=ecef_to_geodetic(x, y, z) % # Convert from ECEF cartesian coordinates to % # latitude, longitude and height. WGS-84 a = 6378137; b = 6356752.3142; f = (a - b) / a; e_sq = f * (2-f); pi = 3.14159265359; x2 = x .^ 2 ; y2 = y .^ 2 ; z2 = z .^ 2 ; a = 6378137.0000; %earth radius in meters b = 6356752.3142 ; %earth semiminor in meters e = sqrt (1-(b/a)^2); b2 = b*b ; e2 = e ^ 2; ep = e*(a/b); r = sqrt(x2+y2); r2 = r.*r ; E2 = a ^ 2 - b ^ 2; F = 54.*b2.*z2 ; G = r2 + (1-e2).*z2 - e2*E2 ; c = (e2.*e2.*F.*r2)./(G.*G.*G) ; s = ( 1 + c + sqrt(c.*c + 2.*c) ).^(1/3) ; P = F ./ (3 .* (s+1./s+1).^2 .* G.*G) ; Q = sqrt(1+2.*e2.*e2.*P) ; ro = -(P.*e2.*r)./(1+Q) + sqrt((a*a/2).*(1+1./Q) - (P.*(1-e2).*z2)./(Q.*(1+Q)) - P.*r2/2); tmp = (r - e2.*ro) .^ 2 ; U = sqrt( tmp + z2 ) ; V = sqrt( tmp + (1-e2).*z2 ) ; zo = (b2.*z)./(a.*V) ; height = U.*( 1 - b2./(a.*V) ); lat = atan( (z + ep.*ep.*zo)./r ) ; temp = atan(y./x) ; if x >=0 long = temp ; else if (x < 0) & (y >= 0) long = pi + temp ; else long = temp - pi ; end end lat0 = lat./(pi/180) ; lon0 = long./(pi/180) ; h0 = height ;