clc;
clear;
format?long
%%?定義變量
%數據時間（s）
Total_Time_s=600;????
%kalman?濾波周期
Kalman_Time_s=1;
%慣導解算周期（s）
dt=0.01;
%是否反饋（1為反饋）
Fankui_Guandao=1;??
%選用數據段（1為讀數據；0為造數據）
Shuju_Laiyuan=1;
%數據個數
n=Total_Time_s/dt;
%kalman運算次數
n1=Total_Time_s/Kalman_Time_s;
%%?慣導存儲信息
V1=zeros（31）;V=zeros（n3）;Pos=zeros（n3）;Gest=zeros（n3）;wibb=zeros（n3）;
fb=zeros（n3）;I=eye（4）;C1=zeros（33）;fp=zeros（31）;wepp=zeros（13）;
wipp=zeros（31）;wipb=zeros（31）;wpbb=zeros（3n）;dxt=zeros（44）;?data=zeros（n6）;pp=zeros（n2）;
wiep=zeros（31）;q=zeros（41）;Vep=[0?0?0];
%%?定義初值
%姿態角（單位弧度）
%?pitch=0.00574143129887982*pi/180;
%?roll=-0.00570276481225802*pi/180;
%?yaw=0.0939588201389148*pi/180;
pitch=0.5*pi/180;
roll=-0.5*pi/180;
yaw=1*pi/180;
%經緯度（單位弧度）
Longitude=116.34340341*pi/180;
Lat=39.9778*pi/180;
%地球半徑
re=6378137;
%橢球率
e=1.0/298.257223563;
%游動方位角
a=0;
%地球自轉角速度（弧度每秒）
wie=7.292115147e-5;
%慣導解算高度
h=40;
%高度計高度
hr=40;
%?X=0;
k=1;
%慣導器件反饋初值
w1=0;w2=0;w3=0;f1=0;f2=0;f3=0;

%求C?初始位置矩陣
C=[-sin（a）*sin（Lat）*cos（Longitude）-cos（a）*sin（Longitude）?-sin（a）*sin（Lat）*sin（Longitude）+cos（a）*cos（Longitude）?sin（a）*cos（Lat）;
???-cos（a）*sin（Lat）*cos（Longitude）+sin（a）*sin（Longitude）?-cos（a）*sin（Lat）*sin（Longitude）-sin（a）*cos（Longitude）?cos（a）*cos（Lat）;
???cos（Lat）*cos（Longitude）?cos（Lat）*sin（Longitude）?sin（Lat）];
%?g的計算
g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;
g=g0*（1+gk1*C（33）^2）*（1-2*h/re）/sqrt（1-gk2*C（33）^2）;
%初始T?姿態矩陣
T=[cos（roll）*cos（yaw）-sin（roll）*sin（pitch）*sin（yaw）?-cos（pitch）*sin（yaw）?sin（roll）*cos（yaw）+cos（roll）*sin（pitch）*sin（yaw）;
???cos（roll）*sin（yaw）+sin（roll）*sin（pitch）*cos（yaw）?cos（pitch）*cos（yaw）?sin（roll）*sin（yaw）-cos（roll）*sin（pitch）*cos（yaw）;
???-sin（roll）*cos（pitch）?sin（pitch）?cos（roll）*cos（pitch）];
???%初始四元數計算
??q（2）=0.5*sign（T（32）-T（23））*sqrt（1+T（11）-T（22）-T（33））;
??q（3）=0.5*sign（T（13）-T（31））*sqrt（1-T（11）+T（22）-T（33））;
??q（4）=0.5*sign（T（21）-T（12））*sqrt（1-T（11）-T（22）+T（33））;
??q（1）=sqrt（1-q（2）^2-q（3）^2-q（4）^2）;
%阻尼系數
K1=0.03;K2=4.0*10^（-4）+2*g/（re+h）;K3=2*10^（-6）;
ad=0;vd=0;a0=0;??
??Rm=re*（1-2*e+3*e*C（33）^2）;???????????Rn=re*（1+e*C（33）^2）;?
??%%?讀文件
if?Shuju_Laiyuan==1?%導入數據
%?load（‘IMU_data_noise.mat‘）;
load（‘E:/靜態1.txt‘）;
end
if?Shuju_Laiyuan==0??%仿造數據
???ug=1e-6*g;???????????????????rd=pi/180/3600;????????????
???for?i=1:n
???????data（i1）=?????0??????????+0.02*rd???;?????%????????+random（‘normal‘0（0.01*rd）1）?;??
???????data（i2）=wie*cos（Lat）????+0.02*rd????;?????%?????????+random（‘normal‘0（0.01*rd）1）?;??
???????data（i3）=wie*sin（Lat）????+0.02*rd?????;?????%???????+random（‘normal‘0（0.01*rd）1）?;?????
???????data（i4）=?????0?????????+100*ug???????;?????%??????+random（‘normal‘0（50*ug）1）???;??
???????data（i5）=?????0?????????+100*ug???????;?????%????????+random（‘normal‘0（50*ug）1）??;??
???????data（i6）=?????g??????????+100*ug??????;?????%?????????+random（‘normal‘0（50*ug）1）?;?
???end
end
%%
?%%卡曼濾波初始化%%
%估計值初值
x=zeros（151）;
fai=zeros（3