文章目录
写在前面
为什么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 structure
obs I satellite observations
nu 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)
,其返回的数据结构如下:
字段 | 类型 | 数据描述 | 单位 |
---|---|---|---|
double | 卫星位置 | ||
double | 卫星位置 | ||
double | 卫星位置 | ||
double | 卫星速度 | ||
double | 卫星速度 | ||
double | 卫星速度 | ||
… | double[6] | 下一卫星的位置速度 |
返回值dts
是卫星的钟差钟漂,dts=mat(2,n)
,其返回的数据结构如下:
字段 | 类型 | 数据描述 | 单位 |
---|---|---|---|
double | 卫星位置钟差 | ||
double | 卫星钟漂 | ||
… | 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,我们就以这个模型为例,其返回的数据结构如下:
字段 | 类型 | 数据描述 | 单位 |
---|---|---|---|
double | phase-r | ||
double | code-r | ||
… | double[2] | 下一卫星对应的无差观测 |
为消电离层组合观测的波长
返回值e+nu*3
是基准站的视线向量,e=mat(3,n);
,其返回的数据结构如下:
字段 | 类型 | 数据描述 | 单位 |
---|---|---|---|
double | NA | ||
double | NA | ||
double | NA | ||
… | double[3] | 下一卫星对应的视线向量 |
其中, 为卫星 的位置, 为基准站坐标,均为ecef坐标系。 。
返回值azel+nu*2
是基准站的方位角仰角向量,azel=zeros(2,n);
,其返回的数据结构如下:
字段 | 类型 | 数据描述 | 单位 |
---|---|---|---|
double | 方位角 | ||
double | 仰角 | ||
… | 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的此节可以略过,不要浪费时间
例子,存在非线性量测方程, ,为了应用kalman滤波,我们使用泰勒展开将她线性化
即,
就是这样, 变成了 , 变成了
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))) {
上述函数执行之后,
其实到这里,相对定位的浮点解已经出来了!!!
固定解
下边才是重头戏,单差整周模糊度的固定,只有固定了模糊度,才能得到固定解,也就是真正意义上的高精度解。
/* resolve integer ambiguity by LAMBDA */
else if (stat!=SOLQ_NONE&&resamb_LAMBDA(rtk,bias,xa)>1) {
rtklib中的整周固定解通过lambda算法求解,整周固定解的求解过程其数学本质是一个混合整数最小二乘问题,即待求解的变量中有一部分是浮点数,一部分是整数(单差模糊度)。如果不考虑计算效率,那么我们通过估计误差设置一个搜索区间,带入所有模糊度的组合,取残差最小的那一组就是我们要求解的整周模糊度。但是,这个计算量无疑是一个天文数字!想象一下,我们有14*3=42个单差模糊度要求解,每个浮点周围我们取10个待算整数,那么组合数量就是
,即便是后处理应用,这个量级也挺吓人的。
所以,lambda算法本质上是解决计算速度的问题,relpos的核心算法或者之一,后边单独介绍一下。
这里我们需要知道,通过这个函数之后固定解rtk->xa
得到了。
总结
现在不结,感觉有点长了。如果对照代码看会发现还有地方没提到。是的,总体来说从top level来说 relpos这个函数已经讲完了。但是有的细节确实是没有提及,从后向前说:
- hold and fix: 在取得固定解之后再做一次kalman滤波的量测更新
- 固定解获取后,计算双差残差,进行有效性验证
- 浮点解解算完成后,计算双差残差,进行有效性验证
- 对于每个函数的实现细节都没有提及
这些将在后续章节陆续写出来。