gendthet.m

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

function deltheta = gendthet(DCMnb_prof)

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

for k = 2:size(DCMnb_prof,1),
  dcmnb2=[DCMnb_prof(k,1:3); DCMnb_prof(k,4:6); DCMnb_prof(k,7:9)];
  dcmnb1=[DCMnb_prof(k-1,1:3); DCMnb_prof(k-1,4:6); DCMnb_prof(k-1,7:9)];
  del_PSI = dcmnb1 * dcmnb2' - eye(3);
  deltapsi(k-1,1) = del_PSI(2,1);
  deltathe(k-1,1) = del_PSI(1,3);
  deltaphi(k-1,1) = del_PSI(3,2);
end
deltheta = [deltaphi deltathe deltapsi];