jljs.m

捷联惯导方程矩阵在matlab环境下的仿真源代码程序

load jlfw                                   %数据放在工作目录下

%%%%%%%%%%%%%%%%%%%%%%%% 初始化 initial parameters %%%%%%%%%%%%%%%%%%%%%%%%%

%已知参数
e=1/298.3;
T=1/80;                                     %采样周期
k=600*80;                                   %迭代次数
h=30;                                       %初始高度
Latitude=zeros(1,k-1);
Longitude=zeros(1,k-1);
V=zeros(3,k-1); 
Longitude(1)=116.344695283*pi/180;          %初始经度
Latitude(1)=39.975172*pi/180;               %初始纬度
V(:,1)=[0.000048637;0.000206947;0.007106781];    %初始速度
zitai=[0.120992605 0.010445947 -91.637207]*pi/180;%初始姿态角
Wiee=[0;0;7.292115147e-5];                  %地球自转角速度
Re=6378245;                                 %地球椭球长半径
g0=9.7803267714;
gk1=0.00193185138639;
gk2=0.00669437999013;

%初始化姿态矩阵
Cnb=[cos(zitai(2))*cos(zitai(3))+sin(zitai(1))*sin(zitai(2))*sin(zitai(3)) , -cos(zitai(2))*sin(zitai(3))+sin(zitai(1))*sin(zitai(2))*cos(zitai(3))  , -sin(zitai(2))*cos(zitai(1));
       cos(zitai(1))*sin(zitai(3)) , cos(zitai(1))*cos(zitai(3)) , sin(zitai(1));
     sin(zitai(2))*cos(zitai(3))-cos(zitai(2))*sin(zitai(1))*sin(zitai(3)) , -sin(zitai(2))*sin(zitai(3))-cos(zitai(2))*sin(zitai(1))*cos(zitai(3)) , cos(zitai(1))*cos(zitai(2))];
Cbn=Cnb';

%初始化Cen
Cen=[      -sin(Longitude(1))                 cos(Longitude(1))                   0;
    -sin(Latitude(1))*cos(Longitude(1)) -sin(Latitude(1))*sin(Longitude(1)) cos(Latitude(1));
    cos(Latitude(1))*cos(Longitude(1))  cos(Latitude(1))*sin(Longitude(1))  sin(Latitude(1))];


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 开始解算 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i=1:k
    
    Rxn=Re*(1+e*Cen(3,3)^2)+h;    
    Ryn=Re*(1-2*e+3*e*Cen(3,3)^2)+h;
    g=[0;0;-g0*(1+gk1*Cen(3,3)^2)*(1-2*h/Re)/sqrt(1-gk2*Cen(3,3)^2)];

    aibn=Cbn*f_INSc(:,i);

    Wien=Cen*Wiee;
    Wenn=[-V(2,i)/Ryn;V(1,i)/Rxn;V(1,i)*tan(Latitude(i))/Rxn];
    Winb=Cnb*(Wien+Wenn);    
    Wnbb=wib_INSc(:,i)-Winb;

   dvx=aibn(1)+2*Wien(3)*V(2,i)+V(1,i)*V(2,i)*tan(Latitude(i))/Rxn;
   V(1,i+1)=V(1,i)+dvx*T;
   dvy=aibn(2)-2*Wien(3)*V(1,i)-V(1,i)*tan(Latitude(i))/Rxn;
   V(2,i+1)=V(2,i)+dvy*T;
    
     %更新Cnb
    Omega1=[   0    -Wnbb(3)  Wnbb(2);
           Wnbb(3)    0      -Wnbb(1);
          -Wnbb(2)   Wnbb(1)    0];
    dCnb= -Omega1*Cnb;
    Cnb=Cnb+dCnb*T;

    Cbn=Cnb';
   

    %更新Cen
    Omega0=[   0    -Wenn(3)  Wenn(2);
           Wenn(3)    0      -Wenn(1);
          -Wenn(2)   Wenn(1)    0];
    dCen= -Omega0*Cen;
    Cen=Cen+dCen*T;
    
    Latitude(i+1)=asin(Cen(3,3));
    Longitude(i+1)=atan(Cen(3,2)/Cen(3,1));
    
    if abs(Cen(3,1))>eps
        
    if Cen(3,1)<0
        if Cen(3,2)>0
            Longitude(i+1)=Longitude(i+1)+pi;
        else
            Longitude(i+1)=Longitude(i+1)-pi;
        end
    else 
        Longitude(i+1)=Longitude(i+1);
    end
    
    elseif Cen(3,2)>0
        Longitude(i+1)=pi/2;
   else
      Longitude(i+1)=-1/2*pi;
    end
end

Latitude=Latitude*180/pi;
Longitude=Longitude*180/pi;

plot(Longitude,Latitude);figure
plot(Latitude);figure
plot(Longitude);