close?all;
clear?all;
clc;
load?a.txt;?%讀文件，為一個六列矩陣，前三列為陀螺輸出角速度，后三列為加計輸出
format?long??%有效數字十六位
%初始化參數
%?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;?????????????????????%初始經緯度，經緯度化成弧度
%?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;????%采樣時間
j=14;

g=9.78049;

wib_c?=?a（:1:3）‘;???%將a矩陣里的一至三列放入wib_c
f_c?=?a（:4:6）‘;??????%將a矩陣里的一至三列放入f_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）