rtklib一之带你一步一步读懂rtklib 单点定位代码及算法

1 调用关系图

下图是rtklib的单点定位的调用关系图，入口函数为pntpos（位于pntpos.c）。

2 单点定位程序流程

从上图中可以看出，pntpos这个函数做了3件事，基本上分别对应了三个函数。

2.1 计算卫星位置和速度

函数estpos完成了卫星位置的计算，这一部分代码基本是对着公式敲代码。

2.2 位置估计

函数estpos用来完成位置估计，使用的算法是加权最小二乘法，位置估计如果失败会调用raim算法重新进行位置估计，前提是收星数满足至少六颗卫星，并且对应的解算参数要设置opt->posopt[4]=1;。
位置估计的本质是解算下边的方程组：
$P_1=\sqrt{(x-x_1^s)^2+(y-y_1^s)^2+(z-z_1^s)^2}+cdt_u+I_1+T_1\\ P_2=\sqrt{(x-x_2^s)^2+(y-y_2^s)^2+(z-z_2^s)^2}+cdt_u+I_2+T_2\\ ...\\ P_m=\sqrt{(x-x_m^s)^2+(y-y_m^s)^2+(z-z_m^s)^2}+cdt_u+I_m+T_m$
其中，
$P_i$是$i$卫星对应的伪距观测量，从观测数据中获得，为已知量，
$(x_i^s,y_i^s,z_i^s)$为$i$卫星对应的位置，在2.1中已经计算得到，也是已知量，
$c$为光速，
$I_i$为$i$卫星对应的电离层延迟，通过建模得到，下边的第3章中介绍，这里看做已知量，
$T_i$为$i$卫星对应的对流层延迟，通过建模得到，下边的第3章中介绍，这里看做已知量，
那么方程组中只剩下$(x,y,z,dt_u)$四个未知数了（分别是接收机位置和接收机钟差），解算四个未知数需要至少四个方程，所以要完成单点定位解算至少需要四颗卫星。

这里特别说明一下，以上的方程和结论是建立在单系统（如单GPS，或者单北斗）解算的基础上的，如果是多系统混合解算那么，就不应该只有一个接收机钟差。实际上每增加一个解算系统，就会增加一个未知数，相应的观测卫星的数据也至少加1才能解算。

下边看看estpos中是怎么解算这个方程组的

2.2.1 模型线性化和观测值补偿

函数rescode几乎是位置估算中最重要的一个函数，完成了解算模型的线性化，以及伪距补偿，加权参数估计等工作，是最小二乘计算的基础。可以这么说，最小二乘中用到的所有参数（除卫星位置外）都是在这个函数里计算的。

2.2.1.1 伪距修正

这个函数用于修正伪距，如双拼伪距融合等，具体方法见3

if ((P=prange(obs+i,nav,azel+i*2,iter,opt,&vmeas))==0.0) continue;

2.2.1.2 电离层修正

此处完成电离层修正dion，当然还有电离层修正的方差vion，vion会在计算权重时用到。

/* ionospheric corrections */
if (!ionocorr(obs[i].time,nav,obs[i].sat,pos,azel+i*2,
                iter>0?opt->ionoopt:IONOOPT_BRDC,&dion,&vion)) continue;
        
/* GPS-L1 -> L1/B1 */
if ((lam_L1=nav->lam[obs[i].sat-1][0])>0.0) {
    dion*=SQR(lam_L1/lam_carr[0]);
}

2.2.1.3 对流层修正

此处完成电离层修正dtrop，当然还有对流层修正的方差vtrp，vtrp会在计算权重时用到。

if (!tropcorr(obs[i].time,nav,pos,azel+i*2,
             iter>0?opt->tropopt:TROPOPT_SAAS,&dtrp,&vtrp)) {
    continue;
}

另外的部分是模型线性化和观测值方差计算，这个可以在3中介绍。

2.2.2 加权

下边的代码用来给观测值对应的$H$阵相应的行乘一个权值，可以看到用的是var[j]这个变量做的加权，这个参数的计算见3

for (j=0;j<nv;j++) {
    sig=sqrt(var[j]);
    v[j]/=sig;
    for (k=0;k<NX;k++) H[k+j*NX]/=sig;
}

2.2.3 最小二乘求解

下边的代码用来计算最小二乘解，
$x=(A*A')^{-1}*A*y$

if ((info=lsq(H,v,NX,nv,dx,Q))) {
    sprintf(msg,"lsq error info=%d",info);
    break;
}

2.2.4 结果检验valsol

解算结果是否合格要看以下两个检验是不是通过

2.2.4.1 残差检验

残差检验用的是卡方检验，其实这个检验对低精度板卡来说有点严格，如果解算的对象是低精度板卡这个检测应该禁掉，否则很多解算结果是通不过的。

if (nv>nx&&vv>chisqr[nv-nx-1]) {
    trace(3,"chi-square error nv=%d vv=%.1f cs=%.1f\n",nv,vv,chisqr[nv-nx-1;
    return 0;
}

2.2.4.2 dop值检验

下边第一行代码是dop值的计算，从这个函数的参数也可以看出，dop仅与卫星的几何分布有关系，azels为卫星们的方位角仰角。

dops(ns,azels,opt->elmin,dop)
if (dop[0]<=0.0||dop[0]>opt->maxgdop) {
    sprintf(msg,"gdop error nv=%d gdop=%.1f",nv,dop[0]);
    return 0;
}

2.3 速度估计

函数estvel用来完成速度估计，前提是数据中要有多普勒频移观测值（即结构体obsd_t中的D），如果没有则解算失败，如果这种情况下仍然必须要获取速度，只能通过位置进行估计了。

typedef struct {        /* observation data record */
...
    float  D[NFREQ+NEXOBS]; /* observation data doppler frequency (Hz) */
} obsd_t;

速度估计的本质是解算下边的方程组：
$d_1=\vec{e_1}\cdot(\vec{v_{s1}}-\vec{v})+c\dot{b}\\ d_2=\vec{e_2}\cdot(\vec{v_{s2}}-\vec{v})+c\dot{b}\\ ...\\ d_m=\vec{e_m}\cdot(\vec{v_{sm}}-\vec{v})+c\dot{b}\\$
其中，
$d_i$是$i$卫星的多普勒观测量，即obs_t中的D，
$\vec{v_{si}}$是卫星速度向量，
$\vec{v}$是接收机速度，为未知量
$\dot{b}$为接收机钟漂，为未知量
$\vec{e_i}$为卫星$i$的方向余弦矢量，由于卫星的位置和接收机位置已经在前边解算出来了，那么这个方向余弦也是已知的
可以看出速度的求解是求解一组四元一次线性方程，这点与位置解算不同，无需迭代。

3 单点定位中的关键函数对应算法

3.1 求解非线性方程组

卫星位置解算的本质是求解一组非线性方程，没有学过数值计算的人可能理解起来稍微有一点点费解，这里从一个一维的例子扩展到高维。学过数值计算的可以跳过这一节了。
首先看下边这么一个非线性方程：
$x^2=4$
下边我们通过线性化和迭代的方法解算一下这个方程，首先通过泰勒级数线性化，泰勒展开需要在某点展开，首先令$x_0=1$。
$x^2=x_0^2+2x_0(x-x_0)=1+2(x-1)=4$
方程变成了一个线性方程，解算得$x=2.5$
然后我们令$x=2.5$,
$2.5^2+2*2.5(x-2.5)=4$
解算得到，$x=2.05$,就这么一直迭代解算下去，我们可以看到迭代次数和解算结果的关系，可以看到只需要经过四次迭代，几乎得到正确结果了。

迭代次数	解算结果	残差
1	2.5	-2.25
2	2.05	-0.2025
3	2.000609756	-0.00244
4	2.000000093	-3.7e-7

现在可以扩展到高维了，四个未知数的情形和上边几乎是一模一样的，唯一不同的是线性化时，使用的是多元函数的泰勒展开而已,迭代多少次合适呢？是不是迭代次数越多越好呢？rtklib中使用以下方法：

for (i=0;i<MAXITR;i++) {
    ...
    if (norm(dx,NX)<1E-4) {
        ...

从上边的代码可以看出，rtklib通过限制迭代次数（最大十次）和残差范数来结束迭代，当残差范数小于1e-4时，结束迭代，一般6次左右的迭代就能结束迭代了。

3.2 RAIM算法

RAIM即Receiver Autonomous Integrity Monitoring，FDE即Fault Detection Exclusion,是接收机自主完好性监测和故障检测与排除。rtklib中通过每次排除一颗卫星进行解算，然后选取残差最小的一次解算结果作为最终的解算结果，此次对应的卫星就是故障卫星，这个方法不适合有超过一个故障卫星的场合。
以下是RAIM算法的一个直观解释，
老师让五个同学测量黑板的长度，下边是测量结果：

姓名	小明	jack	李雷	韩梅梅	贾宝玉
测量值	2.01	1.98	2.00	2.12	2.4

老师一平均2.102米，这就是最小二乘法，老师觉得李雷学习成绩好，做事认真应该更相信李雷，贾宝玉整天无所事事，不靠谱，不能信他，于是给他们加了信任度。

姓名	小明	jack	李雷	韩梅梅	贾宝玉
测量值	2.01	1.98	2.00	2.12	2.4
权	2	2	3	2	1

老师这次一平均，哦，2.062似乎结果更准了，这就叫加权最小二乘。
什么是RAIM呢？我们每次剔除一个学生的结果，算一下加权平均。

姓名	小明	jack	李雷	韩梅梅	贾宝玉
测量值	2.01	1.98	2.00	2.12	2.4
权	2	2	3	2	1
剔除后估计值	2.075	2.0825	2.0886	2.0475	2.0244

表中可以看出raim的结果，贾宝玉对应的测量值2.4被剔除，估计值为2.0244，
残差的计算这里省略。

3.3 伪距修正

以下是消电离层组合，

/* iono-free combination */
PC=(gamma*P1-P2)/(gamma-1.0);

$PC=\frac{\gamma P1-P2}{\gamma-1.0} \\ =\frac{f_1^2P1-f_2^2P2}{f_1^2-f_2^2}\\ =\frac{f_1^2 \rho-f_2^2\rho+f_1^2 ion_1-f_2^2ion_2+f_1^2 trop-f_2^2trop}{f_1^2-f_2^2}$
因为gps的电离层延迟与频率相关，其一阶近似项为，
$ion_i=\frac{A}{f_i^2}$
其中$A$为常数，因此$f_1^2 ion_1-f_2^2ion_2=0$.
所以，
$PC =\frac{f_1^2 \rho-f_2^2\rho+f_1^2 ion_1-f_2^2ion_2+f_1^2 trop-f_2^2trop}{f_1^2-f_2^2}\\ =\frac{f_1^2 \rho-f_2^2\rho+f_1^2 trop-f_2^2trop}{f_1^2-f_2^2}\\ =\rho+trop$
从上边的公式可以看出，电离层误差被消除了，所以这个组合也叫消电离层组合。