llw2dcm.m

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

function DCMel = llw2dcm(llw_vect)
%地球系和当地水平坐标系之间的转换矩阵；输入纬度经度和航向角；输出
  if nargin<1,error('insufficient number of input arguments'),end

  lat = -llw_vect(1); long = llw_vect(2); alpha = llw_vect(3);
  %% note the minus sign on the latitude takes care of the negative rotation

  clat = cos(lat); slat = sin(lat);
  clong = cos(long); slong = sin(long);
  calpha = cos(alpha); salpha = sin(alpha);

  C1 = [clong  slong 0; ...
        -slong clong 0; ...
         0     0   1];            % Rotation of LONG radians about z
  C2 = [clat  0  -slat; ...
          0   1     0 ; ...
        slat  0   clat];          % Rotation of LAT radians about y
  C3 = [1   0    0;   ...
        0  calpha salpha; ...
        0 -salpha calpha];        % Rotation of ALPHA radians about x

  C = C3 * C2 * C1;  % We've converted from lat-long-wander to Up-East-North
  
  Cconv = ...
     [0 1 0;
     0 0 1;
     1 0 0];   % Use this to convert from Up-East-North to East-North-Up
  
  DCMel = Cconv*C;