transfer_alignment.m

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

function [data,FilterPM,record]=transfer_alignment(num,num_record,Algorithm_PM,data,Sensor,FilterPM,step,record,filter_start)

[q_s,attitude_s,v_s,T_s,wepp_s,wiep_s,phi_s,lamda_s,q_m,attitude_m,v_m,T_m,wepp_m,wiep_m,phi_m,lamda_m] = GetInfor(data);
t_n = floor(2*num);
M_wib1 = Sensor.M_wib(t_n-1,:)';
M_wib2 = Sensor.M_wib(t_n-0,:)';
M_wib3 = Sensor.M_wib(t_n+1,:)';
M_fb3  = Sensor.M_fib(t_n+1,:)';
M_Tbn  = [Sensor.M_Tbn(t_n+1,1:3);Sensor.M_Tbn(t_n+1,4:6);Sensor.M_Tbn(t_n+1,7:9)];
t_n = floor(2*num);
S_wib1 = Sensor.S_wib(t_n-1,:)';
S_wib2 = Sensor.S_wib(t_n-0,:)';
S_wib3 = Sensor.S_wib(t_n+1,:)';
S_fb3  = Sensor.S_fib(t_n+1,:)';
S_Tbn  = [Sensor.S_Tbn(t_n+1,1:3);Sensor.S_Tbn(t_n+1,4:6);Sensor.S_Tbn(t_n+1,7:9)];

[q_s,attitude_s,v_s,T_s,wepp_s,wiep_s,phi_s,lamda_s,wnbb_s,d_V_s]=strapdown(S_wib1,S_wib2,S_wib3,T_s,wepp_s,wiep_s,q_s,step.strapdown,S_fb3,v_s,phi_s,lamda_s);
[q_m,attitude_m,v_m,T_m,wepp_m,wiep_m,phi_m,lamda_m,wnbb_m,d_V_m]=strapdown(M_wib1,M_wib2,M_wib3,T_m,wepp_m,wiep_m,q_m,step.strapdown,M_fb3,v_m,phi_m,lamda_m);

data=FormInfor(q_s,attitude_s,v_s,T_s,wepp_s,wiep_s,phi_s,lamda_s,q_m,attitude_m,v_m,T_m,wepp_m,wiep_m,phi_m,lamda_m);

if filter_start==1
    FilterPM.t=num_record;
    para.Algorithm_PM=Algorithm_PM;
    para.FilterPM=FilterPM;
    para.v_m=v_m;
    para.v_s=v_s;
    para.q_m=q_m;
    para.q_s=q_s;
    para.T_m=T_m;
    para.T_s=T_s;
    para.fb_m=M_fb3;
    para.fb_s=S_fb3;
    para.wiep_m=wiep_m;
    para.wiep_s=wiep_s;
    para.wepp_m=wepp_m;
    para.wepp_s=wepp_s;
    para.wibb_s=S_wib3;
    para.wibb_m=M_wib3;
    para.wnbb_m=wnbb_m;
    para.wnbb_s=wnbb_s;
    para.phi_m=phi_m;
    para.step=step.fliter;  
    para.MeasureVe=Sensor.MeasureVe;
    para.MeasureAtte=Sensor.MeasureAtte;
    para.N=FilterPM.particle;
    disp([Algorithm_PM.filter ':' num2str(num_record)]);
    [FilterPM,align]=Tran_RBUKFADD(para);
    C_T=S_Tbn'*M_Tbn;
        
    record.time(num_record,1)=step.fliter*(num_record-1);
    
    record.align(num_record,1:3)=align(1:3)';
    record.align(num_record,4:6)=dcm_eulr(C_T)';
    switch Algorithm_PM.error
        case 'Consider'
            record.cal(num_record,1:5)=align(4:8)';
            record.cal(num_record,6:10)=[FilterPM.A_bias(1:2)' FilterPM.G_drift'];
        case 'LevelArm_Consider'
            record.levelarm(num_record,1:3)=align(9:11);
    end
    
    record.attitude(num_record,1:3)=attitude_s';
    record.attitude(num_record,4:6)=Sensor.S_attitude(t_n+1,:)';
    record.attitude(num_record,7:9)=attitude_m';
    record.attitude(num_record,10:12)=Sensor.M_attitude(t_n+1,:)';
    
    record.path(num_record,1:2)=[phi_s lamda_s];
    record.path(num_record,3:4)=[phi_m lamda_m];
    record.path(num_record,5:6)=Sensor.fly_path(t_n+1,1:2);
end