[RTKLIB]模糊度固定相关问题(二)

404是NotFound呀

于 2023-08-08 17:15:28 发布

阅读量855

点赞数 5

分类专栏： RTKLIB 文章标签： RTKLIB

本文链接：https://blog.csdn.net/qq_36142248/article/details/132166765

版权

RTKLIB 专栏收录该内容

2 篇文章 1 订阅

订阅专栏

文章目录

一、固定模糊度的前置工作

在上一篇文章中，介绍了RTKLIB中manage_amb_LAMBDA()函数，并详细介绍了其操作方式和工作方法。可见[RTKLIB]模糊度固定相关问题(一)
本篇文章中，我们针对其模糊度固定函数resamb_LAMBDA()、ddidx()做详细的分析，进一步的探索模糊度固定中有意思的部分。

一、固定模糊度的前置工作

1. 做好固定模糊度的准备

在前一篇文中我们说到，为了提高浮点模糊度的固定率，进行了部分模糊度*、延迟上星的操作。但究其根本，还没有真正接触到模糊度固定的底层，并没有创建模糊度双差数组，也没有去建立模糊度求解的矩阵方程，更还没尝试通过整数最小二乘方法去固定模糊度。
模糊度固定是一个比较复杂和繁琐的过程，前期会有许多的准备过程。从状态量中提取我们需要的状态，通过方差-协方差阵计算我们需要的Q矩阵。然后通过LAMBDA方法去求解固定后的状态向量，并通过一定的方法转换到单差模糊度，修正已有状态量。… 上述过程听着就挺复杂的，我们可以通过函数仔细看看做了哪些工作，从中理解设计和求解的思想。
附上源代码：

/* resolve integer ambiguity by LAMBDA ---------------------------------------*/
static int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa,int gps,int glo,int sbs)
{
    prcopt_t *opt=&rtk->opt;
    int i,j,nb,nb1,info,nx=rtk->nx,na=rtk->na;
    double *DP,*y,*b,*db,*Qb,*Qab,*QQ,s[2];
    int *ix;
    double var=0,coeff[3];
    double QQb[MAXSAT];

    trace(3,"resamb_LAMBDA : nx=%d\n",nx);

    rtk->sol.ratio=0.0;
    rtk->nb_ar=0;

    if (rtk->opt.mode<=PMODE_DGPS||rtk->opt.modear==ARMODE_OFF||
        rtk->opt.thresar[0]<1.0) {
        return 0;
    }
    /* skip AR if position variance too high to avoid false fix */
    for (i=0;i<3;i++) var+=rtk->P[i+i*rtk->nx];
    var=var/3.0; /* maintain compatibility with previous code */
    trace(3,"posvar=%.6f\n",var);
    if (var>rtk->opt.thresar[1]) {
        errmsg(rtk,"position variance too large:  %.4f\n",var);
        return 0;
    }
    /* Create index of single to double-difference transformation matrix (D')
          used to translate phase biases to double difference */
    ix=imat(nx,2);
    if ((nb=ddidx(rtk,ix,gps,glo,sbs))<(rtk->opt.minfixsats-1)) {  /* nb is sat pairs */
        errmsg(rtk,"not enough valid double-differences\n");
        free(ix);
        return -1; /* flag abort */
    }
    rtk->nb_ar=nb;
    /* nx=# of float states, na=# of fixed states, nb=# of double-diff phase biases */
    y=mat(nb,1); DP=mat(nb,nx-na); b=mat(nb,2); db=mat(nb,1); Qb=mat(nb,nb);
    Qab=mat(na,nb); QQ=mat(na,nb);


    /* phase-bias covariance (Qb) and real-parameters to bias covariance (Qab) */
    /* y=D*xc, Qb=D*Qc*D', Qab=Qac*D' */
    for (i=0;i<nb;i++) {
        y[i]=rtk->x[ix[i*2]]-rtk->x[ix[i*2+1]];
    }
    for (j=0;j<nx-na;j++) for (i=0;i<nb;i++) {
        DP[i+j*nb]=rtk->P[ix[i*2]+(na+j)*nx]-rtk->P[ix[i*2+1]+(na+j)*nx];
    }
    for (j=0;j<nb;j++) for (i=0;i<nb;i++) {
        Qb[i+j*nb]=DP[i+(ix[j*2]-na)*nb]-DP[i+(ix[j*2+1]-na)*nb];
    }
    for (j=0;j<nb;j++) for (i=0;i<na;i++) {
        Qab[i+j*na]=rtk->P[i+ix[j*2]*nx]-rtk->P[i+ix[j*2+1]*nx];
    }
    for (i=0;i<nb;i++) QQb[i]=1000*Qb[i+i*nb];

    trace(3,"N(0)=     "); tracemat(3,y,1,nb,7,2);
    trace(3,"Qb*1000=  "); tracemat(3,QQb,1,nb,7,4);

    /* lambda/mlambda integer least-square estimation */
    /* return best integer solutions */
    /* b are best integer solutions, s are residuals */
    if (!(info=lambda(nb,2,y,Qb,b,s))) {
        trace(3,"N(1)=     "); tracemat(3,b   ,1,nb,7,2);
        trace(3,"N(2)=     "); tracemat(3,b+nb,1,nb,7,2);

        rtk->sol.ratio=s[0]>0?(float)(s[1]/s[0]):0.0f;
        if (rtk->sol.ratio>999.9) rtk->sol.ratio=999.9f;

        /* adjust AR ratio based on # of sats, unless minAR==maxAR */
        if (opt->thresar[5]!=opt->thresar[6]) {
            nb1=nb<50?nb:50; /* poly only fitted for upto 50 sat pairs */
            /* generate poly coeffs based on nominal AR ratio */
            for ((i=0);i<3;i++) {
                 coeff[i] = ar_poly_coeffs[i][0];
                 for ((j=1);j<5;j++)
                    coeff[i] = coeff[i]*opt->thresar[0]+ar_poly_coeffs[i][j];
            }
            /* generate adjusted AR ratio based on # of sat pairs */
            rtk->sol.thres = coeff[0];
            for (i=1;i<3;i++) {
                rtk->sol.thres = rtk->sol.thres*1/(nb1+1)+coeff[i];
            }
            rtk->sol.thres = MIN(MAX(rtk->sol.thres,opt->thresar[5]),opt->thresar[6]);
        } else
            rtk->sol.thres=(float)opt->thresar[0];
        /* validation by popular ratio-test of residuals*/
        if (s[0]<=0.0||s[1]/s[0]>=rtk->sol.thres) {
            
            /* init non phase-bias states and covariances with float solution values */
            /* transform float to fixed solution (xa=xa-Qab*Qb\(b0-b)) */
            for (i=0;i<na;i++) {
                rtk->xa[i]=rtk->x[i];
                for (j=0;j<na;j++) rtk->Pa[i+j*na]=rtk->P[i+j*nx];
            }
            /* y = differences between float and fixed dd phase-biases
               bias = fixed dd phase-biases   */
            for (i=0;i<nb;i++) {
                bias[i]=b[i];
                y[i]-=b[i];
            }
            /* adjust non phase-bias states and covariances using fixed solution values */
            if (!matinv(Qb,nb)) {  /* returns 0 if inverse successful */
                /* rtk->xa = rtk->x-Qab*Qb^-1*(b0-b) */
                matmul("NN",nb,1,nb, 1.0,Qb ,y,0.0,db); /* db = Qb^-1*(b0-b) */
                matmul("NN",na,1,nb,-1.0,Qab,db,1.0,rtk->xa); /* rtk->xa = rtk->x-Qab*db */
                
                /* rtk->Pa=rtk->P-Qab*Qb^-1*Qab') */
                /* covariance of fixed solution (Qa=Qa-Qab*Qb^-1*Qab') */
                matmul("NN",na,nb,nb, 1.0,Qab,Qb ,0.0,QQ);  /* QQ = Qab*Qb^-1 */
                matmul("NT",na,na,nb,-1.0,QQ ,Qab,1.0,rtk->Pa); /* rtk->Pa = rtk->P-QQ*Qab' */
                
                trace(3,"resamb : validation ok (nb=%d ratio=%.2f thresh=%.2f s=%.2f/%.2f)\n",
                      nb,s[0]==0.0?0.0:s[1]/s[0],rtk->sol.thres,s[0],s[1]);
                
                /* translate double diff fixed phase-bias values to single diff 
                fix phase-bias values, result in xa */
                restamb(rtk,bias,nb,xa);
            }
            else nb=0;
        }
        else { /* validation failed */
            errmsg(rtk,"ambiguity validation failed (nb=%d ratio=%.2f thresh=%.2f s=%.2f/%.2f)\n",
                   nb,s[1]/s[0],rtk->sol.thres,s[0],s[1]);
            nb=0;
        }
    }
    else {
        errmsg(rtk,"lambda error (info=%d)\n",info);
        nb=0;
    }
    free(ix);
    free(y); free(DP); free(b); free(db); free(Qb); free(Qab); free(QQ);
    
    return nb; /* number of ambiguities */
}

我们来拆解步骤，并给出详细的解释。

准入条件判断，根据定为模式、位置方差判断当前是否适合进入模糊度固定。ps:可以想想如果位置方差很大，为什么不适合进入模糊度固定呢？
申请了一个im=(nx,2)的空间，用于保存构建双差浮点模糊度的索引值。之后通过ddidx()函数构建了双差浮点模糊度矩阵，在此处做了一个模糊度个数的判断，判断是否小于设定的最小固定卫星。ps:若出现三颗三频的卫星，此处的nb=(3-1)*3=6，确实满足最小卫星>4的条件，但是此处的固定是否有意义呢？我觉得这是一个版本迭代的BUG
根据公式计算双差浮点模糊度y、双差协方差矩阵Qb、双差实参协方差矩阵Qab。ps:如果从理论角度来说，在此处我们需要求四个矩阵，分别为Qa、Qab、Qba、Qb，但实质上我们只用到Qb、Qab就能完成整个计算流程。
输出固定前的浮点双差模糊度N(0)和模糊度的方差Qb*1000，在调试模糊度固定的时候，这两个值是非常重要的两个值。
使用lambda()函数进行浮点双差模糊度的固定，如果解算失败就释放申请的内存；若解算成功，则进行ratio test。ps:这里的解算成功不是指成功求解整数模糊度，而是指成功进行lambda()解算
输出计算过后的整数双差模糊度的最优解和次优解，如果设置了动态AR阈值，还需要根据卫星数计算模糊度阈值。
进行ratio test判断模糊度最优解与次优解的残差是否合规，模糊度是否可以被采纳。
将计算的整数双差模糊度通过计算更新到固定解位置和相应的方差-协方差解算矩阵上，并通过restamb()函数将整数双差模糊度转换到对应的浮点单差模糊度。ps:保存在xa变量中了,此处的restamb()与ddidx()互为反映射关系
返回模糊度个数nb。

2. 建立双差模糊度

如果要进行模糊度解算，肯定需要在系统中简历双差模糊度方程，如何做双差?选择怎样的卫星作为第一颗卫星?其中的奥秘都集中在ddidx()函数中。
附上源代码：

/* index for single to double-difference transformation matrix (D') --------------------*/
static int ddidx(rtk_t *rtk, int *ix, int gps, int glo, int sbs)
{
    int i,j,k,m,f,n,nb=0,na=rtk->na,nf=NF(&rtk->opt),nofix;
    double fix[MAXSAT],ref[MAXSAT];
    
    trace(3,"ddmat: gps=%d/%d glo=%d/%d sbs=%d\n",gps,rtk->opt.gpsmodear,glo,rtk->opt.glomodear,sbs);
    
    /* clear fix flag for all sats (1=float, 2=fix) */
    for (i=0;i<MAXSAT;i++) for (j=0;j<NFREQ;j++) {
        rtk->ssat[i].fix[j]=0;
    }
    for (m=0;m<6;m++) { /* m=0:GPS/SBS,1:GLO,2:GAL,3:BDS,4:QZS,5:IRN */
        
        /* skip if ambiguity resolution turned off for this sys */
        nofix=(m==0&&gps==0)||(m==1&&glo==0)||(m==3&&rtk->opt.bdsmodear==0);        
        
        /* step through freqs */ 
        for (f=0,k=na;f<nf;f++,k+=MAXSAT) {
            
            /* look for first valid sat (i=state index, i-k=sat index) */
            for (i=k;i<k+MAXSAT;i++) {
                /* skip if sat not active */
                if (rtk->x[i]==0.0||!test_sys(rtk->ssat[i-k].sys,m)||
                    !rtk->ssat[i-k].vsat[f]) {
                    continue;
                }
                /* set sat to use for fixing ambiguity if meets criteria */
                if (rtk->ssat[i-k].lock[f]>=0&&!(rtk->ssat[i-k].slip[f]&2)&&
                    rtk->ssat[i-k].azel[1]>=rtk->opt.elmaskar&&!nofix) {
                    rtk->ssat[i-k].fix[f]=2; /* fix */
                    break;/* break out of loop if find good sat */
                }
                /* else don't use this sat for fixing ambiguity */
                else rtk->ssat[i-k].fix[f]=1;
            }
            if (rtk->ssat[i-k].fix[f]!=2) continue;  /* no good sat found */
            /* step through all sats (j=state index, j-k=sat index, i-k=first good sat) */
            for (n=0,j=k;j<k+MAXSAT;j++) {
                if (i==j||rtk->x[j]==0.0||!test_sys(rtk->ssat[j-k].sys,m)||
                    !rtk->ssat[j-k].vsat[f]) {
                    continue;
                }
                if (sbs==0 && satsys(j-k+1,NULL)==SYS_SBS) continue; 
                if (rtk->ssat[j-k].lock[f]>=0&&!(rtk->ssat[j-k].slip[f]&2)&&
                    rtk->ssat[j-k].vsat[f]&&
                    rtk->ssat[j-k].azel[1]>=rtk->opt.elmaskar&&!nofix) {
                    /* set D coeffs to subtract sat j from sat i */
                    ix[nb*2  ]=i; /* state index of ref bias */
                    ix[nb*2+1]=j; /* state index of target bias */
                    /* inc # of sats used for fix */
                    ref[nb]=i-k+1;
                    fix[nb++]=j-k+1;
                    rtk->ssat[j-k].fix[f]=2; /* fix */
                    n++; /* count # of sat pairs for this freq/constellation */
                }
                /* else don't use this sat for fixing ambiguity */
                else rtk->ssat[j-k].fix[f]=1;
            }
            /* don't use ref sat if no sat pairs */
            if (n==0) rtk->ssat[i-k].fix[f]=1;
        }
    }

    if (nb>0) {
        trace(3,"refSats=");tracemat(3,ref,1,nb,7,0);
        trace(3,"fixSats=");tracemat(3,fix,1,nb,7,0);
    }
    return nb;
}

我们来拆解步骤，并给出详细的解释。

对所有的卫星rtk->ssat[i].fix[j]结构体进行清空，进行初始化。
对每个星座的每个频率进行双重循环，确定每个星座和每个频率中的参考卫星第一颗符合规定的卫星，对符合条件的卫星设定rtk->ssat[i-k].fix[f]=2; /* fix */，对有载波观测值但不符合条件的卫星设定rtk->ssat[i-k].fix[f]=1;完成卫星清洗工作，选出参考卫星。ps:本段存在很大的问题，在后面详细叙述。
上一个循环已经确定了参考卫星，在这个循环中确定同星座同频率的非参考卫星，与参考卫星组pairs，并记录对应的索引号、卫星号。
返回双差卫星(模糊度)个数。

3. 问题与总结

通过上述的两个函数，我们就完成了除整数最小二乘(LAMBDA)外的所有模糊度固定的流程。表面上似乎没问题，但通过仔细的解析和细细回味，可以发现很多不合理的地方。

选择参考星的时候，并没有选择高度角最高的卫星；ps:这个问题其实困扰我一段时间了，似乎如果只更新xa、xp，选择高度角最高的卫星和系统内的第一颗卫星并没有很大的差别
双差模糊度nb和参与计算的卫星ns之争。在上述的代码中，使用nb判断这次解算是否有效，这似乎是一个随着版本迭代的bug。在单频状态下，nb确实可以代表参与解算的卫星数，但随着频率的增加，多频模糊度固定时，nb的数量可能会很大，但是真实参与解算和固定的卫星ns可能不满足固定的最小卫星数，此时固定的解算结果有意义吗？ps:通过固定的模糊度修正的nx实际上是秩亏的，个人认为是不正确的。
restamb()与ddidx()互为映射关系，但其中的判断条件太简单了，且需要丰富判断条件。