dcmelgen.m

gpsoft 的惯性导航工具箱

function DCMel_prof = dcmelgen(lat_prof, lon_prof, tc_prof)
%DCMELGEN   Function to generate a direction cosine matrix
%           (earth-frame to local-level-frame) for a path
%           profile generated by GREATCIR.  Note: local-level
%           frame is north pointing.
%
%	DCMel_prof = dcmelgen(lat_prof, lon_prof, tc_prof)
%
%   INPUTS
%       lat_prof = vector of latitudes for each waypoint in the path (radians)
%       lon_prof = vector of longitudes for each waypoint in the path (radians)
%       tc_prof = profile of true course from each waypoint
%                 on the great circle route (radians relative
%                 to north; east directions are positive)
%
%   OUTPUTS
%       DCMel_prof(i,1:9) = elements of the direction cosine matrix
%                         (DCM) for vehicle attitude (earth to local-level); 
%                         1 = DCM(1,1), 2 = DCM(1,2), 3 = DCM(1,3),
%                         4 = DCM(2,1), et cetera
%

%	M. & S. Braasch 6-98
%	Copyright (c) 1997 by GPSoft LLC
%	All Rights Reserved.
%

if nargin<2,error('insufficient number of input arguments'),end

for i = 1:max(size(tc_prof)),
   
   alpha = 0;                        % North-pointing case
   DCMel = llw2dcm([lat_prof(i) lon_prof(i) alpha]);

   DCMel_prof(i,1) = DCMel(1,1);
   DCMel_prof(i,2) = DCMel(1,2);
   DCMel_prof(i,3) = DCMel(1,3);
   DCMel_prof(i,4) = DCMel(2,1);
   DCMel_prof(i,5) = DCMel(2,2);
   DCMel_prof(i,6) = DCMel(2,3);
   DCMel_prof(i,7) = DCMel(3,1);
   DCMel_prof(i,8) = DCMel(3,2);
   DCMel_prof(i,9) = DCMel(3,3);
   
end