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为大地坐标BLH
for (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;}
#endif
else 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-rtklib
if (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后的步骤。
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
