clc
clear all
global Re f wie h vx vy vz g0 longi
Re=6378137;%地球半径
f=1/298.257;%椭圆度
wie=360/24/3600;%地球自转角速度
h=2000;%飞机飞行高度
longi=116;%飞机飞行经度
tl=1000;%时长
g0=9.7536;
wg=g0*10^-6;
ws=2*pi/(84.4*60);
%%飞机飞行速度
vx=0;
vy=300;
vz=0;
%%飞机飞行偏航角、俯仰角、滚动角
p=0;
q=0;
r=0;
%%飞机飞行轨迹
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
%%设置初始状态
X=zeros(15,tl+1);
X1=zeros(15,tl+1);
Xerror=zeros(15,tl+1);
Z=zeros(6,tl);
O3=zeros(3,3);
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(tl,1); %% GPS故障
%%加速度计故障
Nxa=zeros(tl,1);
Nya=zeros(tl,1);
Nza=zeros(tl,1);
kkkgps=1;
%%开始滤波
for kgps=1:5
Ngps(300:tl,1)=(40+kgps*20); %五种GPS故障
% Nxa(300:tl,1)=(500+100*kgps)*wg; %x轴加速度计
% Nya(300:tl,1)=(500+100*kgps)*wg; %y轴加速度计
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(6,9) Fm];
ph=I15;
for k=11:2
ph=I15+F/k*ph;
end
Phi=I15+F*ph;
hh=zeros(3,6);
hhh=diag([(Rm(i)+h) (Rn(i)+h)*cos(lati(i)) 1]);
Hp=[hh hhh hh];
Hv=[diag([1 1 1]) zeros(3,12)];
H=[Hp;Hv];
Qwt=zeros(15,1);
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(1,15))*[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(1,6))*[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(kkkgps,ki,i)=residual(ki,i)'*inv(P0(ki,ki))*residual(ki,i);
end
end
beta(kkkgps,:,:)=beta1(kkkgps,:,:)/max(max(beta1(kkkgps,:,:)));
kkkgps=kkkgps+1
end
end
beta=beta;
save Chi2test.mat beta;
