matlab 经纬度 东北天 直角坐标系转换代码

function enu = xyz2enu(xyz,orgxyz) %XYZ2ENU Convert from WGS-84 ECEF cartesian coordinates to % rectangular local-level-tangent ('East'-'North'-Up) % coordinates. % % enu = XYZ2ENU(xyz,orgxyz) % % INPUTS % xyz(1) = ECEF x-coordinate in meters % xyz(2) = ECEF y-coordinate in meters % xyz(3) = ECEF z-coordinate in meters % % orgxyz(1) = ECEF x-coordinate of local origin in meters % orgxyz(2) = ECEF y-coordinate of local origin in meters % orgxyz(3) = ECEF z-coordinate of local origin in meters % % OUTPUTS % enu: Column vector % enu(1,1) = 'East'-coordinate relative to local origin (meters) % enu(2,1) = 'North'-coordinate relative to local origin (meters) % enu(3,1) = Up-coordinate relative to local origin (meters) % Reference: Alfred Leick, GPS Satellite Surveying, 2nd ed., % Wiley-Interscience, John Wiley & Sons, % New York, 1995. % % M. & S. Braasch 10-96 % Copyright (c) 1996 by GPSoft % All Rights Reserved. if nargin<2,error('insufficient number of input arguments'),end tmpxyz = xyz; tmporg = orgxyz; % if size(tmpxyz) ~= size(tmporg), tmporg=tmporg'; end, % difxyz = zeros(size(tmpxyz),3); difxyz(:,1) = tmpxyz(:,1) - tmporg(1); difxyz(:,2) = tmpxyz(:,2) - tmporg(2); difxyz(:,3) = tmpxyz(:,3) - tmporg(3); % [m,n] = size(difxyz); if m<n, difxyz=difxyz'; end, orgllh = xyz2llh(orgxyz); phi = orgllh(1); lam = orgllh(2); sinphi = sind(phi); cosphi = cosd(phi); sinlam = sind(lam); coslam = cosd(lam); R = [ -sinlam coslam 0 ; ... -sinphi*coslam -sinphi*sinlam cosphi; ... cosphi*coslam cosphi*sinlam sinphi]; enu = (R*difxyz')';