xyz2enu.asv

惯性导航系统传递对准系统模型程序

function enu = xyz2enu(xyz,orgxyz)
%位置增量计算公式enu
if nargin<2,error('insufficient number of input arguments'),end
tmpxyz = xyz;
tmporg = orgxyz;
if size(tmpxyz) ~= size(tmporg), tmporg=tmporg'; end,
difxyz = tmpxyz - tmporg;
[m,n] = size(difxyz); if m<n, difxyz=difxyz'; end,
orgllh = xyz2llh(orgxyz);
phi = orgllh(1);%纬度值;
lam = orgllh(2);%经度值;
sinphi = sin(phi);
cosphi = cos(phi);
sinlam = sin(lam);
coslam = cos(lam);
R = [ -sinlam          coslam         0     ; ...
      -sinphi*coslam  -sinphi*sinlam  cosphi; ...
       cosphi*coslam   cosphi*sinlam  sinphi];%地球系和当地水平坐标系之间的坐标变换阵；
enu = R*difxyz;%求出位置增量;