close?all;
clear?all;
clc;
format?long
%初始化參數(shù)
%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;?????????????????????%初始經(jīng)緯度
%B（1）=-91.637207*2*pi/360;?????????%初始航向角???tian
%C（1）=0.120992605*2*pi/360;?????????%初始俯仰角???dong
%D（1）=0.010445947*2*pi/360;?????????%初始橫滾角???bei?%初始姿態(tài)角
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（j1）;
????Wibyb_INSc=randn（j1）;
????Wibzb_INSc=randn（j1）;%陀螺在各個方向上測量的數(shù)據(jù)
????Fbx_INSc=randn（j1）;
????Fby_INSc=randn（j1）;
????Fbz_INSc=randn（j1）;%加速度計在各個方向上測量的數(shù)據(jù)
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（i1）/g*Ti;
????T32=Fby_INSc（i1）/g*Ti;
????T33=Fbz_INSc（i1）/g*Ti;
????T21=（Wibxb_INSc（i1）-Ti*T31*Wie*sin（L（1）））/Ti*Wie*cos（L（1））;
????T22=（Wibyb_INSc（i1）-Ti*T32*Wie*sin（L（1）））/Ti*Wie*cos（L（1））;
????T23=（Wibzb_INSc（i1）-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];%粗對準后確定的姿態(tài)矩陣
????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;?%地球自轉(zhuǎn)角速度在機體系的表示
????Wibb=[Wibxb_INSc（i1）?Wibyb_INSc（i1）?Wibzb_INSc（i1）]‘;%陀螺輸出的各個軸表示
????Fb=[Fbx_INSc（i1）?Fby_INSc（i1）?Fbz_INSc（i1）]‘;??%加速度計輸出的各個軸表示
????%Wibb‘=Wien‘*Cbn;
????%[Fbx_INSc（i1）?Fby_INSc（i1）?Fbz_INSc（i1）]=-1*gn‘*Cbn;
??%姿態(tài)角的計算
????if?abs（Cnb（22））>1e-10
????????if?Cnb（22）>0
???????????B（i+1）=atan（Cnb（21）/Cnb（22））;??????????????
????????elseif?Cnb（21）>0
???????????B（i+1）=atan（Cnb（21）/Cnb（22））+pi;
????????else?
????????????B（i+1）=atan（Cnb（21）/Cnb（22））-pi;
????????end
????elseif?Cnb（21）>0
???????B（i+1）=pi/2;
????else?
???????B（i+1）=-pi/2;
????end?????????????????????%求航向角
???
????C（i+1）=asin（Cnb（23））;???%求俯仰角
????if?abs（Cnb（33））>1e-10
????????if?Cnb（33）>0
???????????D（i+1）=atan（-Cnb（13）/Cnb（33））;
????????elseif?Cnb（13）>0
????????????D（i+1）=atan（-Cnb（13）/Cnb（33））-pi;
????????else
???????????D（i+1）=atan（-Cnb（13）/Cnb（33））+pi;
????????end
???elseif?Cnb（13）>0
???????D（i+1）=-pi/2;
???else?
???????D（i+1）=pi/2;
???end???????????????????%求橫滾角
???
????vx（i+1）=（Fbx_INSc（i1）+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（i1）-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（i1）+（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）;????????%經(jīng)度
?????
????

end


?t=0:0.1:j;
??%figure（1）
??%plot（jd‘r‘）;xlab

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???????3616??2007-11-08?19:19??INS_OUT.m

-----------?---------??----------?-----??----

?????????????????3834????????????????????2