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('横滚角');