RTKlib相对定位源码解析： ddres函数

最近阅读RTKlib开源代码，非常感谢“塔奇克敲代码”博主的博客（RTKLIB源码解析——单点定位），他将单点定位部分整理成函数小卡片，为我理解RTKlib提供了很大的帮助。我参照他的格式，记录整理对相对定位部分的个人理解。由于刚开始接触卫星定位，所以可能有理解不到位的地方，还请诸位指正。

本文对ddres函数，以及其中的ionmapf、prectrop函数进行了解析。

我所基于的代码版本是RTKlib 2.4.3的一个拓展版本RTKexplore Demo5，这个版本主要针对低成本的GNSS定位。该版本整体算法并未做较大更改，只是针对低成本接收机进行了完善。

ddres

static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt, const double *x,
                 const double *P, const int *sat, double *y, double *e,
                 double *azel, const int *iu, const int *ir, int ns, double *v,
                 double *H, double *R, int *vflg)

• 所在文件：Rtkpos.c
• 功能说明：计算双差残差和量测矩阵H
• 参数说明：

        O rtk->ssat[i].resp[j] = residual pseudorange error
        O rtk->ssat[i].resc[j] = residual carrier phase error
        I rtk->rb= base location
        I nav  = sat nav data
        I dt = time diff between base and rover observations (usually 0)
        I x = rover pos & vel and sat phase biases (float solution)
        I P = error covariance matrix of float states
        I sat = list of common sats
        I y = zero diff residuals (code and phase, base and rover)
        I e = line of sight unit vectors to sats
        I azel = [az, el] to sats
        I iu,ir = user and ref indices to sats
        I ns = # of sats
        O v = double diff innovations (measurement-model) (phase and code)
        O H = linearized translation from innovations to states (az/el to sats)
        O R = measurement error covariances
        O vflg = bit encoded list of sats used for each double diff  */

• 调用关系：

• 处理过程：

1. 调用baseline函数计算基站和移动站间的基线长度；并将基站位置、移动站位置分别从ECEF转到WGS84坐标系（纬度、经度、高度）；
2. 对一些中间变量进行初始化，并将双差伪距残差和双差载波相位残差初始化为0；
3. 如果卡尔曼滤波器包含电离层状态量，调用ionmapf函数分别计算基站和移动站处的投影函数，然后求其平均值作为“单差垂直电离层延迟状态量”的投影系数。ionmapf函数中的计算公式在RTKlib manual 152页（E.5.19），不过该公式应该是除以${f_i}^2$，而不是$f_i$；
4. 如果卡尔曼滤波中包含对流层状态量，调用prectrop函数计算基站和移动站的对流层延迟湿分量，存在tropu[i]和tropur[i]中；
5. 接下来对各个系统以及载波相位、伪距分别进行循环处理：
   1）首先找到高度角最大的卫星作为参考星；
   2）利用zdres函数中计算的载波相位/伪距无差残差，计算每颗卫星的载波相位/伪距双差残差，并对观测矩阵H中和位置状态量相关的部分进行赋值；
   3）如果卡尔曼滤波中包含电离层状态量，以GPS L1为基准（lam_carr[0]为L1的波长，该全局变量的初始化在rtkcom.c中），计算当前频率的电离层单差延迟量，需要注意伪距和载波相位的电离层延迟大小相同，但是符号相反。然后，从2）计算的载波相位/伪距双差残差值中扣除该卫星的双差电离层延迟量，并对观测方程H中和电离层状态量相关的部分进行赋值；
   4）如果卡尔曼滤波中包含对流层状态量，从载波相位/伪距双差残差值中扣除对流层双差湿分量，并对观测方程H中和对流层状态量相关的部分进行赋值；
   5）如果不是无电离层组合，从载波相位双差残差中扣除双差模糊度部分（即phase-bias），并对H矩阵中和模糊度相关的部分进行赋值。
   6）最后将载波相位/伪距残差存入对应变量中。
6. 对载波相位/伪距双差残差进行检查，如果过大超过阈值，则认为无效；
7. 调用varerr函数计算载波相位/伪距单差的量测噪声方差；
8. 设置卫星有效标志；
9. 如果是动基线的模式，增加基线长度约束；
10. 调用ddcov计算载波相位/伪距双差量测噪声。

• 注意事项：

1. ddres感觉上是RTKlibk卡尔曼滤波中最重要的函数，通过这个函数其实主要是做了两件事：（1）计算了卡尔曼滤波中的$y_k-h（X_{k/k-1})$，即载波相位/伪距双差残差；（2）计算好了量测矩阵H和量测噪声。这部分对应RTKlib manual 的167-168页，可以找到所有的量测方程。卡尔曼滤波所需要的所有量到此已经全部准备好，下一步则可以进行量测更新。
2. RTKlib卡尔曼滤波的步骤和我之前的常用的步骤不太相同，通常我们会先计算观测值的一步预测值，即$h（X_{k/k-1})$，然后再利用观测值减去其一步预测值来得到残差，即$y_k-h（X_{k/k-1})$。而RTKlib是先在zdres函数（计算载波相位/伪距无差残差）中，通过利用移动站的位置状态量计算了一个伪距/载波相位估计值，再利用这个伪距/载波相位估计值和实际的伪距/载波相位量测值相减，得到载波相位/伪距的无差残差。同时，基站也通过zdres函数得到了载波相位/伪距的误差残差，最后通过ddres函数计算得到载波相位/伪距的双差残差。虽然思路不同，但是最终的结果都是一致的。
3. 需要注意的是，在ddres函数中仅扣除了对流层延迟的湿分量，这是因为在zdres函数计算载波相位/伪距无差残差时已经将干分量扣除了。

• 我的疑惑：

1. 第3步中,为什么“单差垂直电离层延迟状态量”的投影系数是基站和移动站投影系数的平均值？

ionmapf

extern double ionmapf(const double *pos, const double *azel)

• 所在文件：Rtkcmn.c
• 功能说明：计算电离层投影系数
• 参数说明：

/* ionosphere mapping function -------------------------------------------------
* compute ionospheric delay mapping function by single layer model
* args   : double *pos      I   receiver position {lat,lon,h} (rad,m)
*          double *azel     I   azimuth/elevation angle {az,el} (rad)
* return : ionospheric mapping function
*-----------------------------------------------------------------------------*/
extern double ionmapf(const double *pos, const double *azel)
{
    if (pos[2]>=HION) return 1.0;
    return 1.0/cos(asin((RE_WGS84+pos[2])/(RE_WGS84+HION)*sin(PI/2.0-azel[1])));
}

• 处理过程：

1. 这部分对应的公式在RTKlib manual的151页，电离层模型部。本函数实际是在计算电离层延迟的投影系数，即$1/cos(z^{\prime })$。

prectrop

static double prectrop(gtime_t time, const double *pos, int r,
                       const double *azel, const prcopt_t *opt, const double *x,
                       double *dtdx)

• 所在文件：Rtkpos.c
• 功能说明：精密对流层模型，计算对流层湿分量以及东向、北向梯度因子
• 参数说明：

I      gtime_t time     当前历元时间
I      double *pos      基站/移动站位置：纬、经、高
I      int r            0表示移动站，1表示基站，用来计算对流层状态量在卡尔曼滤波状态矢量的索引
I      double *azel     高度角，方位角
I      prcopt_t *opt    处理选项
I      double *x        卡尔曼滤波状态矢量
IO     double *dtdx     对流层湿分量、北向和东向梯度因子
*-----------------------------------------------------------------------------*/
static double prectrop(gtime_t time, const double *pos, int r,
                       const double *azel, const prcopt_t *opt, const double *x,
                       double *dtdx)
{
    double m_w=0.0,cotz,grad_n,grad_e;
    int i=IT(r,opt);
    
    /* wet mapping function */
    tropmapf(time,pos,azel,&m_w);
    
    if (opt->tropopt>=TROPOPT_ESTG&&azel[1]>0.0) {
        
        /* m_w=m_0+m_0*cot(el)*(Gn*cos(az)+Ge*sin(az)): ref [6] */
        cotz=1.0/tan(azel[1]);
        grad_n=m_w*cotz*cos(azel[0]);
        grad_e=m_w*cotz*sin(azel[0]);
        m_w+=grad_n*x[i+1]+grad_e*x[i+2];
        dtdx[1]=grad_n*x[i];
        dtdx[2]=grad_e*x[i];
    }
    else dtdx[1]=dtdx[2]=0.0;
    dtdx[0]=m_w;
    return m_w*x[i];

• 处理过程：

1. 首先找到对流层天顶方向延迟状态量的索引；
2. 调用tropmap函数计算湿分量投影函数，tropmap函数在之前解析zdres函数的博客里已经进行了解析，所以这里不再赘述；
3. 按照Rtklib manual 150页精密对流层模型的公式计算是湿延迟。

• 我的疑惑：

1. 该函数返回值为m_w*x[i]，如果按照manual 150页公式（E.5.6），这里的X[i]应该是天顶方向的湿延迟，而不是总延迟。但是在167页公式（E.7.25）表示是将天顶总延迟作为状态量，有点奇怪。