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