IMU导航解算程序

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=