guzhangshibielv.m

RAIM 完好性分析

clear all;
clc;
ratebuffer =[];
ratebuffer_s =[];
biasbuffer = [];
gps_flag = 1;
galileo_flag = 2;
bd_flag = 3;
tao = 10;
sigma = 10;
for bias=0:10:500;
     bnum=0;anum=0;
     bnum_s=0;anum_s=0;
    for t=0:tao:3600
        %--------GPS---------------------------------------
        [Xs_gps,Ys_gps,Zs_gps]=gps_sat(t); %解卫星在地心地固坐标系中的位置
        %--------Galileo-----------------------------------
        [Xs_galileo,Ys_galileo,Zs_galileo]=galileo_sat(t);
         %--------Beidou-----------------------------------
        [Xs_beidou,Ys_beidou,Zs_beidou]=bd_sat(t); %解卫星在地心地固坐标系中的位置
        %------用户轨迹------------------------------------     
        [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;
            if k_gps>=6
                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
                    [errsate_s,num_noproblem_s,errorsys_flag_s]=judge_errsate(k_gps,0,0,H_s,y_s,num_gps);
                    if  num(i) == errsate_s
                        bnum_s = bnum_s+1;                  
                    end 
                end
            end
        end
        
        if k<=6
           anum=anum; 
        else
           anum=anum+1;
           if k>=8
               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,sigma);
               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
                   [errsate,num_noproblem,errorsys_flag]=judge_errsate(k_gps,k_galileo,k_beidou,H,y,num);
                   if   num(i)==errsate
                       bnum=bnum+1;                  
                   end    
               end
           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