捷联惯导粗对准

disp（‘初始化‘）;
%%%%%%%地球数据
WIE  = 7.292115e-5 ;%（地球自转角速度，弧度制commented by LiYou）
e   = 0.0818191908426;
R   = 6378137.0;
Tm = 0.01;
%%%%%%%速度初值
Vn = 0;
Vt = 0;
Ve = 0;
V_last = [VnVtVe]‘;
%%%位置初值
Lati  = 28.2202*pi/180;
Longti = 112.9916*pi/180;
Alti      = 0;
g0=9.7803267711905*（1+0.00193185138639*sin（Lati）^2）/（（1-0.00669437999013*sin（Lati）^2）^0.5*（1.0+Alti/R）^2）;
Pos = zeros（31）;
disp（‘初始化完成‘）;
%%%%%%%%%%%%%%%%%%%%%%%%%%%% 初 始 化 完 成 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%载 入 数 据 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp（‘载入数据‘）;
load d_GyroAcc2012.dat
[row columu] = size（d_GyroAcc2012）;
disp（‘载入数据完成‘）
%%%%%%%%%%%%%%%%%%%%%%载 入 数 据 完 成 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%   解 析 初 对 准 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp（‘解析粗对准开