?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;??