clc;close?all;
%%?讀取陀螺儀的數據
OMIGAbx=xlsread（‘FlightData.xls‘1‘B2:B1598‘）;
OMIGAby=xlsread（‘FlightData.xls‘1‘C2:C1598‘）;
OMIGAbz=xlsread（‘FlightData.xls‘1‘D2:D1598‘）;
%%?讀取加計數據
abx_tem=xlsread（‘FlightData.xls‘2‘B2:B14154‘）;
aby_tem=xlsread（‘FlightData.xls‘2‘C2:C14154‘）;
abz_tem=xlsread（‘FlightData.xls‘2‘D2:D14154‘）;
%%?篩選所需加計數據
t=xlsread（‘FlightData.xls‘3‘A2:A1598‘）;
t1=xlsread（‘FlightData.xls‘2‘A2:A14154‘）;
n=length（OMIGAbx）;
abx=zeros（n1）;aby=abx;abz=abx;
for?i=1:n
????for?j=1:length（t1）
????????if?t1（j）==t（i）
????????????abx（i）=abx_tem（j）;
????????????aby（i）=aby_tem（j）;
????????????abz（i）=abz_tem（j）;
????????end
????end?
end
%%?確定四元素速度以及位置的初始值
phi=0;psi=0;gam=0;%姿態角
Vx=zeros（n1）;Vy=Vx;Vz=Vx;%速度
Xx=Vx;Xy=Xx;Xz=Xx;%位移
C11=Vx;C12=C11;C13=C11;C21=