matlab编写的捷联导航程序.m

基于matlab平台编写的捷联导航程序

Re=6378245;%初值
e=1/298.25;
wie=7.292115147e-5;
T=1/80;
lambda(1)=116.344695283*pi/180;
L(1)=39.975172*pi/180;
theta(1)= 0.120992605*pi/180;
gamma(1)= 0.010445947*pi/180;
psi(1)= 91.63720700*pi/180;
V= [0.000048637;0.000206947];
Vt(1)=sqrt(V(1,1)^2+V(2,1)^2);
q0 = cos(2*pi-psi(1)/2)*cos(theta(1)/2)*cos(gamma(1)/2)+sin(2*pi-psi(1)/2)*sin(theta(1)/2)*sin(gamma(1)/2);%四元数的四个元素
q1 = cos(2*pi-psi(1)/2)*sin(theta(1)/2)*cos(gamma(1)/2)+sin(2*pi-psi(1)/2)*cos(theta(1)/2)*sin(gamma(1)/2);
q2 = cos(2*pi-psi(1)/2)*cos(theta(1)/2)*sin(gamma(1)/2)-sin(2*pi-psi(1)/2)*sin(theta(1)/2)*cos(gamma(1)/2);
q3 = cos(2*pi-psi(1)/2)*sin(theta(1)/2)*sin(gamma(1)/2)-sin(2*pi-psi(1)/2)*cos(theta(1)/2)*cos(gamma(1)/2);
Q = [q0;q1;q2;q3];
Cnb= [Q(1)^2+Q(2)^2-Q(3)^2-Q(4)^2  2*(Q(2)*Q(3)+Q(1)*Q(4))         2*(Q(2)*Q(4)-Q(1)*Q(3));
      2*(Q(2)*Q(3)-Q(1)*Q(4))      Q(1)^2-Q(2)^2+Q(3)^2-Q(4)^2     2*(Q(3)*Q(4)+Q(1)*Q(2));
      2*(Q(2)*Q(4)+Q(1)*Q(3))      2*(Q(3)*Q(4)-Q(1)*Q(2))         Q(1)^2-Q(2)^2-Q(3)^2+Q(4)^2];
Cbn=Cnb'; 
load G:\xx\惯导\homeword2-06\jlfw.mat
for i=1:47999
    Rxt(i)=Re/(1-e*sin(L(i))^2);
    Ryt(i)=Re/(1+2*e-3*e*sin(L(i))^2);
    Wibb=wib_INSc(1:3,i);
    Fibb=f_INSc(1:3,i);
    Fibn=Cbn*Fibb;
    lambda(i+1)=(V(1,i)/(Rxt(i)*cos(L(i))))*T+lambda(i);
    L(i+1)=(V(2,i)/Ryt(i))*T+L(i);
    Axt=Fibn(1)+(2*wie*sin(L(i))+V(1,i)/Rxt(i)*tan(L(i)))*V(2,i);%求速度
    Ayt=Fibn(2)-(2*wie*sin(L(i))+V(1,i)/Rxt(i)*tan(L(i)))*V(1,i);
    V(1,i+1)=Axt*T+V(1,i);
    V(2,i+1)=Ayt*T+V(2,i);
    Vt(i+1)=sqrt(V(1,i+1)^2+V(2,i+1)^2);
    Wenn=[-V(2,i)/Ryt(i);V(1,i)/Rxt(i);V(1,i)/Rxt(i)*tan(L(i))];
    Wien=[0;wie*cos(L(i));wie*sin(L(i))];
    Winn=Wien+Wenn;
    Winb=Cnb*Winn;
    Wnbb=Wibb-Winb;  
    s_1 = Wnbb*T;
    s = sqrt(s_1(1)^2+s_1(2)^2+s_1(3)^2);%角增量
    Q=((1-s^2/8-s^4/384)*eye(4)+(1/2-s^2/48)*[0 -s_1(1) -s_1(2) -s_1(3);
                                              s_1(1) 0 s_1(3) -s_1(2);
                                              s_1(2) -s_1(3) 0 s_1(1);
                                              s_1(3) s_1(2) -s_1(1) 0])*Q;
    Cnb=[Q(1)^2+Q(2)^2-Q(3)^2-Q(4)^2     2*(Q(2)*Q(3)+Q(1)*Q(4))         2*(Q(2)*Q(4)-Q(1)*Q(3));
         2*(Q(2)*Q(3)-Q(1)*Q(4))         Q(1)^2-Q(2)^2+Q(3)^2-Q(4)^2     2*(Q(3)*Q(4)+Q(1)*Q(2));
         2*(Q(2)*Q(4)+Q(1)*Q(3))         2*(Q(3)*Q(4)-Q(1)*Q(2))         Q(1)^2-Q(2)^2-Q(3)^2+Q(4)^2];
    Cbn=Cnb.'; 
    theta(i+1)=asin(Cbn(3,2));%俯仰
    gamma_1=atan(-Cbn(3,1)/Cbn(3,3));%横滚
    if gamma_1>0           
        if(Cbn(3,3)>0 )
           gamma(i+1) = gamma_1;
        elseif(Cbn(3,3)<0)
            gamma(i+1) = gamma_1-pi;
        end;
    elseif gamma_1<0
        if(Cbn(3,3)>0)
            gamma(i+1) = gamma_1;
        elseif(C_b_n(3,3)<0)
            gamma(i) = gamma_1+pi;
        end;
    elseif gamma_1==0;
        if(Cbn(3,3)>0)
            gamma(i+1)=0;
        elseif(Cbn(3,3)<0)
            gamma(i+1)=pi;
        end;
    end;   
   psi_1 = atan(Cnb(1,2)/Cnb(2,2));
    if psi_1>0                
        if(Cbn(2,2)>0) %判断游动航向角所在的象限
           psi(i+1) = psi_1;
        elseif(Cbn(2,2)<0)
            psi(i+1) =psi_1+pi;
        end
    elseif psi_1<0
        if(Cbn(1,2)>0)
            psi(i+1) = psi_1+2*pi;
        elseif(Cbn(1,2)<0)
            psi(i+1) = psi_1+pi;
        end;
   elseif psi_1==0
        if(Cbn(2,2)>0)
           psi(i+1) = 0;
        elseif(Cbn(2,2)<0)
            psi(i+1) = pi;
        end;
    end;
end;
figure;
plot(lambda*180/pi,L*180/pi);xlabel('经度(\circ)');ylabel('纬度(\circ)');grid on;
j=1/80:1/80:600;
figure;
plot(j,lambda*180/pi);xlabel('时间(s)');ylabel('经度(\circ)');grid on;
figure;
plot(j,L*180/pi);xlabel('时间(s)');ylabel('纬度(\circ)');grid on;
figure;
plot(j,theta*180/pi);xlabel('时间(s)');ylabel('俯仰角(\circ)');grid on;
figure;
plot(j,gamma*180/pi);xlabel('时间(s)');ylabel('横滚角(\circ)');grid on;
figure;
plot(j,-psi*180/pi);xlabel('时间(s)');ylabel('航向角(\circ)');grid on;
figure;
plot(j,V(1,:));xlabel('时间(s)');ylabel('东向速度(m/s)');grid on;
figure;
plot(j,V(2,:));xlabel('时间(s)');ylabel('北向速度(m/s)');grid on