model_v_a_q_nonlinear_m.m

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

function Y=Model_V_A_Q_nonlinear_m(X,noi,modelpara)

T_m = modelpara.T_m;
T_s = modelpara.T_s;
wnbb_m = modelpara.wnbb_m;
wien_m = modelpara.wien_m;
wenn_m = modelpara.wenn_m;
wnbb_s = modelpara.wnbb_s;
wien_s = modelpara.wien_s;
wenn_s = modelpara.wenn_s;
fb_m = modelpara.fb_m;
v_m = modelpara.v_m;
wimn_s = wien_s+wenn_s+T_s*wnbb_s;
wimn_m = wien_m+wenn_m+T_m*wnbb_m;
d_V = T_m*fb_m-X_Multiply(2*wien_m+wenn_m,v_m);
switch modelpara.error
    case 'Consider'
        Y=X(1:6,:);
        if ~isempty(noi)
            Y = Y + noi;
        end
   
    case 'LevelArm_Consider'
        for i=1:size(X,2)
            dV=X(1:2,i);
            rm=X(16:18,i);
            rn=T_m*rm;
            temp=X_Multiply(wimn_m,rn);
            %temp=X_Multiply(wimn_s,rn);
            Y(1:2,i)=dV+temp(1:2);
            Y(3:6,i)=X(3:6,i);
        end
        if ~isempty(noi)
            Y = Y + noi;
        end
    
end