prostrai.m

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

function profile = prostrai(initpos,initvel,initacc,initdcm,duration,deltat)

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

    t = deltat:deltat:duration; t=t';
    tmp = ones(max(size(t)),1);

    profile(:,7) = initacc(1)*tmp;
    profile(:,8) = initacc(2)*tmp;
    profile(:,9) = initacc(3)*tmp;

    profile(:,4) = initvel(1)*tmp + initacc(1)*t;
    profile(:,5) = initvel(2)*tmp + initacc(2)*t;
    profile(:,6) = initvel(3)*tmp + initacc(3)*t;

    profile(:,1) = initpos(1)*tmp + initvel(1)*t + 0.5*initacc(1)*t.*t;
    profile(:,2) = initpos(2)*tmp + initvel(2)*t + 0.5*initacc(2)*t.*t;
    profile(:,3) = initpos(3)*tmp + initvel(3)*t + 0.5*initacc(3)*t.*t;

    profile(:,10) = initdcm(1,1)*tmp;
    profile(:,11) = initdcm(1,2)*tmp;
    profile(:,12) = initdcm(1,3)*tmp;
    profile(:,13) = initdcm(2,1)*tmp;
    profile(:,14) = initdcm(2,2)*tmp;
    profile(:,15) = initdcm(2,3)*tmp;
    profile(:,16) = initdcm(3,1)*tmp;
    profile(:,17) = initdcm(3,2)*tmp;
    profile(:,18) = initdcm(3,3)*tmp;