rtklib三之relpos rtkpo庖丁解牛

文章目录

写在前面
- 为什么relpos看起来这么晦涩
- 怎么读懂
Top逻辑层
- 卫星位置解算
- 基准站无差观测
- 共视星
- 卡尔曼滤波的时间更新
- 计算移动站的无差观测
- 双差观测及观测矩阵
- 量测更新
- 固定解
总结

写在前面

为什么relpos看起来这么晦涩

relpos这个函数是rtklib高精度相对定位的核心函数，完成了相对定位的绝大部分工作。虽说其核心算法不外乎用于解算浮点解的EKF和固定模糊度的lambda算法，但真正的c代码实现却真的让人却步。之所以看起来有点儿费劲，个人认为有以下几点原因：

首先relpos代码长度250行左右，绝对是一个大函数。

总之，十行以内是整洁的函数比较合适的长度，若没有特殊情况，我们最好将单个函数控制在十行以内.《代码整洁之道》

C语言写的，很多很多的变量写在函数开始，当然这些在读函数的时候可以先不关注
relpos中调用的子函数也很长，如ddres
relpos的子函数中包含了GNSS领域的domain knowledge，如无差观测生成，双差观测量和雅克比矩阵的计算,也包含一些比较专业的算法如EKF算法和用于解决混合整数最小二乘的算法，如果不懂这些知识强看代码也会很头痛。
函数似乎没有遵循讲故事原则，即代码都在一个逻辑层次。个人愚见，按逻辑层次分层写出来的relpos可能长下边这个模样：

void relpos() {ekf();    // 计算浮点解lambda(); // 计算固定解
}
void ekf(){temporal_update(); // 卡尔曼滤波的时间更新meas_update();     // 卡尔曼滤波的量测更新
}
...

怎么读懂

一个原则：从上到下的阅读策略。具体说来就是无论是relpos还是其子函数，阅读当前函数时，对于子函数只关注（且读懂）其输入输出，其内容不做深究。
只有当输入输出无法从上下文和注释推测时，才进行最小限度的子函数展开阅读。

Top逻辑层

按照从上向下的原则，首先看relpos的输入输出，观测数据obs和星历nav当然是必须的，nu和nr分别是移动站和基准站的观测值条数，rtk这个结构有多种用途，作为输出结果的承载结构，rtk->opt中含有解算参数，另外其中还包含了过程参数如浮点解，固定解，及其对应的P阵，这些既是输入又是输出。

/* relpos()relative positioning ------------------------------------------------------*  args:  rtk      IO      gps solution structureobs      I       satellite observationsnu       I       # of user observations (rover)nr       I       # of ref observations  (base)nav      I       satellite navigation data*/

还是先上张图，不过个人觉得作用不是很大~

卫星位置解算

/* compute satellite positions, velocities and clocks */satposs(time,obs,n,nav,opt->sateph,rs,dts,var,svh);

返回值rs是卫星的位置速度数组，rs=mat(6,n),其返回的数据结构如下：

字段	类型	数据描述	单位
x1x^1x1	double	卫星位置 xxx	mmm
y1y^1y1	double	卫星位置 yyy	mmm
z1z^1z1	double	卫星位置 zzz	mmm
vx1v_x^1vx1	double	卫星速度vxv_xvx	m/sm/sm/s
vy1v_y^1vy1	double	卫星速度vyv_yvy	m/sm/sm/s
vz1v_z^1vz1	double	卫星速度vzv_zvz	m/sm/sm/s
…	double[6]	下一卫星的位置速度

返回值dts是卫星的钟差钟漂，dts=mat(2,n),其返回的数据结构如下：

字段	类型	数据描述	单位
dt1dt_1dt1	double	卫星位置钟差	sss
dt1˙\dot{dt_1}dt1˙	double	卫星钟漂	s/ss/ss/s
…	double[2]	下一卫星的钟差钟漂

基准站无差观测

我们知道rtklib中浮点解是用ekf算法计算的，ekf的量测更新要有量测向量和线性化的量测矩阵，从无差观测值开始，量测向量的计算开始了。

if (!zdres(1,obs+nu,nr,rs+nu*6,dts+nu*2,var+nu,svh+nu,nav,rtk->rb,opt,1,y+nu*nf*2,e+nu*3,azel+nu*2)) {...
}

返回值y+nu*nf*2是基准站的无差观测向量，y=mat(nf*2,n);,nf在iono-free模型下是1，我们就以这个模型为例，其返回的数据结构如下：

字段	类型	数据描述	单位
λϕ1−rb1\lambda \phi_1-r_b^1λϕ1−rb1	double	phase-r	mmm
P1−rb1P_1-r_b^1P1−rb1	double	code-r	mmm
…	double[2]	下一卫星对应的无差观测

λ\lambdaλ为消电离层组合观测的波长

返回值e+nu*3是基准站的视线向量，e=mat(3,n);,其返回的数据结构如下：

字段	类型	数据描述	单位
exe_xex	double	xs1−xbr\frac{x_s^1-x_b}{r}rxs1−xb	NA
eye_yey	double	ys1−ybr\frac{y_s^1-y_b}{r}rys1−yb	NA
eze_zez	double	zs1−zbr\frac{z_s^1-z_b}{r}rzs1−zb	NA
…	double[3]	下一卫星对应的视线向量

其中，(xsi,ysi,zsi)(x_s^i,y_s^i,z_s^i)(xsi,ysi,zsi)为卫星iii的位置，(xb,yb,zb)(x_b,y_b,z_b)(xb,yb,zb)为基准站坐标，均为ecef坐标系。ri=(xsi−xb)2+(ysi−yb)2+(zsi−zb)2r_i=\sqrt{(x_s^i-x_b)^2+(y_s^i-y_b)^2+(z_s^i-z_b)^2}ri=(xsi−xb)2+(ysi−yb)2+(zsi−zb)2。

返回值azel+nu*2是基准站的方位角仰角向量，azel=zeros(2,n);,其返回的数据结构如下：

字段	类型	数据描述	单位
azazaz	double	方位角	[0,2π)[0,2\pi)[0,2π)
elelel	double	仰角	[−π2,π2][\frac{-\pi}{2},\frac{\pi}{2}][2−π,2π]
…	double[2]	下一卫星对应的方位角仰角

共视星

共视星是基准站和移动站能同时“看到”的卫星，只有基准站或者移动站能看到的卫星不参与高精度相对定位。

if ((ns=selsat(obs,azel,nu,nr,opt,sat,iu,ir))<=0) {

用以例子来表示selsat输入与输出之间的关系，
obs中sat依次为[1,2,3,4,5,7,9,23, 2,3,4,6,23,32]
这个例子中nu为8，nr为6，函数输出为，

sat	iu	ir
2	1	8
3	2	9
4	3	10
23	7	12

卡尔曼滤波的时间更新

udstate(rtk,obs,sat,iu,ir,ns,nav);

从时间更新函数udstate下边的函数声明可以看出，此函数只有一个输出rtk，

时间更新中的函数都以ud开头,表示update

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)

这部分的算法部分在 rtklib相对定位算法中已有介绍，因此在top逻辑层中不再详细介绍。
输出为rtk，x, P根据卡尔曼滤波的时间更新方程，做了一步预测。

计算移动站的无差观测

if (!zdres(0,obs,nu,rs,dts,var,svh,nav,xp,opt,0,y,e,azel)) {

此函数与上边计算基站的无差观测为同一个函数，只不过总体说来有两点不同：

站点坐标使用的是xp,这个是kalman滤波时间更新中得到的新坐标，
所有的输出，如y,e,azel，起始存储位置为0，而上边的基准站无差观测时为nu*size

双差观测及观测矩阵

if ((nv=ddres(rtk,nav,obs,dt,xp,Pp,sat,y,e,azel,iu,ir,ns,v,H,R,vflg))<1)

上边函数最重要的三个输出v,H,R都是卡尔曼滤波量测更新所必须的。
这里多说一句，rtklib相对定位算法中我们从算法上介绍过，ekf的量测向量是双差载波和双差伪距，这里为什么观测变成双差残差了呢？对ekf熟悉的可以略过下边的解释了，如果有疑惑，那么请看

量测方程线性化
熟悉ekf的此节可以略过，不要浪费时间
例子，存在非线性量测方程，y=h(x)=x2y=h(x)=x^2y=h(x)=x2，为了应用kalman滤波，我们使用泰勒展开将她线性化
y=x02+2x0(x−x0)y=x_0^2+2x_0(x-x_0)y=x02+2x0(x−x0)
即，2x0(x−x0)=H(x−x0)=y−x022x_0(x-x_0)=H(x-x0)=y-x_0^22x0(x−x0)=H(x−x0)=y−x02
就是这样， hhh变成了HHH，yyy变成了y−x02y-x_0^2y−x02

v[nv]=(y[f+iu[i]*nf*2]-y[f+ir[i]*nf*2])-(y[f+iu[j]*nf*2]-y[f+ir[j]*nf*2]);

这个式子就是求双差残差，带入前边的无差观测即可。
本函数详细内容不在Top逻辑层介绍了，下边专门介绍一下。

量测更新

到这里，量测更新的观测向量，雅可比矩阵，噪声矩阵都有了，万事俱备只欠东风，下边这个函数完成的就是kalman滤波的量测更新。

if ((info=filter(xp,Pp,H,v,R,rtk->nx,nv))) {

上述函数执行之后，
xp=x+K∗vPp=(I−K∗H′)∗Pxp=x+K*v\\ Pp=(I-K*H')*P xp=x+K∗vPp=(I−K∗H′)∗P
其实到这里，相对定位的浮点解已经出来了！！！

固定解

下边才是重头戏，单差整周模糊度的固定，只有固定了模糊度，才能得到固定解，也就是真正意义上的高精度解。

/* resolve integer ambiguity by LAMBDA */else if (stat!=SOLQ_NONE&&resamb_LAMBDA(rtk,bias,xa)>1) {

rtklib中的整周固定解通过lambda算法求解，整周固定解的求解过程其数学本质是一个混合整数最小二乘问题，即待求解的变量中有一部分是浮点数，一部分是整数（单差模糊度）。如果不考虑计算效率，那么我们通过估计误差设置一个搜索区间，带入所有模糊度的组合，取残差最小的那一组就是我们要求解的整周模糊度。但是，这个计算量无疑是一个天文数字！想象一下，我们有14*3=42个单差模糊度要求解，每个浮点周围我们取10个待算整数，那么组合数量就是104210^{42}1042，即便是后处理应用，这个量级也挺吓人的。
所以，lambda算法本质上是解决计算速度的问题，relpos的核心算法或者之一，后边单独介绍一下。
这里我们需要知道，通过这个函数之后固定解rtk->xa得到了。

总结

现在不结，感觉有点长了。如果对照代码看会发现还有地方没提到。是的，总体来说从top level来说 relpos这个函数已经讲完了。但是有的细节确实是没有提及，从后向前说：

hold and fix: 在取得固定解之后再做一次kalman滤波的量测更新
固定解获取后，计算双差残差，进行有效性验证
浮点解解算完成后，计算双差残差，进行有效性验证
对于每个函数的实现细节都没有提及

这些将在后续章节陆续写出来。