【manage_amb_LAMBDA】模糊度固定函数解析

模糊度固定函数_manage_amb_LAMBDA解析


文章目录

  • 模糊度固定函数_manage_amb_LAMBDA解析
  • 一、函数功能描述
  • 二、函数功能分析
    • 1.变量解析
    • 2.逐块分析
  • 三、总结


一、函数功能描述

rtkpos.c:函数注释如下,
/* resolve integer ambiguity by LAMBDA using partial fix techniques and multiple attempts -----------------------*/
从上述注释可知,使用部分固定技术和多次尝试两种方法进行LAMBDA算法实现。
那么,本文重点对这两种技术的实现进行讲解。


二、函数功能分析

推荐大家阅读源码的时候,使用Source Insight软件。

1.变量解析

先贴出来该函数的局部变量,

    int i,f,lockc[NFREQ],ar=0,excflag=0,arsats[MAXOBS]={0};
    int gps1=-1,glo1=-1,sbas1=-1,gps2,glo2,sbas2,nb,rerun,dly;
    float ratio1,var=0,posvar=0;

(1)上述变量ar,ar是一个计数器,表示上次进行AR(模糊度解算)可用卫星数量,在以下代码块中使用(出现两次)。

    /* if no fix on previous sample and enough sats, exclude next sat in list */
    if (rtk->sol.prev_ratio2<rtk->sol.thres&&rtk->nb_ar>=rtk->opt.mindropsats) {
        /* find and count sats used last time for AR */
        for (f=0;f<nf;f++) for (i=0;i<ns;i++) 
            if (rtk->ssat[sat[i]-1].vsat[f] && rtk->ssat[sat[i]-1].lock[f]>=0 && rtk->ssat[sat[i]-1].azel[1]>=rtk->opt.elmin) {
                arsats[ar++]=i;
            }
        if (rtk->excsat<ar) {
            i=sat[arsats[rtk->excsat]];
            for (f=0;f<nf;f++) {
                lockc[f]=rtk->ssat[i-1].lock[f];  /* save lock count */
                /* remove sat from AR long enough to enable hold if stays fixed */
                rtk->ssat[i-1].lock[f]=-rtk->nb_ar;
            }
            trace(3,"AR: exclude sat %d\n",i);
            excflag=1;
        } else rtk->excsat=0; /* exclude none and reset to beginning of list */
    }

(2)上述变量excflag,从变量定义可知,是排除卫星的标志,也出现两次,除了在(1)中出现,还在以下代码中出现。

    /* restore excluded sat if still no fix or significant increase in ar ratio */
    if (excflag && (rtk->sol.ratio<rtk->sol.thres) && (rtk->sol.ratio<(1.5*rtk->sol.prev_ratio2))) {
        i=sat[arsats[rtk->excsat++]];
        for (f=0;f<nf;f++) rtk->ssat[i-1].lock[f]=lockc[f];
        trace(3,"AR: restore sat %d\n",i);
    }

(3)上述变量arsats,表示前一历元的共视卫星数的索引信息,从索引信息和共视卫星sat可知卫星的svid,变量出现三次,分别在(1)和(2)的代码块中。


(4)上述变量nb,表示当前历元使用的模糊度数量,与之对比的是nb_ar,表示前一历元使用的模糊度数量。


(5)上述变量rerun,是重新运行的标志,总是把它和return看混,其实变量定义成re_run也好点,不易混淆。


(6)上述变量dly,是控制出问题的卫星过几个历元再参与模糊度的一个变量,和stagger是一个意思吧,用于调节卫星的lock,进而影响是否参与模糊度解算。


(7)上述变量posvar,是位置方差,即state状态(xyz)rtk->x[0],rtk->x[1],rtk->x[2]对应的rtk->P[0],rtk->P[1+rtk->nx],rtk->P[2+2*rtk->nx]的均值。代码如下,

    /* calc position variance, will skip AR if too high to avoid false fix */
 for (i=0;i<3;i++) posvar+=rtk->P[i+i*rtk->nx];
 posvar/=3.0; /* maintain compatibility with previous code */

 trace(3,"posvar=%.6f\n",posvar);

2.逐块分析

    /* skip AR if don't meet criteria */
    if (rtk->opt.mode<=PMODE_DGPS||rtk->opt.modear==ARMODE_OFF||
        rtk->opt.thresar[0]<1.0||posvar>rtk->opt.thresar[1]) {
        trace(3,"Skip AR\n");
        rtk->sol.ratio=0.0;
        rtk->sol.prev_ratio1=rtk->sol.prev_ratio2=0.0;
        rtk->nb_ar=0;
        return 0;
    }

(1)上述代码是对进入AR的准则做一些判断,如果模式是DGPS或SINGLE,AR模式选择OFF,配置opt中的ar阈值小于1.0,位置方差大于配置opt中的方差阈值,出现四种其中之一的话,就会跳过AR,我们要关注的是次数被重置的成员是哪些。


    /* if no fix on previous sample and enough sats, exclude next sat in list */
    if (rtk->sol.prev_ratio2<rtk->sol.thres&&rtk->nb_ar>=rtk->opt.mindropsats) {
        /* find and count sats used last time for AR */
        for (f=0;f<nf;f++) for (i=0;i<ns;i++) 
            if (rtk->ssat[sat[i]-1].vsat[f] && rtk->ssat[sat[i]-1].lock[f]>=0 && rtk->ssat[sat[i]-1].azel[1]>=rtk->opt.elmin) {
                arsats[ar++]=i;
            }
        if (rtk->excsat<ar) {
            i=sat[arsats[rtk->excsat]];
            for (f=0;f<nf;f++) {
                lockc[f]=rtk->ssat[i-1].lock[f];  /* save lock count */
                /* remove sat from AR long enough to enable hold if stays fixed */
                rtk->ssat[i-1].lock[f]=-rtk->nb_ar;
            }
            trace(3,"AR: exclude sat %d\n",i);
            excflag=1;
        } else rtk->excsat=0; /* exclude none and reset to beginning of list */
    }

(2)上述代码是对前一历元没有fix解,但是参与模糊度的卫星数又足够多,这时候就要进行剔星的操作。具体实现是把卫星的索引信息放在arsats数组中,根据要剔除的索引信息(rtk->excsat)的卫星进行remove操作,移除卫星的操作通过卫星载波锁定数lock来控制,同时对excflag置为1。在rtk结构体的成员excsat中有说明,其实我觉得改成idx_excsat更好理解这个变量。

 int excsat;         /* index of next satellite to be excluded for partial ambiguity resolution */

   /* first attempt to resolve ambiguities */
   nb=resamb_LAMBDA(rtk,bias,xa,gps1,glo1,sbas1);
   ratio1=rtk->sol.ratio;

(3)进行第一次LAMBDA尝试,将ratio值赋给中间变量ratio1。


  /* reject bad satellites if AR filtering enabled */
   if (rtk->opt.arfilter) {
       rerun=0;
       /* if results are much poorer than previous epoch or dropped below ar ratio thresh, remove new sats */
       if (nb>=0 && rtk->sol.prev_ratio2>=rtk->sol.thres && ((rtk->sol.ratio<rtk->sol.thres) ||
           (rtk->sol.ratio<rtk->opt.thresar[0]*1.1 && rtk->sol.ratio<rtk->sol.prev_ratio1/2.0))) {
           trace(3,"low ratio: check for new sat\n");
           dly=2;
           for (i=0;i<ns;i++) for (f=0;f<nf;f++) {
               if (rtk->ssat[sat[i]-1].fix[f]!=2) continue;
               /* check for new sats */
               if (rtk->ssat[sat[i]-1].lock[f]==0) {
                   trace(3,"remove sat %d:%d lock=%d\n",sat[i],f,rtk->ssat[sat[i]-1].lock[f]);
                   rtk->ssat[sat[i]-1].lock[f]=-rtk->opt.minlock-dly;  /* delay use of this sat with stagger */
                   dly+=2;  /* stagger next try of new sats */
                   rerun=1;
               }
           }
       }
       /* rerun if filter removed any sats */
       if (rerun) {
           trace(3,"rerun AR with new sat removed\n");
           /* try again with new sats removed */
           nb=resamb_LAMBDA(rtk,bias,xa,gps1,glo1,sbas1);
       }
   }

(4)上述代码表示如果ar滤波打开的话,则进行剔星的操作,注意注释中的reject bad satellites ,表明剔除的卫星可能不止一颗。从ratio的值作为条件进行判断是否进入if语句,如果进入的话,则对共视星进行for循环判断,先对fix进行判断,fix=1表示float,fix=2表示fix,fix=3表示hold,lock=0表示这颗卫星是新上星,这个是根据模糊度解算去判断的,可以看看resamb_LAMBDA中ddidx的实现。对新上星的lock进行赋值,如果新上星不止一颗,那么要通过dly进行错开,间隔为2个历元,后处理时可以通过打印ddres中的打印信息分析,如果该卫星一直存在问题,会发现lock是在-2和-1来回变化。

               trace(3,"sat=%3d-%3d %s%d v=%13.3f R=%9.6f %9.6f icb=%9.3f lock=%5d x=%9.3f\n",sat[i],
                       sat[j],code?"P":"L",frq+1,v[nv],Ri[nv],Rj[nv],icb,
                       rtk->ssat[sat[j]-1].lock[frq],rtk->x[IB(sat[j],frq,&rtk->opt)]);

如果rerun标志位被置起来。那么就要再次进行LAMBDA尝试(第二次)。


   /* if fix-and-hold gloarmode enabled, re-run AR with final gps/glo settings if differ from above */
   if ((rtk->opt.navsys&SYS_GLO) && rtk->opt.glomodear==GLO_ARMODE_FIXHOLD && rtk->sol.ratio<rtk->sol.thres) {
       glo2=sbas2=0;
       /* turn off gpsmode if not enabled and got good fix (used for debug and eval only) */
       gps2=rtk->opt.gpsmodear==0&&rtk->sol.ratio>=rtk->sol.thres?0:1;

       /* if modes changed since initial AR run or haven't run yet,re-run with new modes */
       if (glo1!=glo2||gps1!=gps2)
           nb=resamb_LAMBDA(rtk,bias,xa,gps2,glo2,sbas2);
   }

(5)上述代码表示如果选择的卫星系统中有glonass并且模糊度解算模式时HLOD,但是ratio值小于设置的阈值,那么再次进行LAMBDA尝试(第三次)。


   /* restore excluded sat if still no fix or significant increase in ar ratio */
   if (excflag && (rtk->sol.ratio<rtk->sol.thres) && (rtk->sol.ratio<(1.5*rtk->sol.prev_ratio2))) {
       i=sat[arsats[rtk->excsat++]];
       for (f=0;f<nf;f++) rtk->ssat[i-1].lock[f]=lockc[f];
       trace(3,"AR: restore sat %d\n",i);
   }

(6)上述代码是针对第一次LAMBDA尝试没有成功提高ratio时,对(2)中的卫星恢复如初,把lockc保存的剔除卫星前的lock计数重新赋值过来,ratio值没有提升,说明剔除的卫星没有啥问题,有问题的卫星还没有找到,没找到咋办呢,继续下一个索引的卫星呗,即rtk->excsat++,真的建议把这个成员excsat改成idx_excsat。


三、总结

从上述分析可以,该函数从两方面来入手提高模糊度的固定率,最精髓的是这处代码。

   /* reject bad satellites if AR filtering enabled */
   if (rtk->opt.arfilter) {
       rerun=0;
       /* if results are much poorer than previous epoch or dropped below ar ratio thresh, remove new sats */
       if (nb>=0 && rtk->sol.prev_ratio2>=rtk->sol.thres && ((rtk->sol.ratio<rtk->sol.thres) ||
           (rtk->sol.ratio<rtk->opt.thresar[0]*1.1 && rtk->sol.ratio<rtk->sol.prev_ratio1/2.0))) {
           trace(3,"low ratio: check for new sat\n");
           dly=2;
           for (i=0;i<ns;i++) for (f=0;f<nf;f++) {
               if (rtk->ssat[sat[i]-1].fix[f]!=2) continue;
               /* check for new sats */
               if (rtk->ssat[sat[i]-1].lock[f]==0) {
                   trace(3,"remove sat %d:%d lock=%d\n",sat[i],f,rtk->ssat[sat[i]-1].lock[f]);
                   rtk->ssat[sat[i]-1].lock[f]=-rtk->opt.minlock-dly;  /* delay use of this sat with stagger */
                   dly+=2;  /* stagger next try of new sats */
                   rerun=1;
               }
           }
       }
       /* rerun if filter removed any sats */
       if (rerun) {
           trace(3,"rerun AR with new sat removed\n");
           /* try again with new sats removed */
           nb=resamb_LAMBDA(rtk,bias,xa,gps1,glo1,sbas1);
       }
   }

调试的时候,多关注下lock成员变量。

你可能感兴趣的