8.4 正直性检测

8.4.1 raim_fde()：接收机自主完好性检测与故障检测排除

图8.4-1 raim_fde() 函数调用关系

通过每次排除一颗卫星计算，选取残差最小的一次作为最终解算结果，此时对应的卫星就是故障卫星。

1. 参数列表

/* args */
const obsd_t   *obs     I    OBS观测数据
int             n       I    观测数据的数量
const double   *rs      I    卫星位置和速度，长度为6*n，{x,y,z,vx,vy,vz}(ecef)(m,m/s)
const double   *dts     I    卫星钟差，长度为2*n， {bias,drift} (s|s/s)
const double   *vare    I    卫星位置和钟差的协方差 (m^2)
const int      *svh     I    卫星健康标志 (-1:correction not available)
const nav_t    *nav     I    导航数据
const prcopt_t *opt     I    处理过程选项
sol_t          *sol     IO   解算结果
double         *azel    IO   方位角和俯仰角 (rad)
int            *vsat    IO   表征卫星在定位时是否有效
double         *resp    IO   观测卫星的伪距残差，(P-(r+c*dtr-c*dts+I+T)) (1*n)
char           *msg     O    错误信息
/* return */
int             status  -    1:ok,0:error

2. 执行流程

• 遍历所有卫星，每次舍弃一颗卫星，然后利用余下卫星进行定位解算。
• 累加使用当前卫星实现定位后的伪距残差平方和与可用卫星数目，如果 nvsat<5 ，则说明当前卫星数目过少，无法进行 RAIM_FDE 操作。
• 计算伪距残差平方和的标准平均值，如果大于 rms，则说明当前定位结果更合理，将 stat 置为 1，重新更新 sol、azel、vsat(当前被舍弃的卫星，此值置为0)、resp 等值，并将当前的 rms_e 更新到 rms 中（用以下一轮循环的判断）。
• 继续弃用下一颗卫星，重复上述操作。总而言之，在弃用一颗卫星条件下，选取伪距残差标准平均值最小的组合作为最终的结果输出。
• 如果 stat不为 0，则说明在弃用卫星的前提下有更好的解出现，输出信息，指出弃用了哪科卫星。

3. 注意事项

• 源码中有很多关于 i、j、k 的循环。其中，i 表示最外面的大循环，每次将将第 i 颗卫星舍弃不用，这是通过 if (j==i) continue 实现的；j 表示剩余使用的卫星的循环，每次进行相应数据的赋值；k 表示参与定位的卫星的循环，与 j 一起使用。

4. 源码注释

点击查看代码

static int raim_fde(const obsd_t *obs, int n, const double *rs,
                    const double *dts, const double *vare, const int *svh,
                    const nav_t *nav, const prcopt_t *opt, sol_t *sol,
                    double *azel, int *vsat, double *resp, char *msg)
{
    obsd_t *obs_e;
    sol_t sol_e={{0}};
    char tstr[32],name[16],msg_e[128];
    double *rs_e,*dts_e,*vare_e,*azel_e,*resp_e,rms_e,rms=100.0;
    int i,j,k,nvsat,stat=0,*svh_e,*vsat_e,sat=0;
    
    trace(3,"raim_fde: %s n=%2d\n",time_str(obs[0].time,0),n);
    
    if (!(obs_e=(obsd_t *)malloc(sizeof(obsd_t)*n))) return 0;
    rs_e = mat(6,n); dts_e = mat(2,n); vare_e=mat(1,n); azel_e=zeros(2,n);
    svh_e=imat(1,n); vsat_e=imat(1,n); resp_e=mat(1,n); 

    // 遍历所有卫星，每次舍弃一颗卫星，然后利用余下卫星进行定位解算
    for (i=0;i<n;i++) {
        /* satellite exclution */
        for (j=k=0;j<n;j++) {
            if (j==i) continue;
            obs_e[k]=obs[j];
            matcpy(rs_e +6*k,rs +6*j,6,1);
            matcpy(dts_e+2*k,dts+2*j,2,1);
            vare_e[k]=vare[j];
            svh_e[k++]=svh[j];
        }
        // 调用 estpos 函数计算剩余卫星的定位结果
        /* estimate receiver position without a satellite */
        if (!estpos(obs_e,n-1,rs_e,dts_e,vare_e,svh_e,nav,opt,&sol_e,azel_e,
                    vsat_e,resp_e,msg_e)) {
            trace(3,"raim_fde: exsat=%2d (%s)\n",obs[i].sat,msg);
            continue;
        }
        // 累加计算当前卫星实现定位后的伪距残差平方和与可用卫星数目
        for (j=nvsat=0,rms_e=0.0;j<n-1;j++) {
            if (!vsat_e[j]) continue;
            rms_e+=SQR(resp_e[j]);
            nvsat++;
        }
        if (nvsat<5) { // 如果 nvsat<5，则说明当前卫星数目过少，无法进行 RAIM_FDE 操作
            trace(3,"raim_fde: exsat=%2d lack of satellites nvsat=%2d\n",
                  obs[i].sat,nvsat);
            continue;
        }
        rms_e=sqrt(rms_e/nvsat); // 计算伪距残差平方和的标准差rms_e
       
        trace(3,"raim_fde: exsat=%2d rms=%8.3f\n",obs[i].sat,rms_e);
       
        if (rms_e>rms) continue; // 如果伪距残差平方和的标准差rms_e>rms,继续下一次循环
       
        // 如果小于 rms，则说明当前定位结果更合理，将 stat 置为 1，
        // 重新更新 sol、azel、vsat(当前被舍弃的卫星，此值置为0)、resp 等值，并将当前的 rms_e 更新到 rms 中。
        /* save result */
        for (j=k=0;j<n;j++) {
            if (j==i) continue;
            matcpy(azel+2*j,azel_e+2*k,2,1);
            vsat[j]=vsat_e[k];
            resp[j]=resp_e[k++];
        }
        stat=1;
        *sol=sol_e;
        sat=obs[i].sat;
        rms=rms_e;
        vsat[i]=0;
        strcpy(msg,msg_e);
    }
    // 如果 stat不为 0，则说明在弃用卫星的前提下有更好的解出现，输出信息，指出弃用了哪颗卫星。
    if (stat) { 
        time2str(obs[0].time,tstr,2); satno2id(sat,name);
        trace(2,"%s: %s excluded by raim\n",tstr+11,name);
    }
    free(obs_e);
    free(rs_e ); free(dts_e ); free(vare_e); free(azel_e);
    free(svh_e); free(vsat_e); free(resp_e);
    return stat;
}