guandao1.m

惯性导航误差分析程序可以做误差曲线图像

vx(1)=0.000048637;
vy(1)=0.000206947;
vz(1)=0.007106781;          
A(1)=116.344695283*2*pi/360;
L(1)=39.975172*2*pi/360;                    
B(1)=-91.637207*2*pi/360;        
C(1)=0.120992605*2*pi/360;        
D(1)=0.010445947*2*pi/360;            
re=6378254;%地球长半轴
wie=7.292115147e-5;
e=1/298.3;
Ti=1/80;
wib_INSc=rand(3,47999);
f_INSc=randn(3,47999);
%g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;c33=sin(L);
%g=g0*(1+gk1*c33^2)*(1-2*h/re)/sqrt(1-gk2*c33^2)；
for i=1:47999
    Rx(i)=re*(1+e*sin(L(i))^2);%主曲率半径Rn
    Ry(i)=re*(1-2*e+3*e*sin(L(i))^2);%主曲率半径Rm
    Q0= cos(B(i)/2)*cos(C(i)/2)*cos(D(i)/2)+sin(B(i)/2)*sin(C(i)/2)*sin(D(i)/2);
    Q1= cos(B(i)/2)*sin(C(i)/2)*cos(D(i)/2)+sin(B(i)/2)*cos(C(i)/2)*sin(D(i)/2);
    Q2= cos(B(i)/2)*cos(C(i)/2)*sin(D(i)/2)-sin(B(i)/2)*sin(C(i)/2)*cos(D(i)/2);
    Q3= cos(B(i)/2)*sin(C(i)/2)*sin(D(i)/2)-sin(B(i)/2)*cos(C(i)/2)*cos(D(i)/2);
    Q00=[Q0;Q1;Q2;Q3];                                                          
    Cbn0=[Q0^2+Q1^2-Q2^2-Q3^2 2*(Q1*Q2+Q0*Q3) 2*(Q1*Q3-Q0*Q2);2*(Q1*Q2-Q0*Q3) Q0^2-Q1^2+Q2^2-Q3^2  2*(Q2*Q3+Q0*Q1); 2*(Q1*Q3+Q0*Q2) 2*(Q2*Q3-Q0*Q1) Q0^2-Q1^2-Q2^2+Q3^2];
    Wiet=[0;wie*cos(L(i));wie*sin(L(i))];%地球相对惯性系的自转角速率在地理坐标系上的投影 东北天方向
    Wett=[-vy(i)/Ry(i);vx(i)/Rx(i);vx(i)*tan(L(i))/Rx(i)];%载体相对地球的角速度在地理坐标系中的投影 东北天方向
    Wtbb=wib_INSc(:,i)-Cbn0*(Wiet+Wett);
    O=[0 -Wtbb(1) -Wtbb(2) -Wtbb(3); Wtbb(1) 0 Wtbb(3) -Wtbb(2);Wtbb(2) -Wtbb(3) 0 Wtbb(1);Wtbb(3) Wtbb(2) -Wtbb(1) 0]*Ti;%M‘（Wtbb)四元数微分方程
    O0=O(1,2)^2+O(1,3)^2+O(1,4)^2;
    Cn=1-O0/8;
    Sn=1/2-O0/48;
    Q=(Cn*eye(4)+Sn*O)*Q00;      
    Cbn=[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];
    if abs( Cbn(2,2))>1e-10
        if  Cbn(2,2)>0
           B(i+1)=atan( Cbn(2,1)/ Cbn(2,2));              
        elseif  Cbn(2,1)>0
           B(i+1)=atan( Cbn(2,1)/ Cbn(2,2))+pi;
        else 
            B(i+1)=atan(Cbn(2,1)/ Cbn(2,2))-pi;
        end
    elseif  Cbn(2,1)>0
       B(i+1)=pi/2;
    else 
       B(i+1)=-pi/2;
    end                    
   
    C(i+1)=asin( Cbn(2,3));  
    if abs( Cbn(3,3))>1e-10
        if  Cbn(3,3)>0
           D(i+1)=atan(-Cbn(1,3)/Cbn(3,3));
        elseif  Cbn(1,3)>0
            D(i+1)=atan(- Cbn(1,3)/ Cbn(3,3))-pi;
        else
           D(i+1)=atan(- Cbn(1,3)/ Cbn(3,3))+pi;
        end
   elseif  Cbn(1,3)>0
       D(i+1)=-pi/2;
   else 
       D(i+1)=pi/2;
   end                
                                                   
    f= Cbn0'*f_INSc(:,i);
    vx(i+1)=(f(1,1)+2*wie*sin(L(i))*vy(i)+vx(i)*vy(i)*tan(L(i))/Rx(i))*Ti+vx(i);
    vy(i+1)=(f(2,1)-2*wie*sin(L(i))*vx(i)-vx(i)*vx(i)*tan(L(i))/Rx(i))*Ti+vy(i);
    L(i+1)=vy(i)*Ti/Ry(i)+L(i);                   
    A(i+1)=vx(i)*Ti/(Rx(i)*cos(L(i)))+A(i);        
end
A=A*360/(2*pi);L=L*360/(2*pi);B=B*360/(2*pi);C=C*360/(2*pi);D=D*360/(2*pi);         
 figure
plot(A,L),grid;
figure
   t=0:1/80:(600-1/80);
   plot(t,vx),grid;
  figure
   plot(t,B),grid;
   figure
   plot(t,C),grid;
figure
   plot(t,D),grid;