RTKLIB有关整周模糊度的说明（二）

1、RTKLIB中用的整周模糊度固定方法

 

/* resolve integer ambiguity by LAMBDA ---------------------------------------*/
/*
    通过LAMBDA算法求解整周模糊度
    IO  rtk_t *rtk:      rtk solution structure
    I   double *bias     利用lambda算法计算得到的双差整周模糊度
    IO  double *xa       fixed states（在注意事项中进行了更为详细的解释）
*/
static int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa)
{
    prcopt_t *opt=&rtk->opt;
    int i,j,ny,nb,info,nx=rtk->nx,na=rtk->na;
    double *D,*DP,*y,*Qy,*b,*db,*Qb,*Qab,*QQ,s[2];
    
    trace(3,"resamb_LAMBDA : nx=%d\n",nx);
    
    rtk->sol.ratio=0.0;
    
    /*
        首先检查配置中所设置的AR阈值（LAMBDA算法最优解和次优解的比值，通常取3.0），
        如果该阈值小于1，则返回0。
        检查卡尔曼滤波中位置状态量的协方差阵，？？？
        如果位置方差超过所设定的方差阈值，则返回0，
        避免由于方差过大造成false fix。
    */
    if (rtk->opt.mode<=PMODE_DGPS||rtk->opt.modear==ARMODE_OFF||
        rtk->opt.thresar[0]<1.0) {
        return 0;
    }
    /* single to double-difference transformation matrix (D') 
        调用ddmat函数，创建将卡尔曼状态量从单差转到双差的转换矩阵D’。
        主要是将单差相位偏移状态量转换为双差相位偏移，
        这里的D’阵实际就是RTKlib manual 165页中的G阵。
    */
    D=zeros(nx,nx);
    if ((nb=ddmat(rtk,D))<=0) {
        errmsg(rtk,"no valid double-difference\n");
        free(D);
        return 0;
    }

    /*
        定义几个变量和矩阵，其中ny=na+nb，
        na实际就是之前卡尔曼滤波中除了单差相位偏移之外的所有状态量个数（例如：位置+速度+加速度+电离层+对流层……），
        nb则是双差相位偏移的个数（即需要解算的整周模糊度个数）；
    */
    ny=na+nb; y=mat(ny,1); Qy=mat(ny,ny); DP=mat(ny,nx);
    b=mat(nb,2); db=mat(nb,1); Qb=mat(nb,nb); Qab=mat(na,nb); QQ=mat(na,nb);
    
    /* transform single to double-differenced phase-bias (y=D'*x, Qy=D'*P*D) 
       根据RTKlib manual 165页公式（E.7.15）、（E.7.16）计算双差状态量和双差协方差阵。
       之所以要在做LAMBDA算法前将单差转成双差，是为了去除接收机的初始相位偏移，这样就只剩下整周的模糊度。
    */
    matmul("TN",ny, 1,nx,1.0,D ,rtk->x,0.0,y );
    matmul("TN",ny,nx,nx,1.0,D ,rtk->P,0.0,DP);
    matmul("NN",ny,ny,nx,1.0,DP,D     ,0.0,Qy);
    
    /* phase-bias covariance (Qb) and real-parameters to bias covariance (Qab) 
    计算公式（E.7.16）中的Q N，Q_NR矩阵。
    */
    for (i=0;i<nb;i++) for (j=0;j<nb;j++) Qb [i+j*nb]=Qy[na+i+(na+j)*ny];
    for (i=0;i<na;i++) for (j=0;j<nb;j++) Qab[i+j*na]=Qy[   i+(na+j)*ny];
    
    trace(4,"N(0)="); tracemat(4,y+na,1,nb,10,3);
    
    /* lambda/mlambda integer least-square estimation 
       调用lambda函数计算双差整周模糊度最优解以及残差。*/
    if (!(info=lambda(nb,2,y+na,Qb,b,s))) {
        
        trace(4,"N(1)="); tracemat(4,b   ,1,nb,10,3);
        trace(4,"N(2)="); tracemat(4,b+nb,1,nb,10,3);
        
        rtk->sol.ratio=s[0]>0?(float)(s[1]/s[0]):0.0f;
        if (rtk->sol.ratio>999.9) rtk->sol.ratio=999.9f;
        
        /* validation by popular ratio-test */
        /*
            如果最优解和次优解的比值大于阈值，则利用公式（E.7.19），根据lambda算法得到的双差整周模糊度，
            计算除了单差相位偏移之外的所有状态量（例如：位置+速度+加速度+电离层+对流层……），存入rtk->xa中。
            这一步实际就是利用lambda算法得到的整数的整周模糊度对其他状态量进行修正。
        */
        if (s[0]<=0.0||s[1]/s[0]>=opt->thresar[0]) {
            
            /* 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];
            }
            for (i=0;i<nb;i++) {
                bias[i]=b[i];
                y[na+i]-=b[i];
            }
            if (!matinv(Qb,nb)) {
                matmul("NN",nb,1,nb, 1.0,Qb ,y+na,0.0,db);
                matmul("NN",na,1,nb,-1.0,Qab,db  ,1.0,rtk->xa);
                
                /* covariance of fixed solution (Qa=Qa-Qab*Qb^-1*Qab') */
                matmul("NN",na,nb,nb, 1.0,Qab,Qb ,0.0,QQ);
                matmul("NT",na,na,nb,-1.0,QQ ,Qab,1.0,rtk->Pa);
                
                trace(3,"resamb : validation ok (nb=%d ratio=%.2f s=%.2f/%.2f)\n",
                      nb,s[0]==0.0?0.0:s[1]/s[0],s[0],s[1]);
                
                /* restore single-differenced ambiguity 调用restamb函数，
                利用lambda算法计算得到的双差整周模糊度，重新计算单差相位偏移，
                并存入xa中，同时将第8步中得到的其他状态量也存入xa中。*/
                restamb(rtk,bias,nb,xa);
            }
            else nb=0;
        }
        else { /* validation failed */
            errmsg(rtk,"ambiguity validation failed (nb=%d ratio=%.2f s=%.2f/%.2f)\n",
                   nb,s[1]/s[0],s[0],s[1]);
            nb=0;
        }
    }
    else {
        errmsg(rtk,"lambda error (info=%d)\n",info);
        nb=0;
    }
    free(D); free(y); free(Qy); free(DP);
    free(b); free(db); free(Qb); free(Qab); free(QQ);
    
    return nb; /* number of ambiguities */
}

2、resamb_LAMBDA（所在文件：Rtkpos.c）

static int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa,int gps,int glo,int sbs)

功能说明：通过LAMBDA算法求解整周模糊度

参数说明：*rtk: rtk solution structure ；*bias 利用lambda算法计算得到的双差整周模糊度 ； *xa fixed states

步骤：

1. 首先检查配置中所设置的AR阈值（LAMBDA算法最优解和次优解的比值，通常取3.0），如果该阈值小于0，则返回0。
2. 检查卡尔曼滤波中位置状态量的协方差阵，如果位置方差超过所设定的方差阈值，则返回0，避免由于方差过大造成false fix。
3. 调用ddmat函数，创建将卡尔曼状态量从单差转到双差的转换矩阵D’。主要是将单差相位偏移状态量转换为双差相位偏移，这里的D’阵实际就是RTKlib manual 165页中的G阵。
4. 定义几个变量和矩阵，其中ny=na+nb，na实际就是之前卡尔曼滤波中除了单差相位偏移之外的所有状态量个数（例如：位置+速度+加速度+电离层+对流层……），nb则是双差相位偏移的个数（即需要解算的整周模糊度个数）；
5. 根据RTKlib manual 165页公式（E.7.15）、（E.7.16）计算双差状态量和双差协方差阵。之所以要在做LAMBDA算法前将单差转成双差，是为了去除接收机的初始相位偏移，这样就只剩下整周的模糊度。
6. 计算公式（E.7.16）中的Q_NQN​和Q_{NR}QNR​矩阵。
7. 调用lambda函数计算双差整周模糊度最优解以及残差。
8. 如果最优解和次优解的比值大于阈值，则利用公式（E.7.19），根据lambda算法得到的双差整周模糊度，计算除了单差相位偏移之外的所有状态量（例如：位置+速度+加速度+电离层+对流层……），存入rtk->xa中。这一步实际就是利用lambda算法得到的整数的整周模糊度对其他状态量进行修正。
9. 调用restamb函数，利用lambda算法计算得到的双差整周模糊度，重新计算单差相位偏移，并存入xa中，同时将第8步中得到的其他状态量也存入xa中。

注意：

这个函数中的变量比较多，容易混乱。rtk->x是卡尔曼滤波的状态量，rtk->xa是利用lambda算法得到的整数解修正后的、与相位偏差无关的状态量（例如：位置+速度+加速度+电离层+对流层……），rtk->P和rtk->Pa同理。xa这个变量（注意不是rtk->xa，这是两个不同的变量）则包含了两部分，一部分是rtk->xa，另一部分则是利用lambda算法得到的双差整周模糊度计算得到的单差相位偏移。nx=rtk->nx是指卡尔曼滤波状态量的个数，na=rtk->na是除了单差相位偏差之外的所有状态量个数，所以通常相位偏差状态量的索引是从na开始；nb则是双差整周模糊度的个数。

ddmat（所在文件：Rtkpos.c）

static int ddmat(rtk_t *rtk, double *D,int gps,int glo,int sbs)

功能说明：创建将卡尔曼状态量从单差转到双差的转换矩阵D’

参数说明： rtk solution structure ； *D single to double-difference transformation matrix (D')

 步骤：

1. 首先将所有卫星的fix标志都清空置0：rtk->ssat[i].fix[j]=0。
2. 将D矩阵中所有和相位偏移无关的状态量的对角线元素都置1。
3. 对所有卫星系统进行循环，对所有频率进行循环，对所有卫星进行循环，如果该卫星没有被**，则跳过。如果**了，则寻找第一颗满足要求的卫星作为参考星，找到了就break，跳出所有卫星这个循环。
4. 重新对所有卫星进行循环，检查每颗卫星是否符合要求，并根据该符合要求卫星和参考卫星的索引，对D矩阵进行赋值。

restamb（所在文件：Rtkpos.c）

static void restamb(rtk_t *rtk, const double *bias, int nb, double *xa)

功能说明：根据lambda算法计算得到的双差整周模糊度，计算单差相位偏差

参数说明： *rtk: rtk solution structure； *bias 利用lambda算法计算得到的双差整周模糊度；nb 双差整周模糊度的个数； *xa fixed states

 步骤：

1. 首先用将xa状态量的前na个元素赋值为rtk->xa[i]:for (i=0;ina;i++) xa[i]=rtk->xa[i]; 而其余元素（相位偏差状态量）则赋值为卡尔曼滤波浮点解：xa[i]=rtk->x [i];这里需要理解这几个变量的含义，具体看resamb_LAMBDA函数注意事项中的解释。
2. 对所有卫星系统进行循环，对所有频率进行循环，对所有卫星进行循环，找到该卫星相位偏移在卡尔曼滤波状态量中的索引，放到index数组中。
3. 由于参考卫星是第一个符合要求的卫星，因此index[0]即为参考星的单差相位偏移，由此根据lambda算法计算得到的双差整周模糊度，计算其他卫星的单差相位偏移。

3、RTKLIB中的lambda()函数 

/* lambda/mlambda integer least-square estimation ------------------------------
* integer least-square estimation. reduction is performed by lambda,
* and search by mlambda. 用lambda算法降相关，mlambda算法搜索
* args   : int    n      I  number of float parameters
*          int    m      I  number of fixed solutions
*          double *a     I  float parameters (n x 1)
*          double *Q     I  covariance matrix of float parameters (n x n)
*          double *F     O  fixed solutions (n x m)
*          double *s     O  sum of squared residulas of fixed solutions (1 x m)
* return : status (0:ok,other:error)
* notes  : matrix stored by column-major order (fortran convension)
*-----------------------------------------------------------------------------*/
extern int lambda(int n, int m, const double *a, const double *Q, double *F,
                  double *s)
{
    int info;
    double *L,*D,*Z,*z,*E;
    
    if (n<=0||m<=0) return -1;
    L=zeros(n,n); D=mat(n,1); Z=eye(n); z=mat(n,1); E=mat(n,m);
    
    /* LD factorization 对双差模糊度方差-协方差阵Q进行LDLT分解 */
    if (!(info=LD(n,Q,L,D))) {
        
        /* lambda reduction lambda降相关 */
        reduction(n,L,D,Z);
        matmul("TN",n,1,n,1.0,Z,a,0.0,z); /* z=Z'*a */
        
        /* mlambda search mlambda搜索 */
        if (!(info=search(n,m,L,D,z,E,s))) {

            /* 将在新空间中固定的模糊度逆变换回双差模糊度空间中 */
            info=solve("T",Z,E,n,m,F); /* F=Z'\E */
        }
    }
    free(L); free(D); free(Z); free(z); free(E);
    return info;
}

降相关子函数reduction()

/* lambda reduction (z=Z'*a, Qz=Z'*Q*Z=L'*diag(D)*L) (ref.[1]) ---------------*/
static void reduction(int n, double *L, double *D, double *Z)
{
    int i,j,k;
    double del;
    
    j=n-2; k=n-2;
    //这里的调序变换类似插入排序的思路？
    while (j>=0) {
        //由于第k+1，k+2，...，n-2列都进行过降相关并且没有被上一次调序变换影响，
        //因此只需对第0,1，...，k-1，k列进行降相关
        if (j<=k) for (i=j+1;i<n;i++) gauss(n,L,Z,i,j);//从最后一列开始，各列非对角线元素从上往下依次降相关
        del=D[j]+L[j+1+j*n]*L[j+1+j*n]*D[j+1];
        if (del+1E-6<D[j+1]) { /* 检验条件，若不满足检验条件则开始进行调序变换 compared considering numerical error */
            perm(n,L,D,j,del,Z);//调序变换
            k=j; j=n-2;//完成调序变换后重新从最后一列开始进行降相关及排序，k记录最后一次进行过调序变换的列序号
        }
        else j--;
    }
}

搜索子函数search()

static int search(int n, int m, const double *L, const double *D,
                  const double *zs, double *zn, double *s)
{
    int i,j,k,c,nn=0,imax=0;
    double newdist,maxdist=1E99,y;//maxdist，当前超椭圆半径
    double *S=zeros(n,n),*dist=mat(n,1),*zb=mat(n,1),*z=mat(n,1),*step=mat(n,1);
    
    k=n-1; dist[k]=0.0; //k表示当前层，从最后一层（n-1）开始计算
    zb[k]=zs[k];//即zn
    z[k]=ROUND(zb[k]); y=zb[k]-z[k]; step[k]=SGN(y);    //四舍五入取整；取整后的数与未取整的数作差；step记录z[k]是四舍还是五入
    for (c=0;c<LOOPMAX;c++) {
        newdist=dist[k]+y*y/D[k];
        if (newdist<maxdist) {  //如果当前累积目标函数计算值小于当前超椭圆半径
            if (k!=0) { //情况1：若还未计算至第一层，继续计算累积目标函数值
                dist[--k]=newdist;  //记录下当前层的累积目标函数值，dist[k]表示了第k,k+1,...,n-1层的目标函数计算和
                for (i=0;i<=k;i++)
                    S[k+i*n]=S[k+1+i*n]+(z[k+1]-zb[k+1])*L[k+1+i*n];//
                zb[k]=zs[k]+S[k+k*n];   //计算Zk，即第k个整数模糊度参数的备选组的中心
                z[k]=ROUND(zb[k]); y=zb[k]-z[k]; step[k]=SGN(y);    //四舍五入取整；取整后的数与未取整的数作差；记录是四舍还是五入
            }
            else {  //情况2：若已经计算至第一层，意味着所有层的累积目标函数值计算完毕
                //nn为当前候选解数，m为我们需要的固定解数，这里为2，表示需要一个最优解及一个次优解
                //s记录候选解的目标函数值，imax记录之前候选解中的最大目标函数值的坐标
                if (nn<m) { //若候选解数还没满
                    if (nn==0||newdist>s[imax]) imax=nn;//若当前解的目标函数值比之前最大的目标函数值都大，那么更新imax使s[imax]指向当前解中具有的最大目标函数值
                    for (i=0;i<n;i++) zn[i+nn*n]=z[i];//zn存放所有候选解
                    s[nn++]=newdist;//s记录当前目标函数值newdist，并加加当前候选解数nn
                }
                else {  //若候选解数已满（即当前zn中已经存了2个候选解）
                    if (newdist<s[imax]) {  //若 当前解的目标函数值 比 s中的最大目标函数值 小
                        for (i=0;i<n;i++) zn[i+imax*n]=z[i];    //用当前解替换zn中具有较大目标函数值的解
                        s[imax]=newdist;    //用当前解的目标函数值替换s中的最大目标函数值
                        for (i=imax=0;i<m;i++) if (s[imax]<s[i]) imax=i;    //更新imax保证imax始终指向s中的最大目标函数值
                    }
                    maxdist=s[imax];    //用当前最大的目标函数值更新超椭圆半径
                }
                z[0]+=step[0]; y=zb[0]-z[0]; step[0]=-step[0]-SGN(step[0]); //在第一层，取下一个有效的整数模糊度参数进行计算（若zb为5.3，则z取值顺序为5,6,4,7，...）
            }
        }
        else {  //情况3：如果当前累积目标函数计算值大于当前超椭圆半径
            if (k==n-1) break; //如果当前层为第n-1层，意味着后续目标函数各项的计算都会超出超椭圆半径，因此终止搜索
            else {  //若当前层不是第n-1层
                k++;    //退后一层，即从第k层退到第k+1层
                z[k]+=step[k]; y=zb[k]-z[k]; step[k]=-step[k]-SGN(step[k]); //计算退后一层后，当前层的下一个有效备选解
            }
        }
    }

    // 对s中的目标函数值及zn中的候选解进行排序（以s中目标函数值为排序标准，进行升序排序）
    // RTKLIB中最终可以得到一个最优解一个次优解，存在zn中，两解对应的目标函数值，存在s中
    for (i=0;i<m-1;i++) { /* sort by s */
        for (j=i+1;j<m;j++) {
            if (s[i]<s[j]) continue;
            SWAP(s[i],s[j]);
            for (k=0;k<n;k++) SWAP(zn[k+i*n],zn[k+j*n]);
        }
    }
    free(S); free(dist); free(zb); free(z); free(step);
    
    if (c>=LOOPMAX) {
        fprintf(stderr,"%s : search loop count overflow\n",__FILE__);
        return -1;
    }
    return 0;
}