rawimu01.m

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

function record=RawIMU01(profile,IMUError,Cms,wimm,Level_m)

step=initial(profile);
for i = 1:size(profile,1)
    [wib,fb,phi,lamda,altitude,v,a,attitude,Tbn]=data(profile(i,:),IMUError,step,Cms,wimm(i,:)',Level_m);
    record(i,:)=[profile(i,19) wib' fb' attitude' v' a' phi lamda Tbn(1,:) Tbn(2,:) Tbn(3,:) altitude];
end
save record_IMU.mat record;

function step=initial(profile)
    [m,n]=size(profile);
    if (m<2)||(n<19)
        error('The dimension of the input vector is error.');
    end
    step=profile(2,19)-profile(1,19);
    
function [wib,fb,phi,lamda,altitude,v,a,attitude,Tbn]=data(FlightData,IMUError,step,Cms,wimm,Level_m)
    [phi,lamda,altitude,v,a,attitude,Tbn,wnbb,wemm]=DataOut(FlightData,step,Cms,wimm);
    [wenn,wien]=wenn_wien(phi,v);
    [wib,fb]=gyro_acc(phi,wenn,wien,wnbb,a,v,Tbn,IMUError,Cms,wemm,Level_m);

function [phi,lamda,altitude,v,a,attitude,Tbn,wnbb,wemm]=DataOut(FlightData,step,Cms,wimm)
    phi=FlightData(1,1);
    lamda=FlightData(1,2);
    altitude=FlightData(1,3);
    v=FlightData(1,4:6)';
    a=FlightData(1,7:9)';
    Tnm = [FlightData(1,10:12);FlightData(1,13:15);FlightData(1,16:18)];
    [wenn,wien]=wenn_wien(phi,v);
    wemm = wimm - Tnm*wien;
    Tnb=Cms*Tnm;
    Tbn = Tnb';
    attitude = dcm_eulr(Tnb);
    wnbb(1,1)=FlightData(1,20);
    wnbb(2,1)=FlightData(1,21);
    wnbb(3,1)=FlightData(1,22);
    wnbb=Cms*wnbb;
        
function [wib,fb]=gyro_acc(phi,wenn,wien,wnbb,a,v,Tbn,IMUError,Cms,wemm,Level_m)
    G_Drift=IMUError(1:3,1); 
    A_bias=IMUError(4:6,1); 
    g = 9.7803+0.051799*sin(phi)*sin(phi);
    wib = wnbb+Tbn'*(wenn+wien);
    fn  = [a(1)-(2*wien(3,1)+wenn(3,1))*v(2)+(2*wien(2,1)+wenn(2,1))*v(3)
           a(2)+(2*wien(3,1)+wenn(3,1))*v(1)-(2*wien(1,1)+wenn(1,1))*v(3)
           a(3)-(2*wien(2,1)+wenn(2,1))*v(1)+(2*wien(1,1)+wenn(1,1))*v(2)+g];
    fb = Tbn'*fn; 
    fb = fb + Cms*(X_Multiply(wemm,X_Multiply(wemm,Level_m)));
    noise=[randn(1);randn(1);randn(1);randn(1);randn(1);randn(1)];
    wib=wib-G_Drift.*(1+0.1*noise(1:3,1));
    fb=fb-A_bias.*(1+0.1*noise(4:6,1));