Winchell

8.3 estvel()：多普勒测速

8.3.1 estvel()：函数主体

图8.3-1 estvel() 函数调用关系

1. 参数列表

/* args */
obsd_t   *obs     I   observation data
int       n       I   number of observation data
nav_t    *nav     I   navigation data
prcopt_t *opt     I   processing options
sol_t    *sol     IO  solution
double   *azel    IO  azimuth/elevation angle (rad) (NULL: no output)
ssat_t   *ssat    IO  satellite status              (NULL: no output)
char     *msg     O   error message for error exit
/* return */
int       status  -   1:ok,0:error

2. 执行流程

sol->time赋值第一个观测值（顺序存储的第一颗卫星）的时间
如果处理选项不是 SPP（针对 PPP），电离层校正选 Klobuchar 模型，对流层校正采用 Saastmoinen 模型
调用satposs()计算卫星位置、速度和钟差（ECEF 坐标系）
- rs [(0:2)+i*6]：obs[i] 卫星位置 {x, y, z} (m)
- rs [(3:5)+i*6]：obs[i] 卫星速度 {vx, vy, vz} (m/s)
- dts[(0:1)+i*2]：obs[i] 卫星钟差与钟漂 {bias, drift} (s, s/s)
- var[i]：卫星的误差方差 (m^2)，代表了卫星位置和钟差所包含的误差水平，最终会合入伪距的方差中
- svh[i]：卫星健康标志 (-1:correction not available)
调用estpos()使用伪距进行位置估计，采用加权最小二乘，其中会调用 valsol 进行卡方检验和 GDOP 检验
若estpos()中valsol检验失败，即位置估计失败，会调用raim_fde接收机自主完好性监测重新估计，前提是卫星数 > 6、对应参数解算设置opt->posopt[4]=1
调用estvel()使用多普勒进行速度估计
保存方位角和俯仰角信息，赋值卫星状态结构体ssat

3. 注意事项

关于钟漂信息：这里只计算了接收机的钟差，而没有计算接收机的频漂，原因在于 estvel 函数中虽然计算得到了接收机频漂，但并没有将其输出到 sol_t:dtr中。
C语言内存使用问题：C语言中用 malloc 申请的内存需要自己调用 free 来予以回收，源码中的 mat 、imat 、zeros 等函数都只是申请了内存，并没有进行内存的回收，在使用这些函数时，用户必须自己调用 free 来回收内存！源码中将使用这些函数的代码放置在同一行，在调用函数结尾处也统一进行内存回收，位置较为明显，不致于轻易忘记。

4. 问题思考

RTKLIB观测数据中的时间：源码中将 obs[0].time作为星历选择时间传递给 satposs函数，这样对于每一颗观测卫星，都要使用第一颗观测卫星的数据接收时间作为选择星历的时间标准。是否应该每颗卫星都使用自己的观测时间？或者应该使用每颗卫星自己的信号发射时间？还是说这点差别对选择合适的星历其实没有关系？该问题的分析参考附录B.1。
raim_fde 对卫星数目的要求：这里规定能够执行 raim_fde 函数的前提是数目大于等于 6，感觉不是只要大于等于 5就可以了吗？该问题的分析参考附录B.2。

5. 源码注释

点击查看代码

extern int pntpos(const obsd_t *obs, int n, const nav_t *nav,
                  const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
                  char *msg)
{
    prcopt_t opt_=*opt;
    double *rs,*dts,*var,*azel_,*resp;
    int i,stat,vsat[MAXOBS]={0},svh[MAXOBS];
    
    trace(3,"pntpos  : tobs=%s n=%d\n",time_str(obs[0].time,3),n);
    sol->stat=SOLQ_NONE;
    
    if (n<=0) {
        strcpy(msg,"no observation data");
        return 0;
    }

    sol->time=obs[0].time; // sol->time赋值第一个观测值的时间
    msg[0]='\0';
    
    rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n);
    
    if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */
        opt_.ionoopt=IONOOPT_BRDC; // 电离层校正选 Klobuchar 广播星历模型
        opt_.tropopt=TROPOPT_SAAS; // 对流层校正采用 Saastmoinen 模型
    }
    
    // 计算卫星位置、速度和钟差
    /* satellite positons, velocities and clocks */
    satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh);
    
    // 用伪距位置估计，加权最小二乘，其中会调用 valsol 进行卡方检验和 GDOP 检验
    /* estimate receiver position with pseudorange */
    stat=estpos(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
    
    /* RAIM FDE */
    if (!stat&&n>=6&&opt->posopt[4]) {  
        // estpos 中 valsol 检验失败，即位置估计失败，则会调用 RAIM 接收机自主完好性监测重新估计，
        // 前提是卫星数 > 6，且对应参数解算设置 opt->posopt[4]=1，即pos1-posopt5=on（RAIM FDE）
        stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
    }
    // 调用 estvel 进行多普勒速度估计
    /* estimate receiver velocity with Doppler */
    if (stat) {
        estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat);
    }
    if (azel) {
        for (i=0;i<n*2;i++) azel[i]=azel_[i];
    }
    if (ssat) { // 保存卫星状态信息至ssat结构体
        for (i=0;i<MAXSAT;i++) {
            ssat[i].vs=0;
            ssat[i].azel[0]=ssat[i].azel[1]=0.0;
            ssat[i].resp[0]=ssat[i].resc[0]=0.0;
            ssat[i].snr[0]=0;
        }
        for (i=0;i<n;i++) {
            ssat[obs[i].sat-1].azel[0]=azel_[  i*2];
            ssat[obs[i].sat-1].azel[1]=azel_[1+i*2];
            ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0];
            if (!vsat[i]) continue;
            ssat[obs[i].sat-1].vs=1;
            ssat[obs[i].sat-1].resp[0]=resp[i];
        }
    }
    free(rs); free(dts); free(var); free(azel_); free(resp);
    return stat;
}

8.3.2 estvel()：通过多普勒计算接收机速度

1. 参数列表

/* args */
obsd_t   *obs      I   OBS观测数据
int       n        I   观测数据的数量
double   *rs       I   卫星位置和速度，长度为6*n，{x,y,z,vx,vy,vz}(ecef)(m,m/s)
double   *dts      I   卫星钟差，长度为2*n， {bias,drift} (s|s/s)
nav_t    *nav      I   导航数据
prcopt_t *opt      I   处理过程选项
sol_t    *sol      IO  solution
double   *azel     IO  方位角和俯仰角 (rad)
int      *vsat     IO  定位时有效卫星
char     *msg      O   错误消息
/* return */
int       status   -   1:ok，0:error

2. 执行流程

定速的初始值直接给定为 0 ，而不像定位时初值选上一历元的的位置（一定程度上因为 SPP 的速度估计精度较低）。
for循环迭代计算，最大迭代次数（默认10次）
- 调用resdop()，计算定速方程组左边的雅可比矩阵和右端的速度残差，返回定速时所使用的卫星数目
- 调用最小二乘法lsq()函数，解出{速度、频漂}的改正量dx，累加到x中。
- 检查当前计算出的改正量的绝对值是否小于 1E-6 ，满足条件则说明当前解已经很接近真实值了，将接收机三个方向上的速度、协方差存入到 sol->rr 中。否则进行下一次循环。

3. 注意事项

钟漂信息：最终向 sol 速度结果时，并没有存储所计算出的接收器钟漂。

4. 源码注释

点击查看代码

static void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
                   const nav_t *nav, const prcopt_t *opt, sol_t *sol,
                   const double *azel, const int *vsat)
{
   // 定速的初始值直接给定为 0 ，而不像定位时初值选上一历元的的位置（一定程度上因为 SPP 的速度估计精度较低）
   double x[4]={0},dx[4],Q[16],*v,*H;
   double err=opt->err[4]; /* Doppler error (Hz) */
   int i,j,nv;
   
   trace(3,"estvel  : n=%d\n",n);
   
   v=mat(n,1); H=mat(4,n);
   
   for (i=0;i<MAXITR;i++) {
       // 调用 resdop，计算定速方程组左边的雅可比矩阵和右端的速度残余，返回定速时所使用的卫星数目
       /* range rate residuals (m/s) */
       if ((nv=resdop(obs,n,rs,dts,nav,sol->rr,x,azel,vsat,err,v,H))<4) {
           break;
       }
       // 调用最小二乘法 lsq 函数，解出{速度、频漂}的改正量 dx，累加到 x 中。
       // 频漂计算了，但并没有存储下来
       /* least square estimation */
       if (lsq(H,v,4,nv,dx,Q)) break;        
       for (j=0;j<4;j++) x[j]+=dx[j];
       // 检查当前计算出的改正量的绝对值是否小于 1E-6,
       // 是：则说明当前解已经很接近真实值了，将接收机三个方向上的速度存入到 sol->rr 中
       // 否：进行下一次循环
       if (norm(dx,4)<1E-6) {
           matcpy(sol->rr+3,x,3,1);
           sol->qv[0]=(float)Q[0];  /* xx */
           sol->qv[1]=(float)Q[5];  /* yy */
           sol->qv[2]=(float)Q[10]; /* zz */
           sol->qv[3]=(float)Q[1];  /* xy */
           sol->qv[4]=(float)Q[6];  /* yz */
           sol->qv[5]=(float)Q[2];  /* zx */
           break;
       }   
   }
   free(v); free(H);
}

8.1.3 resdop()：定速方程残差计算、雅可比矩阵构建

计算定速方程组左边的雅可比矩阵和右端的速度残余，返回定速时所使用的卫星数目。

1. 参数列表：

/* args */
obsd_t   *obs      I   观测量数据
int      n         I   观测量数据的数量
double   *rs       I   卫星位置和速度，长度为6*n，{x,y,z,vx,vy,vz}(ecef)(m,m/s)
double   *dts      I   卫星钟差，长度为2*n， {bias,drift} (s|s/s)
nav_t    *nav      I   导航数据
double   *rr       I   接收机位置和速度，长度为6，{x,y,z,vx,vy,vz}(ecef)(m,m/s)
double   *x        I   本次迭代开始之前的定速值，长度为4，{vx,vy,vz,drift}
double   *azel     IO  方位角和俯仰角 (rad)
int      *vsat     I   卫星在定速时是否有效
double   *v        O   定速方程的右端部分，速度残差
double   *H        O   定速方程中的雅可比矩阵
/* return */
int       nv       -   定速时所使用的卫星数目

2. 执行流程

调用ecef2pos()函数，将接收机位置由 ECEF-XYZ 转换为大地坐标系LLH，调用xyz2enu()函数，计算到ENU坐标转换矩阵E。
遍历当前历元每一个观测值（卫星），即遍历所有卫星：
- 去除在定速时不可用的卫星。
- 计算当前接收机位置下 ENU中的视向量e，然后用刚刚计算出的转换矩阵E转换得到 ECEF 中视向量的值。
- 计算ECEF中卫星相对于接收机的速度，卫星速度rs[j+3+i*6]-传入的定速初值x[j] 。
- 计算考虑了地球自转的用户和卫星之间的几何距离变化率rate 。
- 计算rate的标准差、残差。
- 构建雅可比矩阵 H，将观测方程数 nv 加1
  RTKLIB总是使用频段的多普勒频率观测值。这些测量方程及其偏导数矩阵构成如下：
  这些方程中卫星相对于接收机的速度从以下公式推导：
  其中，。
遍历完每一个观测值，返回定数方程数nv。

3. 注意事项

亏秩问题：这里与定位不同，构建雅可比矩阵时，就只有4个未知数，而定位时是有 NX 个。并且没有像定位那样为了防止亏秩而进行约束处理。
雅可比矩阵：多普勒定速方程中雅可比矩阵 G 与定位方程中的一样，前三行都是 ECEF 坐标系中由接收机指向卫星的单位观测矢量的反向。而由于转换矩阵 S 本身是一个正交单位矩阵，所以这里在计算 ECEF 中的视向量时，对 E 进行了转置处理。
这里构建的左端雅可比矩阵 H，也与定位方程中的一样，是大部分资料上的雅可比矩阵的转置。

点击查看代码

static int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
                 const nav_t *nav, const double *rr, const double *x,
                 const double *azel, const int *vsat, double err, double *v,
                 double *H)
{
   double freq,rate,pos[3],E[9],a[3],e[3],vs[3],cosel,sig;
   int i,j,nv=0;
   
   trace(3,"resdop  : n=%d\n",n);
   
   // 调用 ecef2pos() 函数，将接收机位置由 ECEF 转换为大地坐标系。
   // 调用 xyz2enu() 函数，计算到ENU坐标转换矩阵 E。
   ecef2pos(rr,pos); xyz2enu(pos,E);  
   
   for (i=0;i<n&&i<MAXOBS;i++) {
       
       freq=sat2freq(obs[i].sat,obs[i].code[0],nav);
       
       // 去除在定速时不可用的卫星。
       if (obs[i].D[0]==0.0||freq==0.0||!vsat[i]||norm(rs+3+i*6,3)<=0.0) {
           continue;
       }
       // 计算当前接收机位置下 ENU 中的视向量 e，然后转换得到 ECEF 中视向量的值。
       /* LOS (line-of-sight) vector in ECEF */    
       cosel=cos(azel[1+i*2]);
       a[0]=sin(azel[i*2])*cosel;
       a[1]=cos(azel[i*2])*cosel;
       a[2]=sin(azel[1+i*2]);              
       matmul("TN",3,1,3,1.0,E,a,0.0,e);   
       
       // 计算 ECEF 中卫星相对于接收机的速度，卫星速度rs[j+3+i*6]-传入的定速初值x[j]
       /* satellite velocity relative to receiver in ECEF */
       for (j=0;j<3;j++) {         
           vs[j]=rs[j+3+i*6]-x[j]; 
       }                           

       // 计算考虑了地球自转的用户和卫星之间的几何距离变化率
       /* range rate with earth rotation correction */
       rate=dot(vs,e,3)+OMGE/CLIGHT*(rs[4+i*6]*rr[0]+rs[1+i*6]*x[0]-
                 rs[3+i*6]*rr[1]-rs[  i*6]*x[1]);	//(F.6.29)

       /* Std of range rate error (m/s) */     
       sig=(err<=0.0)?1.0:err*CLIGHT/freq;     
       
       /* range rate residual (m/s) */
       v[nv]=(-obs[i].D[0]*CLIGHT/freq-(rate+x[3]-CLIGHT*dts[1+i*2]))/sig;
       
       /* design matrix */ // 构建左端项的雅可比矩阵
       for (j=0;j<4;j++) {
           H[j+nv*4]=((j<3)?-e[j]:1.0)/sig; // (E.6.28)
       }
       nv++; // 将观测方程数增 1
   }
   return nv;
}

8.3 estvel()：多普勒测速 ​

8.3.1 estvel()：函数主体 ​

8.3.2 estvel()：通过多普勒计算接收机速度 ​

8.1.3 resdop()：定速方程残差计算、雅可比矩阵构建 ​

8.3 estvel()：多普勒测速

8.3.1 estvel()：函数主体

8.3.2 estvel()：通过多普勒计算接收机速度

8.1.3 resdop()：定速方程残差计算、雅可比矩阵构建