xingzuo_simulation.m

GPS/GALILEO/BEIDOU兼容接收机自主完好性检测程序

clear all;
close all;
clc;

%*************************************************** 
%** 三星座组合sse方法 
%*************************************************** 

%---------------------画球
[xq,yq,zq]=sphere(100);
xq=6378137*xq;yq=6378137*yq;zq=6378137*zq;
mesh(xq,yq,zq);colormap(winter);
hold on;
gps_flag = 1;
galileo_flag = 2;
bd_flag = 3;
gz_flag = 1;
sysnum = 3;
t=10000;
sigma = 10;

%-----------------------画24颗星gps
[Xs_gps,Ys_gps,Zs_gps]=gps_sat(t); %解卫星在地心地固坐标系中的位置
Xs0_gps=reshape(Xs_gps,1,24);
Ys0_gps=reshape(Ys_gps,1,24);
Zs0_gps=reshape(Zs_gps,1,24);
plot3(Xs0_gps,Ys0_gps,Zs0_gps,'b*');
hold on;
%------------------------画6条卫星轨道
[xt_gps,yt_gps,zt_gps]=solve_orbit(gps_flag);
plot3(xt_gps(:,1),yt_gps(:,1),zt_gps(:,1),'m',xt_gps(:,2),yt_gps(:,2),zt_gps(:,2),'m',xt_gps(:,3),yt_gps(:,3),zt_gps(:,3),'m',...
xt_gps(:,4),yt_gps(:,4),zt_gps(:,4),'m',xt_gps(:,5),yt_gps(:,5),zt_gps(:,5),'m',xt_gps(:,6),yt_gps(:,6),zt_gps(:,6),'m');
hold on;
%-----------------------画27颗星galileo
[Xs_galileo,Ys_galileo,Zs_galileo]=galileo_sat(t); %解卫星在地心地固坐标系中的位置
Xs0_galileo=reshape(Xs_galileo,1,27);
Ys0_galileo=reshape(Ys_galileo,1,27);
Zs0_galileo=reshape(Zs_galileo,1,27);
plot3(Xs0_galileo,Ys0_galileo,Zs0_galileo,'r*');
hold on;
%------------------------画3条卫星轨道
[xt_galileo,yt_galileo,zt_galileo]=solve_orbit(galileo_flag);
plot3(xt_galileo(:,1),yt_galileo(:,1),zt_galileo(:,1),'k',xt_galileo(:,2),yt_galileo(:,2),zt_galileo(:,2),'k',xt_galileo(:,3),yt_galileo(:,3),zt_galileo(:,3),'k');
hold on;
%---------------------画2颗星beidou
[Xs_beidou,Ys_beidou,Zs_beidou]=bd_sat(t); %解卫星在地心地固坐标系中的位置
Xs0_beidou=reshape(Xs_beidou,1,2);
Ys0_beidou=reshape(Ys_beidou,1,2);
Zs0_beidou=reshape(Zs_beidou,1,2);
plot3(Xs0_beidou,Ys0_beidou,Zs0_beidou,'g*');
hold on;

%------------------------画用户轨迹
[xut,yut,zut]=user_orbit(t);
plot3(xut,yut,zut,'-r');
hold on;
%------------------------画飞机
[long,lat,h]=usercontrail(t);
we=(7.2921151467e-5)*360/(2*pi);
long=long+we*t;
[xu0,yu0,zu0]=user_ecef(long,lat,h);   
plot3(xu0,yu0,zu0,'kd');
text(7e+7,2.5e+7,2e+7,'飞机：黑 菱形');
hold on;

[long,lat,h]= usercontrail(t);
[xu,yu,zu] = user_ecef(long,lat,h);
[xu1,yu1,zu1]=ecef_g(long,lat,xu,yu,zu);             %转换成用户地点的地理坐标

[Xs1_gps,Ys1_gps,Zs1_gps]=ecef_g(long,lat,Xs_gps,Ys_gps,Zs_gps);
[Xs1_galileo,Ys1_galileo,Zs1_galileo]=ecef_g(long,lat,Xs_galileo,Ys_galileo,Zs_galileo);
[Xs1_beidou,Ys1_beidou,Zs1_beidou]=ecef_g(long,lat,Xs_beidou,Ys_beidou,Zs_beidou);

[num_gps,k_gps] = get_seesate(Xs1_gps,Ys1_gps,Zs1_gps,xu1,yu1,zu1,gps_flag); 
[num_galileo,k_galileo] = get_seesate(Xs1_galileo,Ys1_galileo,Zs1_galileo,xu1,yu1,zu1,galileo_flag); 
[num_beidou,k_beidou] = get_seesate(Xs1_beidou,Ys1_beidou,Zs1_beidou,xu1,yu1,zu1,bd_flag);  

[X_gps,Y_gps,Z_gps]=get_k(num_gps,k_gps,Xs_gps,Ys_gps,Zs_gps,gps_flag);        %算出k颗可见星的坐标(地心地固坐标)
[X_galileo,Y_galileo,Z_galileo]=get_k(num_galileo,k_galileo,Xs_galileo,Ys_galileo,Zs_galileo,galileo_flag);        %算出k颗可见星的坐标(地心地固坐标)
[X_beidou,Y_beidou,Z_beidou] = get_k(num_beidou,k_beidou,Xs_beidou,Ys_beidou,Zs_beidou,bd_flag);

[XS_gps,YS_gps,ZS_gps] = ecef_g(long,lat,X_gps,Y_gps,Z_gps); % 转换为地理坐标
[XS_galileo,YS_galileo,ZS_galileo] = ecef_g(long,lat,X_galileo,Y_galileo,Z_galileo); % 转换为地理坐标
[XS_beidou,YS_beidou,ZS_beidou] = ecef_g(long,lat,X_beidou,Y_beidou,Z_beidou); % 转换为地理坐标

%-------------------------画可见星
plot3(X_gps,Y_gps,Z_gps,'yo')
s11_gps=int2str(k_gps);
s12_gps=mat2str(num_gps);
text(-3.5e+7,4e+7,5.5e+7,strcat('gps可见星有',s11_gps,'颗,号数为：',s12_gps,'  黄o'));
hold on;
plot3(X_galileo,Y_galileo,Z_galileo,'yo')
s11_galileo=int2str(k_galileo);
s12_galileo=mat2str(num_galileo);
text(-3.5e+7,2e+7,5.5e+7,strcat('galileo可见星有',s11_galileo,'颗,号数为：',s12_galileo,'  绿o'));
hold on;
plot3(X_beidou,Y_beidou,Z_beidou,'yo')
s11_beidou = int2str(k_beidou);
s12_beidou = mat2str(num_beidou);
hold on;

k = k_gps+k_galileo+k_beidou;
num = [num_gps,num_galileo,num_beidou];
i=1;
if(i<=k_gps)
    gz_flag = gps_flag;
    Xs = Xs_gps; Ys = Ys_gps; Zs = Zs_gps;
end
if((i>k_gps)&&(k<=k_gps+k_galileo))
    gz_flag = galileo_flag;
    Xs = Xs_galileo; Ys = Ys_galileo; Zs = Zs_galileo;
end
if(i>k_gps+k_galileo)
    gz_flag = bd_flag;
    Xs = Xs_beidou; Ys = Ys_beidou; Zs = Zs_beidou;
end
bias=120;
b = creat_b(k,i,bias);
e = creat_e(k,t);
%-------------------------画设置的故障星
if bias>=100
    [xcf,ycf,zcf] = translatenum(num(1,i),1,Xs,Ys,Zs,gz_flag);       %算故障星在固定坐标系中的坐标
    plot3(xcf,ycf,zcf,'k*');                                %画设置的故障星  黑*
    s4=mat2str(num(1,i));
    sss = mat2str(gz_flag);
    text(-3.5e+7,2e+7,4.5e+7,strcat(sss,'设置的故障星号数为：',s4,'  黑*'));
    hold on;
end

xsg1_gps = XS_gps';ysg1_gps = YS_gps';zsg1_gps = ZS_gps';
xsg1_galileo = XS_galileo';ysg1_galileo = YS_galileo';zsg1_galileo = ZS_galileo';
xsg1_beidou = XS_beidou';ysg1_beidou = YS_beidou';zsg1_beidou = ZS_beidou';
H3   = Third_get_H(k_gps,k_galileo,k_beidou,XS_gps',YS_gps',ZS_gps',XS_galileo',YS_galileo',ZS_galileo',XS_beidou',YS_beidou',ZS_beidou',xu1,yu1,zu1);

xsg1 = [xsg1_gps;xsg1_galileo;xsg1_beidou];
ysg1 = [ysg1_gps;ysg1_galileo;ysg1_beidou];
zsg1 = [zsg1_gps;zsg1_galileo;zsg1_beidou];
y3=solve_yy(k,xsg1,ysg1,zsg1,xu1,yu1,zu1,H3,b,e,sysnum,sigma);
xjs=inv(H3'*H3)*H3'*y3 ;             %xjs是用户状态的最小二乘解
R3=get_R(H3,y3);
sqrtsse=sqrt(R3'*R3/(k-6));
sigma = 10;
freedom = k-6;
P_fa = 0.002/360;
[T_10s,Td_10s] = kaifang_ceiling(sigma,freedom,P_fa);
Td_menian = Td_10s;
if  sqrtsse<=Td_menian
     msgbox('无故障星','检测结果');
else
    [errsate,num_noproblem,errorsys_flag]=judge_errsate(k_gps,k_galileo,k_beidou,H3,y3,num);
end

%-------------------------画测出的故障星
if errorsys_flag ==1
[xcs,ycs,zcs]=translatenum(errsate,1,Xs_gps,Ys_gps,Zs_gps,errorsys_flag);         %画测出的故障星  黑  正五边形
plot3(xcs,ycs,zcs+2000000,'kp');                        %此处给z坐标多加了1500，是为了作图时在星的旁边作记号，以免重叠
s5=mat2str(errsate);
text(-3.5e+7,2e+7,4e+7,strcat('测出的故障gps星号数为：',s5,' 黑  正五边'));
if errsate==num_gps(1,i)
    text(2e+7,-3e+7,4.5e+7,'判断正确');
else
    text(2e+7,-3e+7,4.5e+7,'判断错误');
end
end

%-------------------------画测出的故障星
if errorsys_flag ==2
[xcs,ycs,zcs]=translatenum(errsate,1,Xs_galileo,Ys_galileo,Zs_galileo,errorsys_flag);         %画测出的故障星  黑  正五边形
plot3(xcs,ycs,zcs+2000000,'kp');                        %此处给z坐标多加了1500，是为了作图时在星的旁边作记号，以免重叠
s5=mat2str(errsate);
text(-3.5e+7,2e+7,4e+7,strcat('测出的故障galileo星号数为：',s5,' 黑  正五边'));
if errsate==num_galileo(1,i)
    text(2e+7,-3e+7,4.5e+7,'判断正确');
else
    text(2e+7,-3e+7,4.5e+7,'判断错误');
end
end

%-------------------------画测出的故障星
if errorsys_flag ==3
[xcs,ycs,zcs]=zhang_beidou_translatenum(errsate,1,Xs_beidou,Ys_beidou,Zs_beidou,errorsys_flag);         %画测出的故障星  黑  正五边形
plot3(xcs,ycs,zcs+2000000,'kp');                        %此处给z坐标多加了1500，是为了作图时在星的旁边作记号，以免重叠
s5=mat2str(errsate);
text(-3.5e+7,2e+7,4e+7,strcat('测出的故障beidou星号数为：',s5,' 黑  正五边'));
if errsate==num_beidou(1,i)
    text(2e+7,-3e+7,4.5e+7,'判断正确');
else
    text(2e+7,-3e+7,4.5e+7,'判断错误');
end
end