model_v_a_nonlinear_m.m

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

function Y=Model_V_A_nonlinear_m(X,noi,modelpara)

T_m = modelpara.T_m;%该函数是计算主惯导系统的速度＋全姿态非线性模型计算程序。输入模型参数、状态向量X和噪声noi，输出模型的Y阵，从而得到系统的非线性模型。在此模型中考虑两种情况：
%1、不考虑杆臂效应的情形；2、考虑杆臂效应的影响。考虑到噪声的影响，加入判断语句，如果有噪声，则把噪声加到输出Y中，如果没有，则不考虑噪声。
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:5,:);
        if ~isempty(noi)
            Y = Y + noi;
        end
    case 'LevelArm_Consider'
        for i=1:size(X,2)
            dV=X(1:2,i);
            phim=X(3:5,i);
            rm=X(14:16,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:5,i)=phim;
        end
        if ~isempty(noi)
            Y = Y + noi;
        end
end