rtklib源码 rtk差分解算，rtkpos和replos函数流程梳理

Posted 2022-07-20 haoming Hu

rtklib源码 rtk差分解算，rtkpos和replos函数流程梳理

• rtkpos函数梳理
• • 总体流程
• replos函数梳理
• • replos总体流程
  • 1. 通过satposs函数计算卫星的位置、速度等参数
  • 2. 通过zdres函数计算基站伪距和载波相位的非差分残差
  • • 2.1 初始化残差数组 y
    • 2.2 地球潮汐校正
    • 2.3 基站接收机的星地距离修正（每颗卫星都修正）
    • • 2.3.1 计算接收机到卫星的几何距离 geodist
      • 2.3.2 计算每颗卫星的方位角和仰角 satazel
      • 2.3.3 剔除卫星 satexclude
      • 2.3.4 使用钟差补偿到星地几何距离
      • 2.3.5 估算对流层延迟并补偿到星地距离 tropmodel、tropmapf
      • • 2.3.6 接收机天线相位中心校正
      • 2.4 计算伪距和载波相位残差 zdres_sat
  • 3. 选择移动站和基站之间所有的公共卫星 selsat
  • 4. 当前状态更新 udstate
  • • 4.1 得到相邻历元时间差
    • 4.2 rover位置、速度、加速度及其方差更新 udpos

rtkpos函数梳理

总体流程

1. 分别计算移动站和基站的观测值数量 nu和nr
2. 通过单点定位函数得到移动站的大致位置和速度
3. 判断定位模式，根据不同的定位模式进行解算结果输出，如果是rtk则执行replos函数进行差分解算

replos函数梳理

replos总体流程

1. 计算卫星的位置、速度、钟差、钟漂等参数

1. 通过satposs函数计算卫星的位置、速度等参数

在函数的最开始可以看到，这里计算的卫星数据是所有移动站和基准站的观测到的卫星星历得到的。（此处的n是n=nu+nr，在replos函数头部处定义时已经相加了 ）

for (i=0;i<n&&i<2*MAXOBS;i++) 

2. 通过zdres函数计算基站伪距和载波相位的非差分残差

    if (!zdres(1,obs+nu,nr,rs+nu*6,dts+nu*2,svh+nu,nav,rtk->rb,opt,1,
               y+nu*nf*2,e+nu*3,azel+nu*2)) 
        errmsg(rtk,"initial base station position error\\n");
        free(rs); free(dts); free(var); free(y); free(e); free(azel);
        return 0;
    

传入的obs是obs+nu而不是obs是因为，obs是由移动站和基站的观测值共同组成，基站部分的在后面，所以加一个nu作为偏移，nu：移动站观测的卫星数 nr：参考站观测的卫星数
输入的rtk->rb是基站的位置信息，这个在前面已经进行了定位了，或者配置中已经给出了基站位置

2.1 初始化残差数组 y

for (i=0;i<n*nf*2;i++) y[i]=0.0;  //nf=2，载波数量

2.2 地球潮汐校正

   if (opt->tidecorr) 
        tidedisp(gpst2utc(obs[0].time),rr_,opt->tidecorr,&nav->erp,
                 opt->odisp[base],disp);
        for (i=0;i<3;i++) rr_[i]+=disp[i];
    

（在配置中没有用到这个改正）

2.3 基站接收机的星地距离修正（每颗卫星都修正）

    //huhaoming：将 ecef 转换为大地坐标 pos是转化后的坐标
    ecef2pos(rr_,pos);
    
    for (i=0;i<n;i++) 
        /* compute geometric-range and azimuth/elevation angle 计算几何范围和方位角/仰角 */
        if ((r=geodist(rs+i*6,rr_,e+i*3))<=0.0) continue;
        if (satazel(pos,e+i*3,azel+i*2)<opt->elmin) continue;
        
        /* excluded satellite? 剔除卫星*/
        if (satexclude(obs[i].sat,svh[i],opt)) continue;
        
        /* satellite clock-bias 使用钟差补偿到星地几何距离 */
        r+=-CLIGHT*dts[i*2];
        
        /* troposphere delay model (hydrostatic)*/
        zhd=tropmodel(obs[0].time,pos,zazel,0.0);
        r+=tropmapf(obs[i].time,pos,azel+i*2,NULL)*zhd;
        
        /* receiver antenna phase center correction 接收机天线相位中心校正*/
        antmodel(opt->pcvr+index,opt->antdel[index],azel+i*2,opt->posopt[1], dant);
        
        /* undifferenced phase/code residual for satellite */
        zdres_sat(base,r,obs+i,nav,azel+i*2,dant,opt,y+i*nf*2);
    

2.3.1 计算接收机到卫星的几何距离 geodist

 //huhaoming：求接收机到卫星的几何距离，存在变量r中
        if ((r=geodist(rs+i*6,rr_,e+i*3))<=0.0) continue;```

```c
/* geometric distance ----------------------------------------------------------
 * compute geometric distance and receiver-to-satellite unit vector
 * args   : double *rs       I   satellilte position (ecef at transmission) (m)
 *          double *rr       I   receiver position (ecef at reception) (m)
 *          double *e        O   line-of-sight vector (ecef)
 * return : geometric distance (m) (0>:error/no satellite position)
 * notes  : distance includes sagnac effect correction
*-----------------------------------------------------------------------------*/
extern double geodist(const double *rs, const double *rr, double *e)

    double r;
    int i;
    
    if (norm(rs,3)<RE_WGS84) return -1.0;
    for (i=0;i<3;i++) e[i]=rs[i]-rr[i];
    r=norm(e,3);
    for (i=0;i<3;i++) e[i]/=r;

    //huhaoming：OMGE*(rs[0]*rr[1]-rs[1]*rr[0])/CLIGHT：地球自转改正
    return r+OMGE*(rs[0]*rr[1]-rs[1]*rr[0])/CLIGHT;

• rs：卫星位置
• rr：接收机位置
• e：接收机到卫星的单位向量，方向是由接收机指向卫星

计算的时候通过两个三维向量相减，最后再求范数即可得到星地距离
最后返回值是地球自转改正后的星地距离

2.3.2 计算每颗卫星的方位角和仰角 satazel

//如果大于最小值，则在后面用卫星钟差和对流层延时来修正星地距离
if (satazel(pos,e+i*3,azel+i*2)<opt->elmin) continue;```

/* satellite azimuth/elevation angle -------------------------------------------
* compute satellite azimuth/elevation angle
* args   : double *pos      I   geodetic position lat,lon,h (rad,m)
*          double *e        I   receiver-to-satellilte unit vevtor (ecef)
*          double *azel     IO  azimuth/elevation az,el (rad) (NULL: no output)
*                               (0.0<=azel[0]<2*pi,-pi/2<=azel[1]<=pi/2)
* return : elevation angle (rad)
*-----------------------------------------------------------------------------*/
extern double satazel(const double *pos, const double *e, double *azel)

    double az=0.0,el=PI/2.0,enu[3];
    
    if (pos[2]>-RE_WGS84) 
        ecef2enu(pos,e,enu);
        az=dot(enu,enu,2)<1E-12?0.0:atan2(enu[0],enu[1]);
        if (az<0.0) az+=2*PI;
        el=asin(enu[2]);
    
    if (azel) azel[0]=az; azel[1]=el;
    return el;

先判断基站位置在经纬高坐标系中的高度是否大于-RE_WGS84（基准椭球面的长半径），只有满足才可以进行方位角和高度角的计算。
传入的位置信息 pos 是大地坐标系下的坐标，需要将基站位置坐标由经纬高转换为东北天坐标系中。

ecef2enu(pos,e,enu);

然后在enu坐标系下用课本的公式来计算即可

        az=dot(enu,enu,2)<1E-12?0.0:atan2(enu[0],enu[1]);
        if (az<0.0) az+=2*PI;
        el=asin(enu[2]);

2.3.3 剔除卫星 satexclude

排除卫星的主要是根据prcopt_t结构体中的exsats进行判断是否有要排除或者保留的卫星数据

2.3.4 使用钟差补偿到星地几何距离

r+=-CLIGHT*dts[i*2];

2.3.5 估算对流层延迟并补偿到星地距离 tropmodel、tropmapf

     //huhaoming：用saastamoinen经验模型，通过测站纬度、高程、气温、气压和水汽压等信息计算对流层延迟
        zhd=tropmodel(obs[0].time,pos,zazel,0.0);
        /*huhaoming：计算对流层延迟投影函数（即天顶方向到接收机相对卫星观测方向上的对流层延迟投影系数）
        * 这个函数中有两种投影函数的计算方法，分别是GMF和NMF，对应的两篇参考论文为“Global mapping functions for the atmosphere delay at radio wavelengths”和“Global Mapping Function(GMF): A new empirical mapping function base on numerical weather model data”
        *默认使用的是NMF方法，也可以通过定义IERS_MODEL宏来使用GMF方法。
        *这个函数调用完成后，会将返回的干投影函数和tropmodel中计算得到的天顶方向对流层干延迟相乘，从而得到接收机相对卫星观测方向上的对流层延迟。
        *干投影函数是通过返回值获得的，而湿投影是通过输入/输出参数mapfw获得的。在zdres函数中，本函数输入的mapfw=NULL，即不输出湿投影。
        *湿分量会在之后的ddres（计算双差残差的函数）函数中进行扣除。
        */
        r+=tropmapf(obs[i].time,pos,azel+i*2,NULL)*zhd;

2.3.6 接收机天线相位中心校正

  antmodel(opt->pcvr+index,opt->antdel[index],azel+i*2,opt->posopt[1], dant);

生成一组接收机相对于卫星观测方向的单位矢量e，e[0]，e[1]，e[2]分别为东、北、天三个方向上的分量；
频段不同，天线的相位中心偏移(PCO)不同。先计算出每个频段天线在东、北、天三个方向总的偏移，即相位中心偏移pcv-off[i][j]与del[j]之和
计算2中的相位中心偏移在观测单位矢量e上的投影dot(off,e,3)，由于e是单位矢量，所以和它求内积实际上就在该方向上的投影；
计算天线相位中心变化量（PCV）：不同的高度角，相位中心变化不同，因此根据高度角对pcv->var[i]进行插值计算。
3、4两部分即为总的天线偏移：dant[i]=-dot(off,e,3)+(opt?interpvar(90.0-azel[1]*R2D,pcv->var[i]):0.0);

2.4 计算伪距和载波相位残差 zdres_sat

zdres_sat(base,r,obs+i,nav,azel+i*2,dant,opt,y+i*nf*2);

/* undifferenced phase/code residual for satellite ---------------------------*/
static void zdres_sat(int base, double r, const obsd_t *obs, const nav_t *nav,
                      const double *azel, const double *dant,
                      const prcopt_t *opt, double *y)

    const double *lam=nav->lam[obs->sat-1];
    double f1,f2,C1,C2,dant_if;
    int i,nf=NF(opt);
    
    if (opt->ionoopt==IONOOPT_IFLC)  /* iono-free linear combination */
        if (lam[0]==0.0||lam[1]==0.0) return;
        
        if (testsnr(base,0,azel[1],obs->SNR[0]*0.25,&opt->snrmask)||
            testsnr(base,1,azel[1],obs->SNR[1]*0.25,&opt->snrmask)) return;
        
        f1=CLIGHT/lam[0];
        f2=CLIGHT/lam[1];
        C1= SQR(f1)/(SQR(f1)-SQR(f2));
        C2=-SQR(f2)/(SQR(f1)-SQR(f2));
        dant_if=C1*dant[0]+C2*dant[1];
        
        if (obs->L[0]!=0.0&&obs->L[1]!=0.0) 
            y[0]=C1*obs->L[0]*lam[0]+C2*obs->L[1]*lam[1]-r-dant_if;
        
        if (obs->P[0]!=0.0&&obs->P[1]!=0.0) 
            y[1]=C1*obs->P[0]+C2*obs->P[1]-r-dant_if;
        
    
    else 
        for (i=0;i<nf;i++) 
            if (lam[i]==0.0) continue;
            
            /* check snr mask */
            /*
			函数testsnr()是用来排除接收信号强度小于规定强度的数据，这个规定信号强度（最小信号强度）
			是根据卫星的仰角来得到的（或者说与卫星的仰角有关）。
			*/
            if (testsnr(base,i,azel[1],obs->SNR[i]*0.25,&opt->snrmask)) 
                continue;
            
            /* residuals = observable - pseudorange 残差 = 观测值 - 伪距 */
            //huhaoming： 计算每个频段下的载波相位、伪距残差：残差 = 观测量 - 卫地距 - 天线偏移
            //dant:天线相位偏差
            //y[0]到y[nf - 1]保存载波相位残差，y[nf]到y[2nf - 1]保存伪距残差
            if (obs->L[i]!=0.0) y[i   ]=obs->L[i]*lam[i]-r-dant[i];//载波相位
            if (obs->P[i]!=0.0) y[i+nf]=obs->P[i]       -r-dant[i];//伪距
        
    

处理过程
如果在配置中选择了无电离层线性组合（IFLC：Ionospheric free linear combination）：
a). 调用testsnr函数检查L1，L2观测量的载噪比是否大于配置中SNR Mask的最低要求（SNR Mask设置见RTKlib mannual 3.5章）；
b). 计算无电离层线性组合系数C1、C2, 并采用该系数计算无电离层组合的天线偏移量；
c). 计算无电离层线性组合载波相位和伪距残差：残差 = IFLC观测量 - 卫地距 - 天线偏移量。
如果不是无电离层组合:
a). 调用testsnr，检查每个频段的载噪比是否大于配置中SNR Mask的要求；
b). 计算每个频段下的载波相位、伪距残差：残差 = 观测量 - 卫地距 - 天线偏移量。

3. 选择移动站和基站之间所有的公共卫星 selsat

/* select common satellites between rover and reference station 选择流动站和参考站之间的公共卫星 --------------*/
static int selsat(const obsd_t *obs, double *azel, int nu, int nr,
                  const prcopt_t *opt, int *sat, int *iu, int *ir)

    int i,j,k=0;
    
    trace(3,"selsat  : nu=%d nr=%d\\n",nu,nr);
    
    for (i=0,j=nu;i<nu&&j<nu+nr;i++,j++) 
        if      (obs[i].sat<obs[j].sat) j--;
        else if (obs[i].sat>obs[j].sat) i--;
        else if (azel[1+j*2]>=opt->elmin)  /* elevation at base station */
            sat[k]=obs[i].sat; iu[k]=i; ir[k++]=j;
            trace(3,"(%2d) sat=%3d iu=%2d ir=%2d\\n",k-1,obs[i].sat,i,j);
        
    
    return k;

查找算法：
基站和移动站的观测数据都在obs中，并且obs的卫星号都是经过排序的，所以星号大小是按照从小打到的顺序排列。如果移动站所观察的数据卫星号小于基站所观察的卫星号，则基站观察数据不变，依次搜寻移动站的观察数据，直到不再小于，此时有两种情况，一种是等于，一种是大于。

等于时：将当前为星号存入sat[0,1….]中，将这颗卫星所对应的移动站观测数据号i,
基站观测数据j,分别存入iu[0,1….],ir[0,1…].

大于时：说明移动站没有观测的卫星与当前基站的这颗卫星相同。

如果移动站所观察的数据卫星号大于基站所观察的卫星号，算法思路与上面的一致。

4. 当前状态更新 udstate

主要更新：

1. udpos ：kalman中状态更新方程、状态协方差方程更新，包括接收机位置、速度、加速度；
2. udion ： 电离层状态、协方差更新；
3. udtrop ： 对流层状态、协方差更新；
4. udrcvbias：接收器的时间更新
5. udbias ：更新单差模糊度状态、协方差；
   最后存放到了rtk参数中

4.1 得到相邻历元时间差

double tt=rtk->tt,bl,dr[3];

在前面的时候rtk->tt已经计算过了 rtk->tt=timediff(rtk->sol.time,time); time是前一历元时间，rtk->sol.time是当前历元时间

4.2 rover位置、速度、加速度及其方差更新 udpos

udpos(rtk,tt); ：更新位置、速度、加速度，得到rtk->x，也就是浮点解 float状态

• 如果定位模式是PMODE_FIXED，则将配置中的移动站坐标rtk->opt.ru[]赋值给rtk->x，由此得到状态的协方差矩阵
  
• 初始化第一个历元的位置
  如果检测到状态矩阵位置为0，则说明是当前的历元是首历元，我们将状态的位置部分初始化为单点定位得到的大致移动站位置，状态矩阵对应的协方差矩阵为：
  
• 如果是静态模式 static mode 则直接返回，结束udpos
• 如果是 Kinmatic模式（无动力模型）(本次分析属于这种模式)
  当前的状态矩阵为当前的状态矩阵为：将状态的位置部分初始化为单点定位得到的大致移动站位置。协方差矩阵：
  

最后更新时间：2022年7月18日15:43:15