close?all;
clear?all;
clc;
load?a.txt;?%讀文件，為一個(gè)六列矩陣，前三列為陀螺輸出角速度，后三列為加計(jì)輸出
format?long??%有效數(shù)字十六位
%初始化參數(shù)
%?vx（1）=0.000048637;
%?vy（1）=0.000206947;
%?vz（1）=0.007106781;??????????%初始速度
vx（1）=0;
vy（1）=0;
vz（1）=0;
J（1）=116.344695283*2*pi/360;
L（1）=39.975172*2*pi/360;?????????????????????%初始經(jīng)緯度，經(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;????%采樣時(shí)間
j=14;

g=9.78049;

wib_c?=?a（:1:3）‘;???%將a矩陣?yán)锏囊恢寥蟹湃雡ib_c
f_c?=?a（:4:6）‘;??????%將a矩陣?yán)锏囊恢寥蟹湃雈_c
wib_x?=?a（:1）;
wib_y?=?a（:2）;
wib_z?=?a（:3）;
fx?=?a（:4）;
fy?=?a（:5）;
fz?=?a（:6）;

for?i=1:j;
????Rx=re/（1-e*sin（L（1））^2）;????????%卯酉圈曲率半徑
????Ry=re/（1+2*e-3*e*sin（L（1））^2）;??%子午圈主曲率半徑
??
????q（1）?=?cos（B（1）/2）*cos（C（1）/2）*cos（D（1）/2）-sin（B（1）/2）*sin（C（1）/2）*sin（D（1）/2）;
????q（2）?=?cos（B（1）/2）*sin（C（1）/2）*cos（D（1）/2）-sin（B（1）/2）*cos（C（1）/2）*sin（D（1）/2）;
????q（3）?=?cos（B（1）/2）*cos（C（1）/2）*sin（D（1）/2）+sin（B（1）/2）*sin（C（1）/2）*cos（D（1）/2）;
????q（4）?=?cos（B（1）/2）*sin（C（1）/2）*sin（D（1）/2）+sin（B（1）/2）*cos（C（1）/2）*cos（D（1）/2）;
????
????T11?=?（q（1））^2?+?（q（2））^2?-?（q（3））^2?-?（q（4））^2;
????T12?=?2*q（3）*q（2）?+?2*q（1）*q（4）;?
????T13?=?2*q（4）*q（2）?-?2*q（1）*q（3）;
????T21?=?2*q（3）*q（2）?-?2*q（1）*q（4）;
????T22?=?（q（1））^2?-?（q（2））^2?+?（q（3））^2?-?（q（4））^2;
????T23?=?2*q（3）*q（4）?-?2*q（1）*q（2）;
????T31?=?2*q（4）*q（2）?+?2*q（1）*q（3）;
????T32?=?2*q（3）*q（4）?-?2*q（1）*q（2）;
????T33?=?（q（1））^2?-?（q（2））^2?-?（q（3））^2?+?（q（4））^2;???????
????

????Cbn=[T11?T12?T13;T21?T22?T23;T31?T32?T33];%粗對(duì)準(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
?