捷联惯导初始对准程序matlab，精对准，

 clc;
clear;
load fw;
Pi=3.1415926535897931;
Wie=0.000072921151467;                          %地球自转角速度
Re=6378135.072;
g=9.7803267714;
e=1.0 / 298.25;
T=0.01;
LOOP=65*60/T;
V（LOOP2）=0;
fangwei（LOOP2）=0;
zitai（LOOP3）=0;
wucha（LOOP10）=0;
fangcha（LOOP10）=0;
H=[eye（2）zeros（28）];                           %H量测矩阵
Q=diag（[（50E-6*g）^2（50E-6*g）^2（0.1*Pi/180/3600）^2（0.1*Pi/180/3600）^2（0.1*Pi/180/3600）^200000]）; %系统噪声方差
R=diag（[（0.001）^2（0.001）^2]）;                   %量测噪声方差阵
P0=diag（[（0.1）^2（0.1）^2（1*Pi/180）^2（1*Pi/180）^2（1*Pi/180）^2（100E-6*g）^2（100E-6*g）^2（0.1*Pi/180/3600）^2（0.1*Pi/180/3600）^2（0.1*Pi/180/3600）^2]）; %P初值
X0=zeros（101）;                                  %   X初值
fai=271*Pi/180;                                  %    航向
theta=0.006*Pi/180;