clc
clear?all
global?Re?f?wie?h?vx?vy?vz?g0?longi
Re=6378137;%地球半徑
f=1/298.257;%橢圓度
wie=360/24/3600;%地球自轉(zhuǎn)角速度
h=2000;%飛機(jī)飛行高度
longi=116;%飛機(jī)飛行經(jīng)度
tl=1000;%時(shí)長
g0=9.7536;
wg=g0*10^-6;
ws=2*pi/（84.4*60）;
%%飛機(jī)飛行速度
vx=0;
vy=300;
vz=0;
%%飛機(jī)飛行偏航角、俯仰角、滾動(dòng)角
p=0;
q=0;
r=0;
?
%%飛機(jī)飛行軌跡
lati（1）=39;
Rm（1）=Re*（1-2*f+3*f*sin（lati（1））^2）;
Rn（1）=Re*（1+f*sin（lati（1））^2）;
for?i=2:tl
???lati（i）=vy/（Rm（i-1）+h）+lati（i-1）;
???Rm（i）=Re*（1-2*f+3*f*sin（lati（i））^2）;
Rn（i）=Re*（1+f*sin（lati（i））^2）;
end
?
%%設(shè)置初始狀態(tài)
X=zeros（15tl+1）;
X1=zeros（15tl+1）;
Xerror=zeros（15tl+1）;
Z=zeros（6tl）;
O3=zeros（33）;
I3=eye（3）;
I15=eye（15）;
I17=eye（17）;
Qw=diag（[（0.01/3600）^2?（0.01/3600）^2?（0.01/3600）^2?（50*wg）^2?（50*wg）^2?（50*wg）^2?0?0?0?（0.001/3600）^2?（0.001/3600）^2?（0.001/3600）^2?（50*wg）^2?（50*wg）^2?（50*wg）^2]）;
Rk=diag（[3.5^2?3.5^2?3.5^2?0.25?0.25?0.25]）;
P0=Qw;
Ngps=zeros（tl1）;?%%?GPS故障
%%加速度計(jì)故障
Nxa=zeros（tl1）;
Nya=zeros（tl1）;
Nza=zeros（tl1）;
?
kkkgps=1;
%%開始濾波
for?kgps=1:5???????????
Ngps（300:tl1）=（40+kgps*20）;???%五種GPS故障
%?Nxa（300:tl1）=（500+100*kgps）*wg;????%x軸加速度計(jì)
%?Nya（300:tl1）=（500+100*kgps）*wg;?%y軸加速度計(jì)
????for?kkgps=1:40???????????????%各40次?????????
for?i=1:tl
????Fn=function_F（lati（i）Rm（i）Rn（i））;?
????Fs=[O3?I3;I3?O3;O3?O3];
????Fm=diag（[-1/3600?-1/3600?-1/3600?-1/3600?-1/3600?-1/3600]）;
????F=[Fn?Fs;zeros（69）?Fm];?
????????ph=I15;
for?k=11:2
??ph=I15+F/k*ph;?
end
Phi=I15+F*ph;
?
????hh=zeros（36）;
hhh=diag（[（Rm（i）+h）?（Rn（i）+h）*cos（lati（i））?1]）;
????Hp=[hh?hhh?hh];
????Hv=[diag（[1?1?1]）?zeros（312）];
????H=[Hp;Hv];????
????Qwt=zeros（151）;??
????Qwt（7）=ws^2/（ws^2-wie^2）*（1/wie*sin（wie*i）-1/ws*sin（ws*i））*Nxa（i）+ws^2*wie*sin（lati（i））/（ws^2-wie^2）*（1/ws^2*cos（ws*i）-1/wie^2*cos（wie*i））*Nya（i）+sin（lati（i））/wie*Nya（i）;
????Qwt（7）=Qwt（7）+1/g0*（1-cos（ws*i））*Nya（i）;
????Qwt（8）=（tan（lati（i））/wie*（1-cos（wie*i））-wie*tan（lati（i））/（ws^2-wie^2）*（cos（wie*i）-cos（ws*i）））*Nxa（i）;
????Qwt（8）=Qwt（8）+（sec（lati（i））*（ws^2-wie^2*cos（lati（i））^2）/ws*sin（ws*i）/（ws^2-wie^2）-ws^2*tan（lati（i））*sin（lati（i））/wie*sin（ws*i）/（ws^2-wie^2）-i*cos（lati（i）））*Nya（i）;
????Qwt（8）=Qwt（8）+sec（lati（i））/g0*（1-cos（ws*i））*Nxa（i）;
????
Xerror（:i）=X1（:i）+Qwt;????
Xerror（:i+1）=Phi*（Xerror（:i））+diag（randn（115））*[0?0?0?0?0?0?0?0?0?（0.001/3600）?（0.001/3600）?（0.001/3600）?（50*wg）?（50*wg）?（50*wg）]‘;????
????Z（:i）=H*（Xerror（:i+1））+[Ngps（i）?Ngps（i）?Ngps（i）?0?0?0]‘+diag（randn（16））*[3.5?3.5?3.5?0.5?0.5?0.5]‘;
Pk=Phi*P0*Phi‘;?
KK=Pk*H‘*inv（H*Pk*H‘+?Rk）;
X1（:i+1）=Phi*（X1（:i））;
?X（:i）=X1（:i+1）+KK*（Z（:i）-H*X1（:i+1））;
?residual（:i）=X（:i）-X1（:i+1）;
?P0=（I15-KK*H）*Pk;
?for?ki=1:15
beta1（kkkgpskii）=residual（kii）‘*inv（P0（kiki））*residual（kii）;
?end?
end
beta（kkkgps::）=beta1（kkkgps::）/max（max（beta1（kkkgps::）））;
?kkkgps=kkkgps+1
????end
end
beta=beta;
save?Chi2test.mat?beta;

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----

?????文件???????2937??2015-01-11?21:36??程序\第6章程序\chengxu1.m

?????文件???????4118??2015-01-11?21:38??程序\第6章程序\chengxu2.m

?????文件???22963061??2015-01-11?21:38??程序\第6章程序\Chi2test.mat

?????文件???22802468??2015-01-11?21:31??程序\第6章程序\Chi2test1.mat

?????文件???22639327??2015-01-11?21:29??程序\第6章程序\Chi2test2.mat

?????文件????????929??2014-09-16?21:10??程序\第6章程序\function_F.m

?????文件??????26194??2015-01-11?21:42??程序\第6章程序\第六章程序.docx

?????文件???????1419??2015-01-21?11:19??程序\第7章程序\程序1\hbf.m

?????文件???????1346??2015-01-21?11:19??程序\第7章程序\程序1\hbf_input.m

?????文件???????1289??2015-01-21?11:19??程序\第7章程序\程序1\hbf_obv.m

?????文件????????823??2015-01-21?11:19??程序\第7章程序\程序1\hbf_plant.m

?????文件????????527??2015-01-21?11:19??程序\第7章程序\程序1\hbf_plot.m

?????文件??????29596??2015-01-21?11:19??程序\第7章程序\程序1\hbf_sim.mdl

?????文件???????1210??2015-01-21?11:20??程序\第7章程序\程序2\ccaguzhang.m

?????文件???????9769??2015-01-21?11:20??程序\第7章程序\程序2\ccarb.m

?????文件???????1061??2015-01-21?11:20??程序\第7章程序\程序2\ccaspfx.m

?????文件???????1142??2015-01-21?11:20??程序\第7章程序\程序2\ccaspfy.m

?????文件???????1095??2015-01-21?11:20??程序\第7章程序\程序2\ccaspfz.m

?????文件?????????56??2015-01-21?11:20??程序\第7章程序\程序2\wc.m

?????文件??????32305??2015-01-22?08:59??程序\第7章程序\第七章程序.docx

?????文件????????867??2015-01-11?16:40??程序\第8章程序\chap1_1obv.m

?????文件????????707??2015-01-11?16:39??程序\第8章程序\chap1_1plant.m

?????文件????????541??2015-01-11?16:40??程序\第8章程序\chap1_1plot.m

?????文件???????1407??2015-01-11?16:39??程序\第8章程序\chap1_1rbf.m

?????文件??????30847??2015-01-11?16:39??程序\第8章程序\chap1_1sim.mdl

?????文件??????31376??2015-01-11?16:39??程序\第8章程序\單隱含層模糊遞歸小波神經(jīng)網(wǎng)絡(luò)觀測器的程序.docx

?????文件??????21935??2015-01-11?20:06??程序\第8章程序\第八章程序.docx

?????文件???????8692??2015-01-04?09:55??程序\第9章程序\adkf.m

?????文件????????398??2014-05-30?19:44??程序\第9章程序\ekf.m

?????文件??????12566??2014-06-06?18:55??程序\第9章程序\fangan3.m

............此處省略23個(gè)文件信息