RTKLIB专题学习(五)---单点定位实现进阶(一)
2024-05-13 13:03:01
RTKLIB专题学习(五)
今天我们一起来了解一下，RTKLIB中的单点定位是如何实现的：
简单来说，就是最小二乘法，但是呢，RTKLIB里面的最小二乘实际上是加权最小二乘，因为他给出了观测值的权(实际体现出来的是测量误差的方差)
1.在估计接收机位置参数的函数estpos里面，有两个比较重要的函数，rescode和lsq
/* pseudorange residuals -----------------------------------------------------*/
static int rescode(int iter, const obsd_t *obs, int n, const double *rs,const double *dts, const double *vare, const int *svh,const nav_t *nav, const double *x, const prcopt_t *opt,double *v, double *H, double *var, double *azel, int *vsat,double *resp, int *ns,double *ele, double* R)
{gtime_t time;double r,freq,vmeas,rr[3],pos[3],dtr,e[3],P,par=0.0;double dion = 0;double dtrp = 0;double vion = 0;double vtrp = 0;int i,j,nv=0,sat,sys,mask[NX-3]={0},k=0,y=0;trace(3,"resprng : n=%d\n",n);for (i=0;i<3;i++) rr[i]=x[i];dtr=x[3];ecef2pos(rr,pos);//rr为地心地固空间直角坐标XYZ，pos为大地坐标BLHfor (i=*ns=0;i<n&&i<MAXOBS;i++) {vsat[i]=0; azel[i*2]=azel[1+i*2]=resp[i]=0.0;time=obs[i].time;sat=obs[i].sat;if (!(sys=satsys(sat,NULL))) continue;/* reject duplicated observation data */if (i<n-1&&i<MAXOBS-1&&sat==obs[i+1].sat) {trace(2,"duplicated obs data %s sat=%d\n",time_str(time,3),sat);i++;continue;}/* excluded satellite? */if (satexclude(sat,vare[i],svh[i],opt)) continue;/* geometric distance */if ((r=geodist(rs+i*6,rr,e))<=0.0) continue;if (iter>0) {/* test elevation mask */if (satazel(pos, e, azel + i * 2) < opt->elmin) continue;/* test SNR mask */if (!snrmask(obs+i,azel+i*2,opt)) continue;/* ionospheric correction */if (!ionocorr(time,nav,sat,pos,azel+i*2,opt->ionoopt,&dion,&vion)) {continue;}if ((freq=sat2freq(sat,obs[i].code[0],nav))==0.0) continue;dion*=SQR(FREQ1/freq);vion*=SQR(FREQ1/freq);/* tropospheric correction */if (!tropcorr(time,nav,pos,azel+i*2,opt->tropopt,&dtrp,&vtrp)) {continue;}}/* psendorange with code bias correction */if ((P=prange(obs+i,nav,opt,&vmeas))==0.0) continue;/* pseudorange residual */v[nv]=P-(r+dtr-CLIGHT*dts[i*2]+dion+dtrp);/* design matrix */for (j=0;j<NX;j++) {H[j+nv*NX]=j<3?-e[j]:(j==3?1.0:0.0);}/* time system offset and receiver bias correction */if (sys==SYS_GLO) {v[nv]-=x[4]; H[4+nv*NX]=1.0; mask[1]=1;}else if (sys==SYS_GAL) {v[nv]-=x[5]; H[5+nv*NX]=1.0; mask[2]=1;}else if (sys==SYS_CMP) {v[nv]-=x[6]; H[6+nv*NX]=1.0; mask[3]=1;}else if (sys==SYS_IRN) {v[nv]-=x[7]; H[7+nv*NX]=1.0; mask[4]=1;}
#if 0 /* enable QZS-GPS time offset estimation */else if (sys==SYS_QZS) {v[nv]-=x[8]; H[8+nv*NX]=1.0; mask[5]=1;}
#endifelse mask[0]=1;vsat[i]=1; resp[i]=v[nv]; (*ns)++;/* variance of pseudorange error */var[nv]=varerr(opt,azel[1+i*2],sys)+vare[i]+vmeas+vion+vtrp;nv = nv + 1;trace(4,"sat=%2d azel=%5.1f %4.1f res=%7.3f sig=%5.3f\n",obs[i].sat,azel[i*2]*R2D,azel[1+i*2]*R2D,resp[i],sqrt(var[nv-1]));}/* constraint to avoid rank-deficient */for (i=0;i<NX-3;i++) {if (mask[i]) continue;v[nv]=0.0;for (j=0;j<NX;j++) H[j+nv*NX]=j==i+3?1.0:0.0;var[nv]= var[nv - 1];ele[nv]=0.01;nv = nv + 1;}for (i = 0; i < nv; i++) for (j = 0; j < nv; j++) {R[i + j * nv] = i == j ? var[i] : 0.0;}return nv;
}
/* least square estimation -----------------------------------------------------
* least square estimation by solving normal equation (x=(A*A')^-1*A*y)
* args : double *A I transpose of (weighted) design matrix (n x m)
* double *y I (weighted) measurements (m x 1)
* int n,m I number of parameters and measurements (n<=m)
* double *x O estmated parameters (n x 1)
* double *Q O esimated parameters covariance matrix (n x n)
* return : status (0:ok,0>:error)
* notes : for weighted least square, replace A and y by A*w and w*y (w=W^(1/2))
* matirix stored by column-major order (fortran convention)
* m为观测方程数，n为待求参数，这里的A指的是设计矩阵的转置，y指的是观测方程
*-----------------------------------------------------------------------------*/
extern int lsq(const double *A, const double *y, int n, int m, double *x,double *Q)
{double *Ay;int info;if (m<n) return -1;Ay=mat(n,1);matmul("NN",n,1,m,1.0,A,y,0.0,Ay); /* Ay=A*y */matmul("NT",n,n,m,1.0,A,A,0.0,Q); /* Q=A*A' */if (!(info=matinv(Q,n))) matmul("NN",n,1,n,1.0,Q,Ay,0.0,x); /* x=Q^-1*Ay */free(Ay);return info;
}
2.rescode函数用于计算伪距残差、构建系数矩阵以及计算残差的方差等，这里需要注意，残差的方差，用于观测值加权。
/* weighted by Std*/for (j = 0; j < nv; j++) {sig = sqrt(var[j]);v[j] /= sig;for (k = 0; k < NX; k++) H[k + j * NX] /= sig;//设计矩阵的逐行乘以权重}
上面就是完成加权的过程，由于RTKLIB中最后参数估计得到的方程是如下形式：
因此若要在不改变这个形式的基础上，完成权阵的添加，那么就需要对系数矩阵和观测值矩阵进行变换，也就是乘以一个权阵的二分之一次方，那么按照这个形式计算的话，本质就等于加权最小二乘：
由于权阵就是方差阵的倒数，因此就会出现这样的式子：
sig = sqrt(var[j]);
v[j] /= sig;
for (k = 0; k < NX; k++) H[k + j * NX] /= sig
3.lsq就是参数估计的核心函数了，用于单点定位的参数估计
4.RTKLIB中实际上使用的是迭代的最小二乘，最大迭代次数设置为10
5.上述的伪距残差的方差如下计算公式：
/* variance of pseudorange error */var[nv]=varerr(opt,azel[1+i*2],sys)+vare[i]+vmeas+vion+vtrp;
对应参数意义如下：
W即为权阵，可以看出是方差矩阵的逆矩阵
式中各项计算函数和意义如下：
/* pseudorange measurement error variance ------------------------------------*/
static double varerr(const prcopt_t *opt, double el, int sys)
{double fact,varr;fact=sys==SYS_GLO?EFACT_GLO:(sys==SYS_SBS?EFACT_SBS:EFACT_GPS);if (el<MIN_EL) el=MIN_EL;varr=SQR(opt->err[0])*(SQR(opt->err[1])+SQR(opt->err[2])/sin(el));//model-rtklibif (opt->ionoopt==IONOOPT_IFLC) varr*=SQR(3.0); /* iono-free */return SQR(fact)*varr;
}
上述函数表明，当电离层改正使用的是无电离层组合模型的话，则方差为varr*=SQR(3.0)，否则方差为varr=SQR(opt->err[0])*(SQR(opt->err[1])+SQR(opt->err[2])/sin(el))
vare[i]为卫星位置和钟差的方差；vmeas为伪距偏差的方差；如果是利用克罗布歇模型计算电离层延迟的话，电离层延迟和方差如下式计算：
*ion=ionmodel(time,nav->ion_gps,pos,azel);
*var=SQR(*ion*ERR_BRDCI);
其中ionmodel函数如下：
/* ionosphere model ------------------------------------------------------------
* compute ionospheric delay by broadcast ionosphere model (klobuchar model)
* args : gtime_t t I time (gpst)
* double *ion I iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3}
* double *pos I receiver position {lat,lon,h} (rad,m)
* double *azel I azimuth/elevation angle {az,el} (rad)
* return : ionospheric delay (L1) (m)
*-----------------------------------------------------------------------------*/
extern double ionmodel(gtime_t t, const double *ion, const double *pos,const double *azel)
{const double ion_default[]={ /* 2004/1/1 */0.1118E-07,-0.7451E-08,-0.5961E-07, 0.1192E-06,0.1167E+06,-0.2294E+06,-0.1311E+06, 0.1049E+07};double tt,f,psi,phi,lam,amp,per,x;int week;if (pos[2]<-1E3||azel[1]<=0) return 0.0;if (norm(ion,8)<=0.0) ion=ion_default;/* earth centered angle (semi-circle) */psi=0.0137/(azel[1]/PI+0.11)-0.022;/* subionospheric latitude/longitude (semi-circle) */phi=pos[0]/PI+psi*cos(azel[0]);if (phi> 0.416) phi= 0.416;else if (phi<-0.416) phi=-0.416;lam=pos[1]/PI+psi*sin(azel[0])/cos(phi*PI);/* geomagnetic latitude (semi-circle) */phi+=0.064*cos((lam-1.617)*PI);/* local time (s) */tt=43200.0*lam+time2gpst(t,&week);tt-=floor(tt/86400.0)*86400.0; /* 0<=tt<86400 *//* slant factor */f=1.0+16.0*pow(0.53-azel[1]/PI,3.0);/* ionospheric delay */amp=ion[0]+phi*(ion[1]+phi*(ion[2]+phi*ion[3]));per=ion[4]+phi*(ion[5]+phi*(ion[6]+phi*ion[7]));amp=amp< 0.0? 0.0:amp;per=per<72000.0?72000.0:per;x=2.0*PI*(tt-50400.0)/per;return CLIGHT*f*(fabs(x)<1.57?5E-9+amp*(1.0+x*x*(-0.5+x*x/24.0)):5E-9);
}
如果是利用QZSS进行电离层延迟校正的话，那么计算如下：
*ion=ionmodel(time,nav->ion_qzs,pos,azel);
*var=SQR(*ion*ERR_BRDCI);
vtrp为对流层延迟改正的方差
利用到的函数如下：
/* tropospheric correction -----------------------------------------------------
* compute tropospheric correction
* args : gtime_t time I time
* nav_t *nav I navigation data
* double *pos I receiver position {lat,lon,h} (rad|m)
* double *azel I azimuth/elevation angle {az,el} (rad)
* int tropopt I tropospheric correction option (TROPOPT_???)
* double *trp O tropospheric delay (m)
* double *var O tropospheric delay variance (m^2)
* return : status(1:ok,0:error)
*-----------------------------------------------------------------------------*/
extern int tropcorr(gtime_t time, const nav_t *nav, const double *pos,const double *azel, int tropopt, double *trp, double *var)
{trace(4,"tropcorr: time=%s opt=%d pos=%.3f %.3f azel=%.3f %.3f\n",time_str(time,3),tropopt,pos[0]*R2D,pos[1]*R2D,azel[0]*R2D,azel[1]*R2D);/* Saastamoinen model */if (tropopt==TROPOPT_SAAS||tropopt==TROPOPT_EST||tropopt==TROPOPT_ESTG) {*trp=tropmodel(time,pos,azel,REL_HUMI);*var=SQR(ERR_SAAS/(sin(azel[1])+0.1));return 1;}/* SBAS (MOPS) troposphere model */if (tropopt==TROPOPT_SBAS) {*trp=sbstropcorr(time,pos,azel,var);return 1;}/* no correction */*trp=0.0;*var=tropopt==TROPOPT_OFF?SQR(ERR_TROP):0.0;return 1;
}
如果是Saastamoinen模型的话，那么利用tropmodel函数
*trp=tropmodel(time,pos,azel,REL_HUMI);*var=SQR(ERR_SAAS/(sin(azel[1])+0.1));
/* troposphere model -----------------------------------------------------------
* compute tropospheric delay by standard atmosphere and saastamoinen model
* args : gtime_t time I time
* double *pos I receiver position {lat,lon,h} (rad,m)
* double *azel I azimuth/elevation angle {az,el} (rad)
* double humi I relative humidity
* return : tropospheric delay (m)
*-----------------------------------------------------------------------------*/
extern double tropmodel(gtime_t time, const double *pos, const double *azel,double humi)
{const double temp0=15.0; /* temparature at sea level */double hgt,pres,temp,e,z,trph,trpw;if (pos[2]<-100.0||1E4<pos[2]||azel[1]<=0) return 0.0;/* standard atmosphere */hgt=pos[2]<0.0?0.0:pos[2];pres=1013.25*pow(1.0-2.2557E-5*hgt,5.2568);temp=temp0-6.5E-3*hgt+273.16;e=6.108*humi*exp((17.15*temp-4684.0)/(temp-38.45));/* saastamoninen model */z=PI/2.0-azel[1];trph=0.0022768*pres/(1.0-0.00266*cos(2.0*pos[0])-0.00028*hgt/1E3)/cos(z);trpw=0.002277*(1255.0/temp+0.05)*e/cos(z);return trph+trpw;
}
6.接下来呢就是参数估计的函数了，即lsq
/* least square estimation -----------------------------------------------------
* least square estimation by solving normal equation (x=(A*A')^-1*A*y)
* args : double *A I transpose of (weighted) design matrix (n x m)
* double *y I (weighted) measurements (m x 1)
* int n,m I number of parameters and measurements (n<=m)
* double *x O estmated parameters (n x 1)
* double *Q O esimated parameters covariance matrix (n x n)
* return : status (0:ok,0>:error)
* notes : for weighted least square, replace A and y by A*w and w*y (w=W^(1/2))
* matirix stored by column-major order (fortran convention)
* m为观测方程数，n为待求参数，这里的A指的是设计矩阵的转置，y指的是观测方程
*-----------------------------------------------------------------------------*/
extern int lsq(const double *A, const double *y, int n, int m, double *x,double *Q)
{double *Ay;int info;if (m<n) return -1;Ay=mat(n,1);matmul("NN",n,1,m,1.0,A,y,0.0,Ay); /* Ay=A*y */matmul("NT",n,n,m,1.0,A,A,0.0,Q); /* Q=A*A' */if (!(info=matinv(Q,n))) matmul("NN",n,1,n,1.0,Q,Ay,0.0,x); /* x=Q^-1*Ay */free(Ay);return info;
}
最小二乘这部分没有引用其他特殊的函数，都是矩阵运算。
RTKLIB中解得的是参数的增量，因此计算结束后需要增加如下代码：
for (j = 0; j < NX; j++) {x[j] += dx[j];}
当增量矩阵符合一定条件，则输出结果：
if ((norm(dx, NX) < 1E-4)){sol->type = 0;sol->time = timeadd(obs[0].time, -x[3] / CLIGHT);sol->dtr[0] = x[3] / CLIGHT; /* receiver clock bias (s) */sol->dtr[1] = x[4] / CLIGHT; /* GLO-GPS time offset (s) */sol->dtr[2] = x[5] / CLIGHT; /* GAL-GPS time offset (s) */sol->dtr[3] = x[6] / CLIGHT; /* BDS-GPS time offset (s) */sol->dtr[4] = x[7] / CLIGHT; /* IRN-GPS time offset (s) */for (j = 0; j < 6; j++) sol->rr[j] = j < 3 ? x[j] : 0.0;for (j = 0; j < 3; j++) sol->qr[j] = (float)Q[j + j * NX];sol->qr[3] = (float)Q[1]; /* cov xy */sol->qr[4] = (float)Q[2 + NX]; /* cov yz */sol->qr[5] = (float)Q[2]; /* cov zx */sol->ns = (uint8_t)ns;sol->age = sol->ratio = 0.0;/* validate solution */if ((stat = valsol(azel, vsat, n, opt, v, nv, NX, msg))) {sol->stat = opt->sateph == EPHOPT_SBAS ? SOLQ_SBAS : SOLQ_SINGLE;}
其中valsol为结果有效性检验函数：
/* validate solution ---------------------------------------------------------*/
static int valsol(const double *azel, const int *vsat, int n,const prcopt_t *opt, const double *v, int nv, int nx,char *msg)
{double azels[MAXOBS*2],dop[4],vv;int i,ns;trace(3,"valsol : n=%d nv=%d\n",n,nv);/* Chi-square validation of residuals */vv=dot(v,v,nv);if (nv>nx&&vv>chisqr[nv-nx-1]) {sprintf(msg,"chi-square error nv=%d vv=%.1f cs=%.1f",nv,vv,chisqr[nv-nx-1]);return 0;}/* large GDOP check */for (i=ns=0;i<n;i++) {if (!vsat[i]) continue;azels[ ns*2]=azel[ i*2];azels[1+ns*2]=azel[1+i*2];ns++;}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;}return 1;
}
其中利用到了卡方检验和最大GDOP指检验
7.若结算结果有效，则estpos返回值为1，否则为0；返回值体现在pntpos中，下一篇会分析，在pntpos调用完estpos后的步骤。
RTKLIB专题学习(五)---单点定位实现进阶(一)相关推荐
- RTKLIB专题学习(四)---单点定位实现初识（二）
RTKLIB专题学习(四)-单点定位实现初识(二) 今天我们来继续学习RTKLIB中单点定位的调用情况,上一篇在这里:RTKLIB专题学习(四)-单点定位实现初识(一) 1.上篇说到了调用procpo ...
- RTKLIB专题学习(六)---单点定位应用(二)
RTKLIB专题学习(六)-单点定位应用(二) 上一篇RTKLIB专题学习(六)-单点定位应用(一)我们使用最小二乘以及抗差最小二乘对单点定位进行定位效果分析,发现在存在粗差情况下,抗差最小二乘可以获 ...
- RTKLIB专题学习(七)---精密单点定位实现初识（三）
RTKLIB专题学习(七)-精密单点定位实现初识(三) 上两篇我们介绍了RTKLIB中精密单点定位的大致流程,今天我们对照RTKLIB学习手册,来学习相应改正公式和误差源 1.在PPP模式中 RTKL ...
- RTKLIB专题学习(三)---矩阵应用
RTKLIB专题学习(三) 今天我们来进一步学习RTKLIB中矩阵的各种应用 rtkcmn.c : rtklib common functions 1.这是最小二乘法(实际在应用中为加权最小二乘) m ...
- RTKLIB专题学习(十)—电离层改正
RTKLIB专题学习(十)-电离层改正 今天我们一起来学习下,RTKLIB中对电离层延迟改正的方法,并了解源码的相应模块,以便对原理有一定的了解 1.电离层延迟改正模型 由于存在大量的自由电子和正负离 ...
- IM通讯协议专题学习(八)：金蝶随手记团队的Protobuf应用实践(原理篇)
本文由金蝶随手记技术团队丁同舟分享. 1.引言 跟移动端IM中追求数据传输效率.网络流量消耗等需求一样,随手记客户端与服务端交互的过程中,对部分数据的传输大小和效率也有较高的要求,普通的数据格式如 J ...
- Jmeter(总篇): 针对性能测试工具：Jmeter的专题学习
今天因为要提供给其他同事学习的资料,进行整理Jmeter学习资料. 根据我的博客,整理的针对性能测试工具:Jmeter的专题学习,请参考. 第一阶段:组件介绍 序号 标题 链接 1 1.0Jmeter ...
- 【响应式编程的思维艺术】 （1）Rxjs专题学习计划
[摘要] 请暂时忘掉你的对象,感受一切皆流的世界. 一. 响应式编程 响应式编程,也称为流式编程,对于非前端工程师来说,可能并不是一个陌生的名词,它是函数式编程在软件开发中应用的延伸,如果你对函数式编 ...
- MOOS-ivp 实验五 MOOS编程进阶（3）
MOOS-ivp 实验五 MOOS编程进阶(3) 经过近日的学习与摸索,我来重新完善以下实验五的相关内容,上次做到三分之二的内容放弃了,主要原因还是因为C++功底不够深厚,需要更多的学习和积累.经过我 ...
最新文章
- 《OpenCV3编程入门》学习笔记6 图像处理（四）形态学滤波（2）：开运算、闭运算、形态学梯度、顶帽、黑帽
- 宏基因组公众号14天受邀原创-诚邀同行共享研究经验
- 趁ofo退出美市场 Uber不计成本发展共享单车
- 你的大脑里有AI吗？
- 使用hexo搭建个人博客
- WDA将改变现有的abap的开发方式
- 操作多个表_8_不等值连接
- Win32ASM学习[13]:移位指令SHL,SHR,SAL,SAR,ROL,ROR,RCL,RCR,SHLD,SHRD
- 数据结构树4-二叉搜索树2
- 删除Github上项目
- 首都师范大学数学专业考研试题参考解答
- AttributeError: 'numpy.int64' object has no attribute 'translate'
- python数据标注工具_数据标注工具大全汇总，有了这些工具再也不用自己开发了...
- 腾讯云服务器如何开启虚拟化,腾讯云服务器虚拟化驱动是什么
- 信号处理：时域和频域的关系
- 零基础自学软件测试-项目经验-电商项目实战-测试用例设计-促销中心
- 你想要的宏基因组-微生物组知识全在这(2020.11)
- Java 深入面向对象
- 全网最全AD16——原理图绘制
- 苹果新专利曝光 背后有何暗示？