clc;
clear;
format?long;
%%%%%%%%%%%%%%%%%%%%%讀數(shù)據(jù)%%%%%%%%%%%%%%%%%%%%
fid=fopen（‘Data180528022452.tsv‘‘r‘）;
tline?=?fgetl（fid）;?tline?=?fgetl（fid）;%讀兩行字符
C=textscan（fid‘%f‘）;
count1=size（C{11}）;
n=count1（1）/7;?%n是數(shù)據(jù)的組數(shù)
delta_t=0.01;?%采樣時(shí)間0.01s
%%%%%%%%%%%%%%%%%常值參數(shù)%%%%%%%%%%%%%%%%%
g0=9.78049;
w_ie=7.292115e-5;?%弧度
Re=6378137;
Rp=6356752;
f=1/298.257;
%%%%%%%%%%%%%%%%%%%初始值%%%%%%%%%%%%%%%%%%%%%%
theta=11.8/180*pi;?gamma=168.76/180*pi;?psi=141.3/180*pi;?%初始姿態(tài)角
v_E_n=0;?v_N_n=0;?v_U_n=0;?????%初始速度
latitude=39.96083*pi/180;?longitude=116.31922*pi/180;??height=60;?%初始經(jīng)緯高
%?初始四元數(shù)?及C_n_bC_b_n計(jì)算↘
q=[0;0;0;0];
q（1）=cos（psi/2）*cos（theta/2）*cos（gamma/2）-sin（psi/2）*sin（theta/2）*sin（gamma/2）;
q（2）=cos（psi/2）*sin（theta/2）*cos（gamma/2）+sin（psi/2）*cos（theta/2）*sin（gamma/2）;
q（3）=cos（psi/2）*cos（theta/2）*sin（gamma/2）-sin（psi/2）*sin（theta/2）*cos（gamma/2）;
q（4）=cos（psi/2）*sin（theta/2）*sin（gamma/2）-sin（psi/2）*cos（theta/2）*cos（gamma/2）;
T11=q（1）^2+q（2）^2-q（3）^2-q（4）^2;
T12=2*（q（2）*q（3）-q（1）*q（4））;
T13=2*（q（2）*q（4）+q（1）*q（3））;
T21=2*（q（2）*q（3）+q（1）*q（4））;
T22=q（1）^2-q（2）^2+q（3）^2-q（4）^2;
T23=2*（q（4）*q（3）-q（1）*q（2））;
T31=2*（q（2）*q（4）-q（1）*q（3））;
T32=2*（q（4）*q（3）+q（1）*q（2））;
T33=q（1）^2-q（2）^2-q（3）^2+q（4）^2;
C_n_b=[T11T12T13;T21T22T23;T31T32T33];
C_b_n=C_n_b.‘;
%?由地球自轉(zhuǎn)角速率計(jì)算w_n_ie，w_n_en↘
Rm=Re*（1-2*f+3*f*（sin（latitude））^2）;
Rn=Re*（1+f*（sin（latitude））^2）;
w_n_ie=[0;?w_ie*cos（latitude）;?w_ie*sin（latitude）?];
w_n_en=[?-v_N_n/（Rm+height）;?v_E_n/（Rn+height）;?v_E_n*tan（latitude）/（Rn+height）?];
%?重力加速度的初始值計(jì)算g↘
g=g0*（1-2*height/Re）;
%%%%%%%%%%%%%%%%%計(jì)算循環(huán)%%%%%%%%%%%%%%%%%%%%%%
for?i=1:n-1
????%%%%%%%%%%%%%%%%%%%%%%%%%讀一組數(shù)據(jù)%%%%%%%%%%%%%%
????w_b_ib=zeros（31）;?f_b_ib=zeros（31）;
????time=C{11}（i*7+1）;
????w_b_ib（1）=C{11}（i*7+5）;
????w_b_ib（2）=C{11}（i*7+6）;
????w_b_ib（3）=C{11}（i*7+7）;
????f_b_ib（1）=C{11}（i*7+2）*g;
????f_b_ib（2）=C{11}（i*7+3）*g;
????f_b_ib（3）=C{11}（i*7+4）*g;
????%%%%%%%%%%%%%%%%%%姿態(tài)角速度w_b_nb?計(jì)算%%%%%%%%%%%%%%%
????w_b_nb=w_b_ib-C_n_b*（w_n_ie+w_n_en）;
????%%%%%%%%%%%%%%%%%%%姿態(tài)矩陣求解%%%%%%%%%%%%%%%%%%%
????%/******************四元數(shù)更新矩陣***********************/%
????DELTA_THETA=w_b_nb*delta_t;
????DELTA_THETA0=norm（DELTA_THETA）;
????I=diag（[1111]）;
????DELTA_THETA_MATRIX=[?0?-w_b_nb（1）*delta_t?-w_b_nb（2）*delta_t?-w_b_nb（3）*delta_t;
????????w_b_nb（1）*delta_t?0?w_b_nb（3）*delta_t?-w_b_nb（2）*delta_t;
????????w_b_