抗差估计在rtklib的pntpos函数（标准单点定位spp）中的c代码实现

一、背景

在rtklib中spp定位算法主要是在pntpos函数中实现，pvt在pntpos实现的那是淋漓尽致啊。
1. 从观测值类型上来说：
（1）如果我们有伪距观测值，那么可以在estpos中进行定位和接收机钟差稳定度分析；（2）如果我们还有多普勒频移，那么可以在estvel中进行定速，当然，如果我们还有载波观测值的话，也可以通过历元间载波差进行定速。
2. 从观测值频率上来说：
（1）如果我们是双频伪距观测值的话，那么根据我们对电离层的处理策略不同，使用的观测值类型也是不同的，具体来说，如果我们使用消电离层组合的话，那么需要使用双频观测值进行伪距消电离层组合得到组合观测值；如果我们使用的是brdc处理电离层延迟，那么我们只使用L1伪距观测值，如果我们使用TEQC进行数据质量分析，就会发现L1比L2的mp1要小于mp2，另外，我们在进行rtk的时候，有效卫星数的计算也是根据L1来累加的。
3. 抗差spp的必要性：
为什么要进行spp的抗差估计呢？因为我们在进行高精度定位的时候，spp是给kalman filter初值，如果这个初值不准的话，那么我们经过filter得到的float解偏离实际位置，就会造成fix后的解也是不对的，例如飞点的现象。
4. spp数学模型简介：
在掌握了spp的数学模型基础上，数学模型由函数模型和随机模型组成，函数模型就是伪距观测方程，随机模型包括先验随机模型（等权模型、高度角随机模型、信噪比随机模型）和验后随机模型（helmet方差分量估计），函数模型可以让我们得到误差方程v=Bx-l，随机模型让我们得到了权阵P。那么在rtklib的spp中，是使用加权迭代最小二乘来得到dx，再和初值x0相加即可得到pos和dt值。
5.spp随机模型：
关于随机模型，想多说一点，如果有朋友想使用helmet方差分量估计，那么要保证先验权阵尽量准确，否则就是瞎扯了，其实在rtklib中varerr函数中的权重比，我们可以使用helmet去得到，例如，gps和bds在spp中权重是一样的吗，即使在bds中，geo/meo/igso的权重是一样的吗？大家可以去实践一下。另外，rtklib的高度角随机模型中的sin(el)没有求平方，可以使用SQR(sin(el))。说到这儿，我有个补充，观测误差的方差是由三部分组成，卫星位置方差（卫星端），大气延迟方差（传播端，电离层、对流层），使用随机模型求得的量测误差（接收机端，也就是使用随机模型的地方）。

二、抗差spp实现

在rtklib中，历元k存在多次迭代，迭代次数最多是10，即使是在首次定位的情况下，即状态向量（跟未知向量、未知参数一个意思）全为0的时候，也只需要6次迭代就可以得到最终的结果，后续历元一般需要3次才能得到最终结果，当然了，得到了结果可能是不正确的，这个时候valpos函数就起作用了，valpos中主要通过卡方检验和dop值检验两方面进行有效性检测，如果失败了，那么该历元的结果就不输出了，这点可能有些朋友不会注意到，大家的焦点可能在lsq比较多吧。

2.1.抗差估计代码
rtklib中spp上应用抗差估计代码如下，我是先通过标准化残差找到最大的那个值进行剔除，我这里只有零权和保权处理，具体应用数据具体再修改细节吧。代码如下：

/* compare residual data -------------------------------------------------*/
static int cmpres(const void *p1, const void *p2)
{
	double *q1 = (double *)p1, *q2 = (double *)p2;
	double delta = *q1 - *q2;
	return delta<-0.0 ? -1 : (delta>0.0 ? 1 : 0);
}

/* estimate receiver position ------------------------------------------------*/
static int estpos(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)
{
    double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*v_,*var,sig;
    int i,j,k,info,stat,nv,ns;
	double k0=8.0;
	int ref = 0; // robust estimate start flag

    trace(3,"estpos  : n=%d\n",n);
	
    v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1);
	v_ = mat(n + 4, 1); // deal with residual(derive from [v])
    
    for (i=0;i<3;i++) x[i]=sol->rr[i];
    
    for (i=0;i<MAXITR;i++) {
        
        /* pseudo range residuals */
        nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
                   &ns);// H是按照nv*NX排列的，即nv代表行数，NX代表列数，恒为7
		matcpy(v_, v, ns, 1);
		// predicted normalized residual
		qsort(v_, ns, sizeof(double), cmpres);
		if (fabs(v_[0] < 100 && fabs(v_[ns - 1]) < 100))
		{
			ref = 1;
		}

		if (ref == 1) // robust estimate start
		{
			matcpy(v_, v, ns, 1);
			for (j = 0; j < ns; j++)
			{
				v_[j] = fabs(v_[j]) / sqrt(var[j]);
			}
			qsort(v_, ns, sizeof(double), cmpres);

			// local variable
			int v_max_index=0; // the biggest normalized residual index
			double  v_max= v_[ns - 1]; // the biggest normalized residual value

			matcpy(v_, v, ns, 1);
			for (j = 0; j < ns; j++)
			{
				v_[j] = sqrt(SQR(v_[j]) / var[j]);
				if (fabs(v_[j] -v_max)<1e-9)
				{
					v_max_index = j;
					break;
				}
			}

			if (v_max > k0)
			{
				if (ns > 4)
				{
					v[v_max_index] = 0;
					for (k = 0; k < NX; k++) H[k + v_max_index*NX] = 0;
				}

			}
		}

        if (nv<NX) {
            sprintf(msg,"lack of valid sats ns=%d",nv);
            break;
        }
        /* weight by variance */
        for (j=0;j<nv;j++) {
            sig=sqrt(var[j]);
            v[j]/=sig;
            for (k=0;k<NX;k++) H[k+j*NX]/=sig;
        }

        /* least square estimation */
        if ((info=lsq(H,v,NX,nv,dx,Q))) {
            sprintf(msg,"lsq error info=%d",info);
            break;
        }
		
        for (j=0;j<NX;j++) x[j]+=dx[j];
        
        if (norm(dx,NX)<1E-4) {
            sol->type=0;
            sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
            sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
            sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
            sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
            sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
            for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
            for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
            sol->qr[3]=(float)Q[1];    /* cov xy */
            sol->qr[4]=(float)Q[2+NX]; /* cov yz */
            sol->qr[5]=(float)Q[2];    /* cov zx */
            sol->ns=(unsigned char)ns;
            sol->age=sol->ratio=0.0;
            
            /* validate solution */
            if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) {
                sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE;
            }
            free(v); free(H); free(var);
            
            return stat;
        }
    }
    if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);
    
    free(v); free(H); free(var);
    
    return 0;
}

2.2.数据测试
使用自测的一段数据，强信号，开阔天空测得的静态数据，头文件中的信息如下：

我在历元[2020 6 5 9 8 14.0000000]对G09卫星伪距观测值施加100m的粗差，即由21954773.950变成了21954873.950。

不使用抗差估计，直接由迭代最小二乘得到的结果是：

那么如果使用抗差估计得到的结果是：

前两个历元的s观测值没有粗差污染，其spp结果分别为：


比较可知抗差spp有比较好的可靠性和定位精度。
// 更新于2020.11.27 01：36：00