文章目录
1 调用关系图
下图是rtklib的单点定位的调用关系图,入口函数为pntpos(位于pntpos.c)。
2 单点定位程序流程
从上图中可以看出,pntpos这个函数做了3件事,基本上分别对应了三个函数。
2.1 计算卫星位置和速度
函数estpos完成了卫星位置的计算,这一部分代码基本是对着公式敲代码。
2.2 位置估计
函数estpos用来完成位置估计,使用的算法是加权最小二乘法,位置估计如果失败会调用raim算法重新进行位置估计,前提是收星数满足至少六颗卫星,并且对应的解算参数要设置opt->posopt[4]=1;
。
位置估计的本质是解算下边的方程组:
其中,
是
卫星对应的伪距观测量,从观测数据中获得,为已知量,
为
卫星对应的位置,在2.1中已经计算得到,也是已知量,
为光速,
为
卫星对应的电离层延迟,通过建模得到,下边的第3章中介绍,这里看做已知量,
为
卫星对应的对流层延迟,通过建模得到,下边的第3章中介绍,这里看做已知量,
那么方程组中只剩下
四个未知数了(分别是接收机位置和接收机钟差),解算四个未知数需要至少四个方程,所以要完成单点定位解算至少需要四颗卫星。
这里特别说明一下,以上的方程和结论是建立在单系统(如单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 加权
下边的代码用来给观测值对应的 阵相应的行乘一个权值,可以看到用的是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 最小二乘求解
下边的代码用来计算最小二乘解,
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;
速度估计的本质是解算下边的方程组:
其中,
是
卫星的多普勒观测量,即obs_t
中的D
,
是卫星速度向量,
是接收机速度,为未知量
为接收机钟漂,为未知量
为卫星
的方向余弦矢量,由于卫星的位置和接收机位置已经在前边解算出来了,那么这个方向余弦也是已知的
可以看出速度的求解是求解一组四元一次线性方程,这点与位置解算不同,无需迭代。
3 单点定位中的关键函数对应算法
3.1 求解非线性方程组
卫星位置解算的本质是求解一组非线性方程,没有学过数值计算的人可能理解起来稍微有一点点费解,这里从一个一维的例子扩展到高维。学过数值计算的可以跳过这一节了。
首先看下边这么一个非线性方程:
下边我们通过线性化和迭代的方法解算一下这个方程,首先通过泰勒级数线性化,泰勒展开需要在某点展开,首先令
。
方程变成了一个线性方程,解算得
然后我们令
,
解算得到,
,就这么一直迭代解算下去,我们可以看到迭代次数和解算结果的关系,可以看到只需要经过四次迭代,几乎得到正确结果了。
迭代次数 | 解算结果 | 残差 |
---|---|---|
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);
因为gps的电离层延迟与频率相关,其一阶近似项为,
其中
为常数,因此
.
所以,
从上边的公式可以看出,电离层误差被消除了,所以这个组合也叫消电离层组合。