model_v_a_nonlinear01.m

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

function X=Model_V_A_nonlinear01(X,noi,modelpara)
T_m = modelpara.T_m;%该函数是计算子惯导系统的速度＋全姿态非线性模型计算程序。输入模型参数、状态向量X和噪声noi，输出模型的Y阵，从而得到系统的非线性模型。在此模型中考虑两种情况：
%1、不考虑杆臂效应的情形；2、考虑杆臂效应的影响。考虑到噪声的影响，加入判断语句，如果有噪声，则把噪声加到输出Y中，如果没有，则不考虑噪声。
fb_s = modelpara.fb_s;
T_s = modelpara.T_s;
wnbb_s = modelpara.wnbb_s;
wnbb_m = modelpara.wnbb_m;
wien_s=modelpara.wien_s;
wenn_s=modelpara.wenn_s;
wien_m=modelpara.wien_m;
wenn_m=modelpara.wenn_m;
wimn_m = wien_m+wenn_m+T_m*wnbb_m;
wimn_s = wien_s+wenn_s+T_s*wnbb_s;
step = modelpara.step;
switch modelpara.error
    case 'Consider'
        for i=1:size(X,2)
            dV=X(1:2,i);
            Tms0=eulr_dcm(X(3:5,i));
            Tms=eulr_dcm(X(6:8,i));
            Qms0=dcm_qua(Tms0);
            ea=X(9:10,i);
            eg=X(11:13,i);
            e_aa=[ea;0];
            temp=T_m*Tms0'*(fb_s+e_aa)-T_m*Tms'*fb_s;
            dV=dV+step*temp(1:2,1);
            temp=[0; Tms'*(wnbb_s+eg)];
            dQms0=1/2*Quat_Mult(Qms0,temp);
            temp=[0; wnbb_s];
            dQms0=dQms0-1/2*Quat_Mult(temp,Qms0);
            Qms0=Qms0+step*dQms0;
            C_T=qua_dcm(Qms0);
            angle=dcm_eulr(C_T);
            X(:,i)=[dV;angle;X(6:8,i);ea;eg];
            if ~isempty(noi)
                noise=[noi(:,i);zeros(size(X,1)-size(noi(:,i),1),1)];
                X(:,i)=X(:,i)+noise;
            end
        end
    case 'LevelArm_Consider'
        for i=1:size(X,2)
            dV=X(1:2,i);
            dv=[dV;0];
            Tms0=eulr_dcm(X(3:5,i));
            Tms=eulr_dcm(X(6:8,i));
            Qms0=dcm_qua(Tms0);
            ea=X(9:10,i);
            eg=X(11:13,i);
            e_aa=[ea;0];
            rm=X(14:16,i);
            rn=T_m*rm;
            temp=T_m*Tms0'*(fb_s+e_aa)-T_m*Tms'*fb_s-X_Multiply(2*wien_m+wenn_m,dv)-X_Multiply(wien_m,X_Multiply(wimn_m,rn));
            %temp=T_s*(eye(3)-Tms0*Tms')*fb_s-X_Multiply(2*wien_m+wenn_m,dv)-X_Multiply(wien_m,X_Multiply(wimn_m,rn))+T_s*e_aa;
            dV=dV+step*temp(1:2,1);
            temp=[0; Tms'*(wnbb_s+eg)];
            dQms0=1/2*Quat_Mult(Qms0,temp);
            temp=[0; wnbb_s];
            dQms0=dQms0-1/2*Quat_Mult(temp,Qms0);
            Qms0=Qms0+step*dQms0;
            C_T=qua_dcm(Qms0);
            angle=dcm_eulr(C_T);
            X(:,i)=[dV;angle;X(6:16,i)];
            if ~isempty(noi)
                noise=[noi(:,i);zeros(size(X,1)-size(noi(:,i),1),1)];
                X(:,i)=X(:,i)+noise;
            end
        end
end