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

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

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

udstate

static void udstate(rtk_t *rtk, const obsd_t *obs, const int *sat,
                    const int *iu, const int *ir, int ns, const nav_t *nav)

• 所在文件：Rtkpos.c
• 功能说明：对相对定位中的卡尔曼滤波状态量进行时间更新
• 参数说明：

 args:  IO  rtk_t *rtk:      rtk solution structure
        I   obsd_t *obs:     sat observations
        I   int *sat:        移动站、基站共视星列表
        I   int *iu          移动站共视星所在obs数组中的索引
        I   int *ir          基站共视星所在obs数组中的索引
        I   int ns:          共视星个数
        I   nav_t *nav:      卫星导航电文

• 调用关系：

• 处理过程：

1. 首先对位置、速度、加速度状态量做时间更新；
2. 其次对对流层参数做时间更新；
3. 对单差相位偏移状态量进行时间更新。

• 注意事项：

1. 实际第三步是对GLONASS的接收机误差进行处理，但是由于我并不关注GLONASS系统，所以和GLONASS相关的处理会略过。

udpos

static void udpos(rtk_t *rtk, double tt)

• 所在文件：Rtkpos.c
• 功能说明：对相对定位的位置、速度、加速度状态量进行时间更新
• 参数说明：

 args:  IO  rtk_t *rtk:      rtk solution structure
        I   double tt:       当前历元与前一历元的时间差

• 处理过程：

1. 如果现在的定位模式是PMODE_FIXED的状态，即移动站固定的定位模式，则调用initx函数，利用移动站的位置对前三个状态（即位置）进行初始化，将并其在协方差阵P的方差设置为一个较小的方差值（1E-8）。
2. 对第一个历元时刻的位置、速度、加速度状态量进行初始化，其中位置状态量初始化为单点定位所得到的位置值，方差设置为VAR_POS（60）。如果在配置中选择了将dynamics打开，则将速度状态量初始化为单点定位的速度值，方差设置为VAR_VEL（10），将加速度状态量初始化1E-6，方差设置为VAR_ACC（10）。
3. 如果配置中没有打开Rec dynamics，每次状态更新都将位置状态量赋值为单点定位的位置，方差设置为VAR_POS（60）。
4. 检查位置状态量的协方差，如果位置状态量的平均方差大于VAR_POS（60），则将位置状态量、速度状态量重置为单点定位值，加速度状态量重置为1E-6。根据3我们可以知道，这一步主要是检查dynamics打开时的情况。
5. 对位置、速度、加速度状态量按照卡尔曼滤波方程进行一步预测，即时间更新。实际上速度、位置、加速度之间的状态方程非常简单，即：
6. 该过程噪声建模为随机游走噪声。由于在配置中所设的“Receiver Accel horiz/vertical”是东北天坐标系下的，因此还需要将该过程噪声的方差阵由东北天坐标系转到地球固定坐标系。由此，便完成了位置、速度、加速度状态量的时间更新。

• 注意事项：

1. 如果没有选择打开Rec dynamics，实际上速度、加速度状态量并没有初始化，相当于仅使用了位置状态量。

udion

static void udion(rtk_t *rtk, double tt, double bl, const int *sat, int ns)

• 所在文件：rtkpos.c
• 功能说明：对电离层状态量进行时间更新
• 参数说明：

 args:  IO  rtk_t *rtk:      rtk solution structure
        I   double tt:       当前历元与前一历元的时间差
        I   double bl:       基线长度
        I   int *sat:        移动站、基站共视星列表
        I   int ns:          共视星个数

• 处理过程：

1. 在基线比较长的情况下（>10km）,由于电离层的误差，通过双差不能完全消除，因此需要考虑电离层的影响。在长基线的情况，可以在配置中选择电离层修正方式为IONOOPT_EST。这种配置下，会将垂直方向的单差电离层延迟添加到卡尔曼滤波状态量中。
2. 如果L1，L2载波相位的中断次数大于GAP_RESION（120），则将该卫星的单差电离层延迟状态量（后简称为电离层状态量）0。
3. 对所有共视星进行循环，如果该卫星的电离层状态量为0，则将其初始化为1E-6，方差为SQR(rtk->opt.std[1]*bl/1E4)，即根据在配置中所设置的参数和基线长度来决定；如果电离层状态量不为0，则在它的协方差阵上添加过程噪声，过程噪声大小由高度角，基线长度，过程噪声参数决定。

• 注意事项：

1. 在整个电离层状态量的时间更新中，实际上仅对电离层状态量为0的那些卫星进行了初始化，其余卫星的电离层状态量并没有变化，仅仅是在协方差阵中加入了过程噪声。

udtrop

static void udtrop(rtk_t *rtk, double tt, double bl)

• 所在文件：rtkpos.c
• 功能说明：对对流层状态量进行时间更新
• 参数说明：

 args:  IO  rtk_t *rtk:      rtk solution structure
        I   double tt:       当前历元与前一历元的时间差
        I   double bl:       基线长度

• 处理过程：

1. 在基线比较长（>10km）、或者是基线和移动站之间高度差较大的情况下，由于通过双差不能完全消除对流层误差，因此需要考虑对流层延迟的影响。在长基线的情况，可以在配置中选择对流层修正方式为Estiamte ZTD或者Estimate ZTD+Grad。如果选择是Estiamte ZTD，则会将基站、移动站天顶方向的对流层延迟($Z_b,Z_r$)加入卡尔曼滤波状态量，即新增2个状态量；如果选择Estimate ZTD+Grad，那么除了基线、移动天顶方向对流层延迟外，还会将东向、北向的对流层梯度系数($G_{E,r},G_{N,r},G_{E,b},G_{N,b}$)加入卡尔曼滤波状态量，即新增6个状态量，具体参见RTKlib manual的167页。
2. 该函数首先检查$Z_b,Z_r$状态量是否为0，如果为0，则初始化为INIT_ZWD(0.15m)，如果是在Estimate ZTD+Grad配置下，还会初始化$G_{E,r},G_{N,r},G_{E,b},G_{N,b}$状态量为1E-6；如果$Z_b,Z_r$状态量不为0，则在将其协方差阵加上过程噪声。

• 注意事项：

1. 在整个对流层状态量的时间更新中，实际上仅对对流层状态量为0的那些卫星进行了初始化，其余卫星的对流层状态量并没有变化，仅仅是在协方差阵中加入了过程噪声。

udbias

static void udbias(rtk_t *rtk, double tt, const obsd_t *obs, const int *sat,
                   const int *iu, const int *ir, int ns, const nav_t *nav)

• 所在文件：rtkpos.c
• 功能说明：对单差整周模糊度状态量进行时间更新
• 参数说明：

 args:  IO  rtk_t *rtk:      rtk solution structure
        I   double tt:       当前历元与前一历元的时间差
        I   obsd_t *obs:     sat observations
        I   int *sat:        移动站、基站共视星列表
        I   int *iu          移动站共视星所在obs数组中的索引
        I   int *ir          基站共视星所在obs数组中的索引
        I   int ns:          共视星个数
        I   nav_t *nav:      卫星导航电文

• 处理过程：

1. 首先是对所有共视星进行周跳检测：调用了detslp_ll函数，根据LLI来判断基站和移动站周跳；然后调用detslp_gf_L1L2、detslp_gf_L1L5函数，利用几何无关组合进行周跳检测；最后调用detslp_dop函数，利用多普勒进行周跳检测（但该函数由于时间跳变的原因，暂未使用）。对RTKlib的周跳检测函数，我专门写了一篇博客解析，RTKlib源码解析：ppp和rtkpost中的周跳检测函数
2. 对所有卫星进行循环，判断是否需要重置单差相位偏移状态量。如果所配置的AR的模式为instantaneous，或者卫星载波相位的中断次数大于配置中所设置的最大次数，则将单差相位偏移状态量重置为0。
3. 对共视星进行循环，对单差相位偏移状态量的协方差阵加入过程噪声，如果发现有周跳或者异常值，则单差相位偏移状态量重置为0。
4. 对共视星进行循环，利用“单差伪距”和“单差载波相位”计算一个“单差相位偏移”估计值，来对单差相位偏移状态量进行更新。由于如果忽略伪距误差，那么伪距减去载波相位，则应该是载波相位（m）的相位偏移(m)，所以这里计算的是一个大致的相位偏移bias[i]。如果配置为无电离层组合，计算则按照无电离层组合的方式来计算。最后，仅计算所有有效星（单差相位偏移状态不为0）的offset = sum of (bias - phase-bias)。其实就是计算每颗有效星bias[i]与单单差相位偏移状态量的偏差，然后把所有有效星的这个偏差值加起来，之后会除以有效星的个数，最终就是求一个偏差平均值rtk->com_bias。
5. 利用4求得的bias[i] - rtk->com_bias，来对没有进行初始化的卫星，进行单差相位偏移状态量的初始化。

• 注意事项：

1. 在整个单差相位偏移状态量的时间更新中，实际上仅对单差相位偏移状态量为0的那些卫星进行了初始化，其余卫星的单差相位偏移状态量并没有变化，仅仅是在协方差阵中加入了过程噪声。