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）;%陀螺在各個(gè)方向上測量的數(shù)據(jù)
????Fbx_INSc=randn（j1）;
????Fby_INSc=randn（j1）;
????Fbz_INSc=randn（j1）;%加速度計(jì)在各個(gè)方向上測量的數(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];%粗對準(zhǔn)后確定的姿態(tài)矩陣
????Cnb=Cbn‘;
????
????gnx=0;
????gny=0;
????gnz=g;
????gn=[gnx;gny;gnz];%3*1
????gb=gn‘*Cbn;???%重力加速度在機(jī)體系的表示?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)角速度在機(jī)體系的表示
????Wibb=[Wibxb_INSc（i1）?Wibyb_INSc（i1）?Wibzb_INSc（i1）]‘;%陀螺輸出的各個(gè)軸表示
????Fb=[Fbx_INSc（i1）?Fby_INSc（i1）?Fbz_INSc（i1）]‘;??%加速度計(jì)輸出的各個(gè)軸表示
????%Wibb‘=Wien‘*Cbn;
????%[Fbx_INSc（i1）?Fby_INSc（i1）?Fbz_INSc（i1）]=-1*gn‘*Cbn;
??