clear all;
clc;
ratebuffer =[];
ratebuffer_s =[];
biasbuffer = [];
gps_flag = 1;
galileo_flag = 2;
bd_flag = 3;
tao = 10;
for bias=0:10:200;
     bnum=0;anum=0;
     bnum_s=0;anum_s=0;
    for t=0:tao:2400
        %--------GPS---------------------------------------
        [Xs_gps,Ys_gps,Zs_gps]=gps_sat(t); %解衛(wèi)星在地心地固坐標系中的位置
        %--------Galileo-----------------------------------
        [Xs_galileo,Ys_galileo,Zs_galileo]=galileo_sat(t);
         %--------Beidou-----------------------------------
        [Xs_beidou,Ys_beidou,Zs_beidou]=bd_sat(t); %解衛(wèi)星在地心地固坐標系中的位置
        %------用戶軌跡------------------------------------     
        [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); % 轉換為地理坐標
        
        k = k_gps+k_galileo+k_beidou;
        num = [num_gps,num_galileo,num_beidou];
        i=1;
        b = creat_b(k,i,bias);
        e = creat_e(k,t);
        
        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';
      
        if k_gps<=4
            anum_s = anum_s;
        else
            anum_s = anum_s+1;
            H_s = Single_get_H(k_gps,XS_gps',YS_gps',ZS_gps',xu1,yu1,zu1); 
            y_s = solve_yy(k_gps,xsg1_gps,ysg1_gps,zsg1_gps,xu1,yu1,zu1,H_s,b(1:k_gps),e(1:k_gps),1,sigma);            
            R_s = get_R(H_s,y_s);
            sqrtsse_s = sqrt(R_s'*R_s/(k_gps-4)); 
            [T_menxian_s,Td_menian_s] = kaifang_ceiling(10,k_gps-4,0.002/(3600/tao));
           if sqrtsse_s<=Td_menian_s            
           else
               bnum_s=bnum_s+1;
           end
        end
        
        if k<=6
           anum=anum; 
        else
           anum=anum+1;
           H = 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];
           y = solve_yy(k,xsg1,ysg1,zsg1,xu1,yu1,zu1,H,b,e,3,);
           R = get_R(H,y);
           sqrtsse=sqrt(R'*R/(k-6)); 
           [T_menxian,Td_menian] = kaifang_ceiling(10,k-6,0.002/(3600/tao));
           if sqrtsse<=Td_menian            
           else
               bnum=bnum+1;
           end
       end
  end
    rate=bnum/anum;
    rate_s = bnum_s/anum_s;
    ratebuffer = [ratebuffer rate];
     ratebuffer_s = [ratebuffer_s rate_s];
    biasbuffer = [biasbuffer bias];
end

plot(biasbuffer,ratebuffer,'b-',biasbuffer,ratebuffer_s,'r-','LineWidth',2);
legend('gps/galileo/beidou','gps');
title('故障檢測率');
xlabel('偏差(m)');
ylabel('故障檢測率');
grid on