惯导IMU初始姿态解算粗对准MATLAB

clear;
disp（‘初始化‘）;
WIE  = 7.292115e-5 ;% 地球自转角速度
Tm = 0.005;
Lati  = 51.2124539701*pi/180;
Longti = -114.0248136140*pi/180;
Alti      = 1077.393;
g0=9.7936174;
disp（‘初始化完成‘）;

disp（‘载入数据‘）;
A=load（‘staticdata.txt‘）;
disp（‘载入数据完成‘）;

Data（:1） = A（1:140032）;
Data（:2） = A（1:140033）;
Data（:3） = A（1:140034）; 
Data（:4） = A（1:140035）;
Data（:5） = A（1:140036）;
Data（:6） = A（1:140037）;
%陀螺数据
W1 = Data（:1:3）;
%加速度计数据
A1 = Data（:4:6）;
%导航系
Wien = [WIE*cos（Lati） 0 -WIE*sin（Lati）];
Gn   = [0 0 g0