RTKLIB中estpos()函数之——rescode()函数

最小二乘原理：x=(v^T *v)^-1 * (v^T * H)

x：即为接收机位置

rescode()函数就是求出v和H，为下一步用最小二乘法求解接收机位置做准备

先介绍rescode()函数形参变量名：

obs：当前历元的观测数据         n：观测数            nav ：导航数据          opt ：选择设置           sol：结果

rs：卫星的位置和速度（6*1）   vare：卫星的位置和速度协方差矩阵

dts：卫星的钟差和钟漂              svh：卫星的健康标志（-1：表示此卫星不可用）         azel：卫星的方位角和高度角

iter：迭代次数                            v：伪距残差         var：伪距残差的协方差矩阵             H：伪距观测方程线性化后未知数前面的系数阵

注：第一次计算，因为不知道rover的位置，所以位置赋的是0；

       此函数跟求载波残差的zdres()函数类似

/* pseudorange residuals -----------------------------------------------------*/
static int rescode(int iter, const obsd_t *obs, int n, const double *rs,
                   const double *dts, const double *vare, const int *svh,
                   const nav_t *nav, const double *x, const prcopt_t *opt,
                   double *v, double *H, double *var, double *azel, int *vsat,
                   double *resp, int *ns)
{
    double r,dion,dtrp,vmeas,vion,vtrp,rr[3],pos[3],dtr,e[3],P;
    int i,j,nv=0,sys,mask[4]={0};
    
    trace(3,"resprng : n=%d
",n);
    
    for (i=0;i<3;i++) rr[i]=x[i]; dtr=x[3];
    
    ecef2pos(rr,pos);
    
    for (i=*ns=0;i<n&&i<MAXOBS;i++) {
        vsat[i]=0; azel[i*2]=azel[1+i*2]=resp[i]=0.0;
        
        if (!(sys=satsys(obs[i].sat,NULL))) continue;
        
        /* reject duplicated observation data */ 
        if (i<n-1&&i<MAXOBS-1&&obs[i].sat==obs[i+1].sat) {
            trace(2,"duplicated observation data %s sat=%2d
",
                  time_str(obs[i].time,3),obs[i].sat);
            i++;
            continue;
        }
        /* geometric distance/azimuth/elevation angle */
        if ((r=geodist(rs+i*6,rr,e))<=0.0||                       /*  根据接收机位置和卫星位置计算出卫星距离接收机的距离储存在rs[0-2]   */
            satazel(pos,e,azel+i*2)<opt->elmin) continue;         /*  把计算出卫星的方位角和高度角储存在变量azel中   */       
        
        /* psudorange with code bias correction */                /*  对量测的伪距进行码间偏差改正   */
        if ((P=prange(obs+i,nav,azel+i*2,iter,opt,&vmeas))==0.0) continue;        /*  疑问：为什么只用一个伪距   */
        
        /* excluded satellite? */
        if (satexclude(obs[i].sat,svh[i],opt)) continue;          /*  排除星历不可用（svh=-1）、配置文件中没包括的卫星系统的卫星；opt->exsats？？？  */
        
        /* ionospheric corrections */
        if (!ionocorr(obs[i].time,nav,obs[i].sat,pos,azel+i*2,
                      iter>0?opt->ionoopt:IONOOPT_BRDC,&dion,&vion)) continue;
        
        /* tropospheric corrections */
        if (!tropcorr(obs[i].time,nav,pos,azel+i*2,
                      iter>0?opt->tropopt:TROPOPT_SAAS,&dtrp,&vtrp)) {
            continue;
        }
        /* pseudorange residual */
        v[nv]=P-(r+dtr-CLIGHT*dts[i*2]+dion+dtrp);                /*   最关键的一步，用前面计算的星地距离r和经改正后的伪距观测值P，做差求得v*/
        
        /* design matrix */
        for (j=0;j<NX;j++) H[j+nv*NX]=j<3?-e[j]:(j==3?1.0:0.0);   /*   根据伪距观测方程线性化，求线性化后的未知数前面的系数阵  */
        
        /* time system and receiver bias offset correction */
        if      (sys==SYS_GLO) {v[nv]-=x[4]; H[4+nv*NX]=1.0; mask[1]=1;}
        else if (sys==SYS_GAL) {v[nv]-=x[5]; H[5+nv*NX]=1.0; mask[2]=1;}
        else if (sys==SYS_CMP) {v[nv]-=x[6]; H[6+nv*NX]=1.0; mask[3]=1;}
        else mask[0]=1;
        
        vsat[i]=1; resp[i]=v[nv]; (*ns)++;
        
        /* error variance */
        var[nv++]=varerr(opt,azel[1+i*2],sys)+vare[i]+vmeas+vion+vtrp;
        
        trace(4,"sat=%2d azel=%5.1f %4.1f res=%7.3f sig=%5.3f
",obs[i].sat,
              azel[i*2]*R2D,azel[1+i*2]*R2D,resp[i],sqrt(var[nv-1]));
    }
    /* constraint to avoid rank-deficient */
    for (i=0;i<4;i++) {
        if (mask[i]) continue;
        v[nv]=0.0;
        for (j=0;j<NX;j++) H[j+nv*NX]=j==i+3?1.0:0.0;
        var[nv++]=0.01;
    }
    return nv;
}