Matlab 飞机航向INS仿真

close all;
clear all;
clc;
format long
%初始化参数
%vx(1)=0.000048637;
%vy(1)=0.000206947;
%vz(1)=0.007106781;          %初始速度
vx(1)=0;
vy(1)=0;
vz(1)=0;
jd(1)=116.344695283*2*pi/360;
L(1)=39.975172*2*pi/360;                     %初始经纬度
%B(1)=-91.637207*2*pi/360;         %初始航向角   tian
%C(1)=0.120992605*2*pi/360;         %初始俯仰角   dong
%D(1)=0.010445947*2*pi/360;         %初始横滚角   bei %初始姿态角
B(1)=0;
C(1)=0;
D(1)=0;
re=6378245;
Wie=7.27221e-5;
e=1/298.3;
Ti=1/20;
j=100;
N=200;
g=9.78049;

    Wibxb_INSc=randn(j,1);
    Wibyb_INSc=randn(j,1);
    Wibzb_INSc=randn(j,1);%陀螺在各个方向上测量的数据
    Fbx_INSc=randn(j,1);
    Fby_INSc=randn(j,1);
    Fbz_INSc=randn(j,1);%加速度计在各个方向上测量的数据
for i=1:j
    Rx=re/(1-e*sin(L(1))^2);
    Ry=re/(1+2*e-3*e*sin(L(1))^2);
  
    T31=Fbx_INSc(i,1)/g*Ti;
    T32=Fby_INSc(i,1)/g*Ti;
    T33=Fbz_INSc(i,1)/g*Ti;
    T21=(Wibxb_INSc(i,1)-Ti*T31*Wie*sin(L(1)))/Ti*Wie*cos(L(1));
    T22=(Wibyb_INSc(i,1)-Ti*T32*Wie*sin(L(1)))/Ti*Wie*cos(L(1));
    T23=(Wibzb_INSc(i,1)-Ti*T33*Wie*sin(L(1)))/Ti*Wie*cos(L(1));
    T11=T22*T33-T23*T32;
    T12=T23*T31-T21*T33;
    T13=T21*T32-T22*T31;   
    Cbn=[T11 T12 T13;T21 T22 T23;T31 T32 T33];%粗对准后确定的姿态矩阵
    Cnb=Cbn';
    
    gnx=0;
    gny=0;
    gnz=g;
    gn=[gnx;gny;gnz];%3*1
    gb=gn'*Cbn;   %重力加速度在机体系的表示 1*3*3*3=1*3
    Wien_x=0;
    Wien_y=Wie*cos(L(1));
    Wien_z=Wie*sin(L(1));
    Wien=[Wien_x;Wien_y;Wien_z];%3*1
    Wieb=Wien'*Cbn; %地球自转角速度在机体系的表示
    Wibb=[Wibxb_INSc(i,1) Wibyb_INSc(i,1) Wibzb_INSc(i,1)]';%陀螺输出的各个轴表示
    Fb=[Fbx_INSc(i,1) Fby_INSc(i,1) Fbz_INSc(i,1)]';  %加速度计输出的各个轴表示
    %Wibb'=Wien'*Cbn;
    %[Fbx_INSc(i,1) Fby_INSc(i,1) Fbz_INSc(i,1)]=-1*gn'*Cbn;
  %姿态角的计算
    if abs(Cnb(2,2))>1e-10
        if Cnb(2,2)>0
           B(i+1)=atan(Cnb(2,1)/Cnb(2,2));              
        elseif Cnb(2,1)>0
           B(i+1)=atan(Cnb(2,1)/Cnb(2,2))+pi;
        else 
            B(i+1)=atan(Cnb(2,1)/Cnb(2,2))-pi;
        end
    elseif Cnb(2,1)>0
       B(i+1)=pi/2;
    else 
       B(i+1)=-pi/2;
    end                     %求航向角
   
    C(i+1)=asin(Cnb(2,3));   %求俯仰角
    if abs(Cnb(3,3))>1e-10
        if Cnb(3,3)>0
           D(i+1)=atan(-Cnb(1,3)/Cnb(3,3));
        elseif Cnb(1,3)>0
            D(i+1)=atan(-Cnb(1,3)/Cnb(3,3))-pi;
        else
           D(i+1)=atan(-Cnb(1,3)/Cnb(3,3))+pi;
        end
   elseif Cnb(1,3)>0
       D(i+1)=-pi/2;
   else 
       D(i+1)=pi/2;
   end                   %求横滚角
   
    vx(i+1)=(Fbx_INSc(i,1)+2*Wie*sin(L(1))*vy(i)+vx(i)*vy(i)*tan(L(1))/Rx-2*Wie*cos(L(1))*vz(i)-vx(i)*vz(i)/Rx)*Ti+vx(i);%东向速度
    vy(i+1)=(Fby_INSc(i,1)-2*Wie*sin(L(1))*vx(i)-vx(i)*vx(i)*tan(L(1))/Rx-vy(i)*vz(i)/Rx)*Ti+vy(i);%北向速度
    vz(i+1)=(Fbz_INSc(i,1)+(2*Wie*cos(L(1))+vx(i)/Rx)*vx(i)+vy(i)*vy(i)/Ry-g)*Ti+vz(i);%天向速度
    %L(i+1)=vy(i)*Ti/Ry+L(i);                    %纬度
    %jd(i+1)=vx(i)*Ti/(Rx*cos(L(i)))+jd(i);        %经度
     
    

end


 t=0:0.1:j;
  %figure(1)
  %plot(jd,'r');xlabel('时间'),ylabel('经度');
  %figure(2)
  %plot(L,'g');xlabel('时间'),ylabel('纬度');
  figure(3)
  plot(vx,'b');xlabel('时间'),ylabel('东向速度');
  figure(4)
  plot(vy,'b');xlabel('时间'),ylabel('北向速度');
  figure(5)
  plot(vz,'b');xlabel('t'),ylabel('天向速度');
  figure(6)
  plot(B,'b');xlabel('时间'),ylabel('航向角');
  figure(7)
  plot(C,'g');xlabel('时间'),ylabel('俯仰角');
  figure(8)
  plot(D,'r');xlabel('时间'),ylabel('横滚角');