tran_rbukfadd.m

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

function [FilterPM,align]=Tran_RBUKFADD(para)
    fra = 1;
    Algorithm_PM=para.Algorithm_PM;
    FilterPM=para.FilterPM;
    Q_s=para.q_s;
    Q_m=para.q_m;
    v_s=para.v_s;
    v_m=para.v_m;
    T_s=para.T_s;
    T_m=para.T_m;
    fb_s=para.fb_s;
    fb_m=para.fb_m;
    wnbb_s=para.wnbb_s;
    wnbb_m=para.wnbb_m;
    wien_m=para.wiep_m;
    wenn_m=para.wepp_m;
    wien_s=para.wiep_s;
    wenn_s=para.wepp_s;
    wibb_s=para.wibb_s;
    phi_m=para.phi_m;
    step=para.step;
    wimn_m = wien_m+wenn_m+T_m*wnbb_m;
    
    xEst=FilterPM.X;
    PEst=FilterPM.P;
    Q=FilterPM.Q;
    R=FilterPM.R;
    switch Algorithm_PM.app
        case 'Velocity_Attitude_Euler'
            ffun='Model_V_A_nonlinear01';
            hfun='Model_V_A_nonlinear_m';
            T_m=eulr_dcm(dcm_eulr(T_m)+para.MeasureAtte);
            ang_ms=dcm_eulr(T_s'*T_m);
            Q_m=dcm_qua(T_m);
            v_m(1:2,1)=v_m(1:2,1)+para.MeasureVe(1:2,1);
            modelpara.T_m=T_m;
            modelpara.fb_s=fb_s;
            modelpara.T_s=T_s;
            modelpara.wien_s=wien_s;
            modelpara.wenn_s=wenn_s;
            modelpara.wnbb_s=wnbb_s;
            modelpara.wien_m=wien_m;
            modelpara.wenn_m=wenn_m;
            modelpara.wnbb_m=wnbb_m;
            modelpara.wibb_s=wibb_s;
            modelpara.step=step;
            modelpara.fb_m=fb_m;
            modelpara.v_m=v_m;
            modelpara.error=Algorithm_PM.error;
            modelpara.H=eye(5);
            level=SymMatrix(wimn_m)*T_m;
            level=level(1:2,:);
            modelpara.level=[level;zeros(size(modelpara.H,1)-size(level,1),size(level,2))];
            z = [v_s(1:2,1) - v_m(1:2,1)
                        ang_ms       ];
            z = z + fra*sqrt(R)*randn(size(R,1),1);
        case 'Velocity_Attitude_Quaternion'
            switch Algorithm_PM.app_detail
                case 'algorithm1'
                    ffun='Model_V_A_Q_nonlinear01'; 
                    hfun='Model_V_A_Q_nonlinear_m';
                    T_m=eulr_dcm(dcm_eulr(T_m)+para.MeasureAtte);
                    Q_m=dcm_qua(T_m);
                    v_m(1:2,1)=v_m(1:2,1)+para.MeasureVe(1:2,1);
                    Q_s1=[Q_s(1);-Q_s(2:4)];
                    z = [v_s(1:2,1) - v_m(1:2,1)
                        Quat_Mult(Q_s1,Q_m)       ];
                    z = z + fra*sqrt(R)*randn(size(R,1),1);
                case 'algorithm2'
                    ffun='Model_V_A_Q_nonlinear02'; 
                    hfun='Model_V_A_Q_nonlinear_m';
                    T_m=eulr_dcm(dcm_eulr(T_m)+para.MeasureAtte);
                    Q_m=dcm_qua(T_m);
                    v_m(1:2,1)=v_m(1:2,1)+para.MeasureVe(1:2,1);
                    z = [v_s(1:2,1) - v_m(1:2,1)
                        Q_s-Q_m       ];
                    z = z + fra*sqrt(R)*randn(size(R,1),1);
            end
            modelpara.T_m=T_m;
            modelpara.fb_s=fb_s;
            modelpara.T_s=T_s;
            modelpara.Q_s=Q_s;
            modelpara.Q_m=Q_m;
            modelpara.wien_s=wien_s;
            modelpara.wenn_s=wenn_s;
            modelpara.wnbb_s=wnbb_s;
            modelpara.wien_m=wien_m;
            modelpara.wenn_m=wenn_m;
            modelpara.wnbb_m=wnbb_m;
            modelpara.step=step;
            modelpara.fb_m=fb_m;
            modelpara.v_m=v_m;
            modelpara.error=Algorithm_PM.error;
            modelpara.H=eye(6);
            level=SymMatrix(wimn_m)*T_m;
            level=level(1:2,:);
            modelpara.level=[level;zeros(size(modelpara.H,1)-size(level,1),size(level,2))];

    end
    alpha = 1;
    beta  = 2;
    kappa = 0;
    tic;
    [xEst,PEst]=RBUnscentedKFADD(xEst,PEst,z,Q,R,ffun,hfun,modelpara,alpha,beta,kappa);
    FilterPM.X=xEst;
    FilterPM.P=PEst;
    FilterPM.time=toc;
    
    switch Algorithm_PM.app
        case 'Velocity_Attitude_Euler'
            switch Algorithm_PM.error
                case 'Consider'
                    align=xEst(6:13);
                case 'LevelArm_Consider'
                    align=xEst(6:16);
            end
        case 'Velocity_Attitude_Quaternion'
            switch Algorithm_PM.app_detail
                case 'algorithm1'
                    align=dcm_eulr(qua_dcm(xEst(7:10)));
                case 'algorithm2'
                    Qsn=xEst(7:10)+Q_m;
                    Qsn=Qsn/sqrt(Qsn(1)*Qsn(1)+Qsn(2)*Qsn(2)+Qsn(3)*Qsn(3)+Qsn(4)*Qsn(4));
                    Qns=[Qsn(1);-Qsn(2:4)];
                    align=dcm_eulr(qua_dcm(Quat_Mult(Qns,Q_m)));
            end
            switch Algorithm_PM.error
                case 'Consider'
                    align=[align;xEst(11:15)];
                case 'LevelArm_Consider'
                    align=[align;xEst(11:18)];
            end
    end