progen.m

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

function profile = progen(initpos,initvel,initdcm,segparam)

if nargin<5,deltat=1;end
if nargin<4,error('insufficient number of input arguments'),end

[m,n]=size(initvel); if m>n, initvel=initvel'; end
[m,n]=size(initpos); if m>n, initpos=initpos'; end
[m,n]=size(segparam); nsegs = m;

profile(1,1:3) = initpos;
profile(1,4:6) = initvel;
profile(1,7:9) = [0 0 0];
profile(1,10:12) = initdcm(1,1:3);
profile(1,13:15) = initdcm(2,1:3);
profile(1,16:18) = initdcm(3,1:3);

for i = 1:nsegs,

   fprintf(1,' Creating profile for segment number %i \n',i)
   deltat = segparam(i,9);
   
   if i == 1,
      pos = initpos;
      vel = initvel;
      dcm = initdcm;
   else,
      k = size(profile,1);
      pos = profile(k,1:3);
      vel = profile(k,4:6);
      dcm(1,1:3) = profile(k,10:12);
      dcm(2,1:3) = profile(k,13:15);
      dcm(3,1:3) = profile(k,16:18);
   end,

   if segparam(i,1) == 1,
      acc = [0 0 0];
      duration = segparam(i,2);
      profilez = prostrai(pos,vel,acc,dcm,duration,deltat);
   elseif segparam(i,1) == 2,
      eul_vect = dcm2eulr(dcm);
      z = sin(eul_vect(2));
      x = sin(eul_vect(3));
      y = cos(eul_vect(3));
      acc = segparam(i,3)*9.81*[x y z];
      duration = segparam(i,2);      
      profilez = prostrai(pos,vel,acc,dcm,duration,deltat);
   elseif segparam(i,1) == 3,
      acc = [0 0 0];
      turnamt = segparam(i,4);
      [profilez,errflg] = ...
                proturn(pos,vel,acc,dcm,turnamt,deltat);
   elseif segparam(i,1) == 4,
      acc = [0 0 0];
      if segparam(i,5) == -999,
         pitchv = [segparam(i,7) segparam(i,8)];
         [profilez,errflg] = propitch(pos,vel,acc,dcm,pitchv,deltat);
      else,
         rollv = [segparam(i,5) segparam(i,6)];
         [profilez,errflg] = ...
            proroll(pos,vel,acc,dcm,rollv,deltat);
      end,
   end
   
   if i == 1,
      mm = size(profilez(:,18),1);
      profilez(:,19) = [0:deltat:deltat*(mm-1)]';
      profile = profilez;
   else,
      mm = size(profilez(:,18),1);
      nn = size(profile,1);
      oldtime = profile(nn,19);
      profilez(1:mm,19) = oldtime + deltat*(1:mm)';
      profile = [profile; profilez];
   end
   
end