model_v_a_q_nonlinear02.m

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

function X=Model_V_A_Q_nonlinear02(X,noi,modelpara)
%additive quaternion proposed by this paper
T_m = modelpara.T_m;
fb_s = modelpara.fb_s;
T_s = modelpara.T_s;
Q_s = modelpara.Q_s;
Q_m = modelpara.Q_m;
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];
            dQms0=X(3:6,i);
            dQms=X(7:10,i);
            temp1=[1;0;0;0];
            Qns0=[Q_s(1); -Q_s(2:4)];
            temp2=Quat_Mult(Qns0,dQms0);
            Qms0=temp1-temp2;
            Qms0=Qms0/sqrt(Qms0(1)*Qms0(1)+Qms0(2)*Qms0(2)+Qms0(3)*Qms0(3)+Qms0(4)*Qms0(4));
            Qnm=[Q_m(1); -Q_m(2:4)];
            temp2=Quat_Mult(Qnm,dQms);
            Qsm=temp1+temp2;
            Qsm=Qsm/sqrt(Qsm(1)*Qsm(1)+Qsm(2)*Qsm(2)+Qsm(3)*Qsm(3)+Qsm(4)*Qsm(4));
            Tms0=qua_dcm(Qms0);
            Tsm=qua_dcm(Qsm);Tms=Tsm';
            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];
            ddQms0=1/2*Quat_Mult(Q_s,temp);
            temp=[0; wnbb_m];
            ddQms0=ddQms0-1/2*Quat_Mult(Q_m,temp);
            dQms0=dQms0+step*ddQms0;
            temp=[0; T_m*wnbb_m];
            ddQms=1/2*Quat_Mult(temp,dQms);
            dQms=dQms+step*ddQms;
            X(:,i)=[dV;dQms0;dQms;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];
            dQms0=X(3:6,i);
            dQms=X(7:10,i);
            temp1=[1;0;0;0];
            Qns0=[Q_s(1); -Q_s(2:4)];
            temp2=Quat_Mult(Qns0,dQms0);
            Qms0=temp1-temp2;
            Qms0=Qms0/sqrt(Qms0(1)*Qms0(1)+Qms0(2)*Qms0(2)+Qms0(3)*Qms0(3)+Qms0(4)*Qms0(4));
            Qnm=[Q_m(1); -Q_m(2:4)];
            temp2=Quat_Mult(Qnm,dQms);
            Qsm=temp1+temp2;
            Qsm=Qsm/sqrt(Qsm(1)*Qsm(1)+Qsm(2)*Qsm(2)+Qsm(3)*Qsm(3)+Qsm(4)*Qsm(4));
            Tms0=qua_dcm(Qms0);
            Tsm=qua_dcm(Qsm);Tms=Tsm';
            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];
            ddQms0=1/2*Quat_Mult(Q_s,temp);
            temp=[0; wnbb_m];
            ddQms0=ddQms0-1/2*Quat_Mult(Q_m,temp);
            dQms0=dQms0+step*ddQms0;
            temp=[0; T_m*wnbb_m];
            ddQms=1/2*Quat_Mult(temp,dQms);
            dQms=dQms+step*ddQms;
            X(:,i)=[dV;dQms0;dQms;ea;eg;rm];
            if ~isempty(noi)
                noise=[noi(:,i);zeros(size(X,1)-size(noi(:,i),1),1)];
                X(:,i)=X(:,i)+noise;
            end
        end
end