model_v_a_q_nonlinear01.m

来自「惯性导航系统传递对准系统模型程序」· M 代码 · 共 64 行

function X=Model_V_A_Q_nonlinear01(X,noi,modelpara)
T_m = modelpara.T_m;
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;
wemn_m=wenn_m+T_m*wnbb_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);
            dv=[dV;0];
            Qms0=X(3:6,i);
            Tms0=qua_dcm(Qms0);
            Tms=qua_dcm(X(7:10,i));
            ea=X(11:12,i);
            eg=X(13:15,i);
            e_aa=[ea;0];
            temp=T_s*(fb_s-Tms0*Tms'*fb_s)-X_Multiply(2*wien_m+wenn_m,dv)+T_s*e_aa;
            dV=dV+step*temp(1:2,1);
            temp=[0; Tms*wnbb_m+eg];
            dQms0=1/2*Quat_Mult(temp,Qms0);
            temp=[0; wnbb_m];
            dQms0=1/2*Quat_Mult(Qms0,temp)-dQms0;
            Qms0=Qms0+step*dQms0;
            X(:,i)=[dV;Qms0;X(7:10,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];
            Qms0=X(3:6,i);
            Tms0=qua_dcm(Qms0);
            Tms=qua_dcm(X(7:10,i));
            ea=X(11:12,i);
            eg=X(13:15,i);
            e_aa=[ea;0];
            rm=X(16:18,i);
            rn=T_m*rm;
            temp=T_s*(fb_s-Tms0*Tms'*fb_s)-X_Multiply(2*wien_m+wenn_m,dv)-X_Multiply(2*wien_m,X_Multiply(wemn_m,rn))+T_s*e_aa;
            dV=dV+step*temp(1:2,1);
            temp=[0; Tms*wnbb_m+eg];
            dQms0=1/2*Quat_Mult(temp,Qms0);
            temp=[0; wnbb_m];
            dQms0=1/2*Quat_Mult(Qms0,temp)-dQms0;
            Qms0=Qms0+step*dQms0;
            X(:,i)=[dV;Qms0;X(7:18,i)];
            if ~isempty(noi)
                noise=[noi(:,i);zeros(size(X,1)-size(noi(:,i),1),1)];
                X(:,i)=X(:,i)+noise;
            end
        end
end