ecef2enu.m

来自「利用gps多天线载波相位技术」· M 代码 · 共 47 行

%%========================================
%%     Toolbox for attitude determination
%%     Zhen Dai
%%     [e-mail]
%%     ZESS, University of Siegen, Germany
%%     Last Modified  : 1.Sep.2008
%%========================================
%% Functions:
%%      Convert the coordinate from ECEF to local level frame
%% Input parameters:
%%      vUsrXYZ -> The ECEF coordinate of the point to be converted
%%      vRefXYZ ->  The origion in ECEF
%% Output:
%%     vENU ->  Loca level coordinate of vUsrXYZ w.r.t vRefXYZ
%%                     in the format of [East North Up] 
%%  References:
%%       B.Hofmann-Wellenhof, H.Lichtenegger and J.Collins: GPS Theory
%%       and practice. 2001. Fifth revised edition. Springer, Wien, New York.
%%       pp.283-284 

function vENU = ECEF2ENU(vUsrXYZ,vRefXYZ)

%% Format the input
[a,b]=size( vUsrXYZ);
if (a==1) && (b==3),
    vUsrXYZ=vUsrXYZ';
end 
[a,b]=size( vRefXYZ);
if (a==1) && (b==3),
    vRefXYZ=vRefXYZ';
end

%% Firstly, convert ECEF XYZ to latitude, longitude and height
vLLH = ECEF2LLH(vRefXYZ);
lat = vLLH(1);
lon = vLLH(2);

%% Baseline differences
vBaseline = vUsrXYZ-vRefXYZ;

%% Rotation matrix
mR = [   -sin(lon)                        cos(lon)         0     ;  
              -sin(lat)*cos(lon)  -sin(lat)*sin(lon)  cos(lat);               
              cos(lat)*cos(lon)   cos(lat)*sin(lon)  sin(lat)]; 
          
vENU = mR*vBaseline;