strapdown.m

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

function [q,attitude,v,Tbn,wepp,wiep,phi,lamda,wpbb3,d_V]=strapdown(wib1,wib2,wib3,Tbn,wepp,wiep,q,step,fb3,v,phi,lamda)
    wpbb=[wib1-Tbn'*(wepp+wiep) wib2-Tbn'*(wepp+wiep) wib3-Tbn'*(wepp+wiep)];
    q=Rgkt4_q(q,wpbb,step);
    [attitude,v,Tbn,d_V]=attitude_v(q,fb3,wiep,wepp,v,step);
    [wepp,wiep,phi,lamda]=position(phi,lamda,v,step);
    wpbb3=wpbb(:,3);

function q=Rgkt4_q(q,wpbb,h)
    k1=Rgktf_q([0;wpbb(:,1)],q);                      
    k2=Rgktf_q([0;wpbb(:,2)],q+k1*h/2);
    k3=Rgktf_q([0;wpbb(:,2)],q+k2*h/2);
    k4=Rgktf_q([0;wpbb(:,3)],q+k3*h);
    q=q+(k1+2*k2+2*k3+k4)*h/6;
    q=q/sqrt(q(1)*q(1)+q(2)*q(2)+q(3)*q(3)+q(4)*q(4));
    
function dq=Rgktf_q(omega,q)
    dq= [omega(1)*q(1)-omega(2)*q(2)-omega(3)*q(3)-omega(4)*q(4)
        omega(2)*q(1)+omega(1)*q(2)+omega(4)*q(3)-omega(3)*q(4)
        omega(3)*q(1)-omega(4)*q(2)+omega(1)*q(3)+omega(2)*q(4)
        omega(4)*q(1)+omega(3)*q(2)-omega(2)*q(3)+omega(1)*q(4)];
    dq=dq/2;
    
function [attitude,v,Tbn,Kv]=attitude_v(q,fb,wiep,wepp,v,step)
    Tbn=qua_dcm(q);
    Tnb=Tbn';
    attitude = dcm_eulr(Tnb);
    fp = Tbn*fb;
    fv(1) = fp(1)+(2*wiep(3) + wepp(3))*v(2);
    fv(2) = fp(2)-(2*wiep(3) + wepp(3))*v(1);
    Kv1 = fv';
    tmp_v(1) = v(1) + fv(1)*step;
    tmp_v(2) = v(2) + fv(2)*step;
    fv(1) = fp(1)+(2*wiep(3) + wepp(3))*tmp_v(2);     
    fv(2) = fp(2)-(2*wiep(3) + wepp(3))*tmp_v(1);
    Kv2 = fv';
    Kv = (Kv1+Kv2)/2.0;
    v(1:2) = v(1:2) + Kv*step;
    
function [wepp,wiep,phi,lamda]=position(phi,lamda,v,step)
    global Re e wie;
    [wepp,wiep]=wenn_wien(phi,v);
    Rn = Re*(1+e*sin(phi)*sin(phi));
    Rm = Re*(1-2*e+3*e*sin(phi)*sin(phi));
    phi = phi + step*v(2)/Rm;
    lamda = lamda +step*v(1)/(Rn*cos(phi));    
    
function [wenn,wien]=wenn_wien(phi,v)
    global Re e wie;
    Rn = Re*(1+e*sin(phi)*sin(phi));
    Rm= Re*(1-2*e+3*e*sin(phi)*sin(phi));
    wenn = [-v(2)/Rm
            v(1)/Rn
            v(1)*tan(phi)/Rn];
    wien = [0.0
            wie*cos(phi)
            wie*sin(phi)];