（二）RTKLIB学习——pntpos单点定位

欢喜h~

于 2024-08-25 08:54:34 发布

阅读量810

点赞数 24

分类专栏： RTKLIB学习文章标签：学习

本文链接：https://blog.csdn.net/weixin_50911531/article/details/141509562

版权

RTKLIB学习专栏收录该内容

3 篇文章 0 订阅

订阅专栏

rtklib的main函数位于rnx2rtkp.c中，函数总体的调用关系如下。今天主要学习一下pntpos的单点定位，该函数位于pntpos.c文件下。

函数的定义

extern int pntpos(const obsd_t *obs, int n, const nav_t *nav,
                  const prcopt_t *opt, sol_t *sol, double *azel,
                  ssat_t *ssat, char *msg);

obsd_t *obs：观测文件
n：观测到的卫星数
nav_t *nav：导航文件
prcopt_t *opt：配置选项
sol_t *sol：数据解选项
double azel (out)：天顶角和仰角
ssat_t *ssat: 卫星状态
char *msg：错误message

总体思路

这里代码讲解的是最新版本的rtklib，所以和上图中的顺序有些出入。

1. 变量定义，rs存储位置和速度，dts存储钟差和钟漂，var存储方差，azel_存储天顶角和仰角，resp为残差信息。

prcopt_t opt_=*opt;
double *rs,*dts,*var,*azel_,*resp;
int i,stat,vsat[MAXOBS]={0},svh[MAXOBS];
sol->stat=SOLQ_NONE;

2. 检查卫星个数是否大于0

if (n<=0) {strcpy(msg,"no observation data"); return 0;}

3. 重置变量大小，初始化

rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n);

4. 设置修正模型。当处理选项opt中的模式不是单点定位时，电离层校正采用Klobuchar模型，对流层采用Saastamoinen模型。

Klobuchar电离层模型需要8个广播参数： $\alpha_1, \alpha_2, \alpha_3, \alpha_4, \beta_1, \beta_2, \beta_3, \beta_4$

看看GPT给的计算流程：

Saastamoinen模型

    if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */ 
#if 0
        opt_.sateph =EPHOPT_BRDC;
#endif
        opt_.ionoopt=IONOOPT_BRDC;
        opt_.tropopt=TROPOPT_SAAS;
    }

5. 调用satposs函数，按照观测到的卫星顺序计算出每颗卫星的位置/速度/{钟差，频漂}

satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh);

6. 调用estpos函数，利用伪距观测值计算估计接收机位置

stat=estpos(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);

7.调用raim_fde函数，进行完好性检测

if (!stat&&n>=6&&opt->posopt[4]) {
    stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
}

8. 调用estvel函数，用多普勒观测值估计接收机速度

if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat);

satposs求卫星位置

以下步骤是按照观测顺序，计算每颗卫星的PVC，i表示第i颗卫星

1. 用数据接收时间减去伪距信号传播时间，得到卫星信号的发射时间

time[i]=timeadd(obs[i].time,-pr/CLIGHT);

2. 调用ephclk函数，由广播星历计算出当前观测卫星的钟差。（此时钟差没有考虑相对论效应和TGD）

ephclk(time[i],teph,obs[i].sat,nav,&dt)

先选择星历，然后广播星历求出卫星钟差。这个函数实现并没有考虑相对论效应修正。

extern double eph2clk(gtime_t time, const eph_t *eph)
{
    double t;
    int i;
    
    trace(4,"eph2clk : time=%s sat=%2d\n",time_str(time,3),eph->sat);
    
    t=timediff(time,eph->toc); // toc为卫星中的参考时刻
    
    for (i=0;i<2;i++) {
        t-=eph->f0+eph->f1*t+eph->f2*t*t; // f0为卫星钟偏差/f1为卫星钟的漂移/f2为卫星钟的漂移速度
    }
    return eph->f0+eph->f1*t+eph->f2*t*t;
}

3. 用信号发射时间减去钟差，得到GPS时间下卫星信号发射时间

time[i]=timeadd(time[i],-dt);

4. 调用satpos函数，计算信号发射时刻微信过的P,V,C。（此时钟差考虑了相对论效应，但是还没有考虑TGD）

satpos(time[i],teph,obs[i].sat,ephopt,nav,rs+i*6,dts+i*2,var+i,svh+i)

5. 如果上面计算出的钟差为0，就再次调用ephclk函数，将结果作为最后的卫星钟差

这里选择satpos函数中的ephpos来得到rs,dts,var,svh，也就是通过广播星历来计算卫星信息。

static int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
                  int iode, double *rs, double *dts, double *var, int *svh)
{
    eph_t  *eph;
    geph_t *geph;
    seph_t *seph;
    double rst[3],dtst[1],tt=1E-3;
    int i,sys;
    
    trace(4,"ephpos  : time=%s sat=%2d iode=%d\n",time_str(time,3),sat,iode);
    
    sys=satsys(sat,NULL);
    
    *svh=-1;
    
    if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) {
        if (!(eph=seleph(teph,sat,iode,nav))) return 0;
        
        eph2pos(time,eph,rs,dts,var);
        time=timeadd(time,tt);
        eph2pos(time,eph,rst,dtst,var);
        *svh=eph->svh;
    }
    else if (sys==SYS_GLO) {
        if (!(geph=selgeph(teph,sat,iode,nav))) return 0;
        geph2pos(time,geph,rs,dts,var);
        time=timeadd(time,tt);
        geph2pos(time,geph,rst,dtst,var);
        *svh=geph->svh;
    }
    else if (sys==SYS_SBS) {
        if (!(seph=selseph(teph,sat,nav))) return 0;
        
        seph2pos(time,seph,rs,dts,var);
        time=timeadd(time,tt);
        seph2pos(time,seph,rst,dtst,var);
        *svh=seph->svh;
    }
    else return 0;
    
    /* satellite velocity and clock drift by differential approx */
    for (i=0;i<3;i++) rs[i+3]=(rst[i]-rs[i])/tt;
    dts[1]=(dtst[0]-dts[0])/tt;
    
    return 1;
}

放上代码和公式，自行对应，即可得到结果

extern void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts,
                    double *var)
{
    double tk,M,E,Ek,sinE,cosE,u,r,i,O,sin2u,cos2u,x,y,sinO,cosO,cosi,mu,omge;
    double xg,yg,zg,sino,coso;
    int n,sys,prn;
    
    trace(4,"eph2pos : time=%s sat=%2d\n",time_str(time,3),eph->sat);
    
    if (eph->A<=0.0) {
        rs[0]=rs[1]=rs[2]=*dts=*var=0.0;
        return;
    }
    tk=timediff(time,eph->toe);  // toe:星历参考时刻
    
    switch ((sys=satsys(eph->sat,&prn))) {
        case SYS_GAL: mu=MU_GAL; omge=OMGE_GAL; break;
        case SYS_CMP: mu=MU_CMP; omge=OMGE_CMP; break;
        default:      mu=MU_GPS; omge=OMGE;     break;
    }
    M=eph->M0+(sqrt(mu/(eph->A*eph->A*eph->A))+eph->deln)*tk;
    
    for (n=0,E=M,Ek=0.0;fabs(E-Ek)>RTOL_KEPLER&&n<MAX_ITER_KEPLER;n++) {
        Ek=E; E-=(E-eph->e*sin(E)-M)/(1.0-eph->e*cos(E));
    }
    if (n>=MAX_ITER_KEPLER) {
        trace(2,"kepler iteration overflow sat=%2d\n",eph->sat);
        return;
    }
    sinE=sin(E); cosE=cos(E);
    
    trace(4,"kepler: sat=%2d e=%8.5f n=%2d del=%10.3e\n",eph->sat,eph->e,n,E-Ek);
    
    u=atan2(sqrt(1.0-eph->e*eph->e)*sinE,cosE-eph->e)+eph->omg;
    r=eph->A*(1.0-eph->e*cosE);
    i=eph->i0+eph->idot*tk;
    sin2u=sin(2.0*u); cos2u=cos(2.0*u);
    u+=eph->cus*sin2u+eph->cuc*cos2u;
    r+=eph->crs*sin2u+eph->crc*cos2u;
    i+=eph->cis*sin2u+eph->cic*cos2u;
    x=r*cos(u); y=r*sin(u); cosi=cos(i);
    
    /* beidou geo satellite (ref [9]) */
    if (sys==SYS_CMP&&prn<=5) {
        O=eph->OMG0+eph->OMGd*tk-omge*eph->toes;
        sinO=sin(O); cosO=cos(O);
        xg=x*cosO-y*cosi*sinO;
        yg=x*sinO+y*cosi*cosO;
        zg=y*sin(i);
        sino=sin(omge*tk); coso=cos(omge*tk);
        rs[0]= xg*coso+yg*sino*COS_5+zg*sino*SIN_5;
        rs[1]=-xg*sino+yg*coso*COS_5+zg*coso*SIN_5;
        rs[2]=-yg*SIN_5+zg*COS_5;
    }
    else {
        O=eph->OMG0+(eph->OMGd-omge)*tk-omge*eph->toes;
        sinO=sin(O); cosO=cos(O);
        rs[0]=x*cosO-y*cosi*sinO;
        rs[1]=x*sinO+y*cosi*cosO;
        rs[2]=y*sin(i);
    }
    tk=timediff(time,eph->toc);
    *dts=eph->f0+eph->f1*tk+eph->f2*tk*tk;
    
    /* relativity correction */
    *dts-=2.0*sqrt(mu*eph->A)*eph->e*sinE/SQR(CLIGHT);
    
    /* position and clock error variance */
    *var=var_uraeph(eph->sva);
}

1）计算卫星运行的平均角速度n

$n_{0}=\sqrt{GM/a^{3}}=\sqrt{\mu}/(\sqrt{a})^{3}\\\mu=GM=3986005\cdot10^{8}m^{3}/s^{2}\\n=n_0+\Delta n$

2）计算归化观测时间

电文中给出的GPS卫星轨道参数是对应与参考时刻的。对于某时刻t观测卫星，需要将观测时间t归为 $t_k$ （以参考时刻 $t_{oe}$ 为基准的归化观测时间）

$t_k=t-t_{oe}$

3）计算观测时刻的卫星平近点角

$M_k=M_0+nt_k$

4）计算观测时刻的偏近点角

$E_{_k}=M_{_k}+e\sin E_{_k}$

5）计算真近点角

$f_{_k}=arctg(\sqrt{1-e^{2}}\sin E_{_k})/(\cos E_{_k}-e)$

6）计算升交距角

$\Phi_k=f_k+\omega$

7）计算摄动改正项

$\begin{cases}\delta_{u}=C_{uc}\cos(2\Phi_{k})+C_{us}\sin(2\Phi_{k})\\\delta_{r}=C_{rc}\cos(2\Phi_{k})+C_{rs}\sin(2\Phi_{k})\\\delta_{i}=C_{ic}\cos(2\Phi_{k})+C_{is}\sin(2\Phi_{k})\end{cases}$

8）计算经摄动改正后的升交距角，卫星矢径和轨道倾角

$\begin{cases}u_k=\Phi_k+\delta_u\\r_k=a(1-e\cos E_k)+\delta_r\\i_k=i_0+\delta_i+i\cdot t_k\end{cases}$

9）计算卫星在轨道平面坐标系中的坐标位置

$\begin{cases}x_k=r_k\cos u_k\\y_k=r_k\sin u_k\end{cases}$

10）计算观测时刻t的升交点经度

$L_k=\Omega_0+(\dot{\Omega}-\omega_e)t_k-\omega_e t_{oe}$

11）计算卫星在地心地固坐标系中的坐标位置

$\begin{bmatrix}X_k\\Y_k\\Z_k\end{bmatrix}=\begin{bmatrix}x_k\cos L_k-y_k\cos i_k\sin L_k\\x_k\sin L_k+y_k\cos i_k\cos L_k\\y_k\sin i_k\end{bmatrix}$

estpos估计接收机位置（伪距）

注：上述伪距方程并没有把电离层对流层误差等考虑进去。

设计矩阵和伪距残差构建：

/* 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 r,dion,dtrp,vmeas,vion,vtrp,rr[3],pos[3],dtr,e[3],P,lam_L1;
    int i,j,nv=0,sys,mask[4]={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存储接收机坐标，pos存储转换后的经纬度
    
    for (i=*ns=0;i<n&&i<MAXOBS;i++) {
        vsat[i]=0; azel[i*2]=azel[1+i*2]=resp[i]=0.0;
        
        if (!(sys=satsys(obs[i].sat,NULL))) continue;
        
        /* reject duplicated observation data */
        if (i<n-1&&i<MAXOBS-1&&obs[i].sat==obs[i+1].sat) {
            trace(2,"duplicated observation data %s sat=%2d\n",
                  time_str(obs[i].time,3),obs[i].sat);
            i++;
            continue;
        }
        /* geometric distance/azimuth/elevation angle */
        if ((r=geodist(rs+i*6,rr,e))<=0.0||
            satazel(pos,e,azel+i*2)<opt->elmin) continue;
        
        /* psudorange with code bias correction */
        if ((P=prange(obs+i,nav,azel+i*2,iter,opt,&vmeas))==0.0) continue;
        
        /* excluded satellite? */
        if (satexclude(obs[i].sat,svh[i],opt)) continue;
        
        /* 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]);
        }
        /* tropospheric corrections */
        if (!tropcorr(obs[i].time,nav,pos,azel+i*2,
                      iter>0?opt->tropopt:TROPOPT_SAAS,&dtrp,&vtrp)) {
            continue;
        }
        /* pseudorange residual */
        v[nv]=P-(r+dtr-CLIGHT*dts[i*2]+dion+dtrp);  // 计算伪距残差并填入v数组
        
        /* design matrix */
        for (j=0;j<NX;j++) H[j+nv*NX]=j<3?-e[j]:(j==3?1.0:0.0); // 生成设计矩阵H
        
        /* time system and receiver bias offset 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 mask[0]=1;
        
        vsat[i]=1; resp[i]=v[nv]; (*ns)++;
        
        /* error variance */
        var[nv++]=varerr(opt,azel[1+i*2],sys)+vare[i]+vmeas+vion+vtrp;
        
        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<4;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++]=0.01;
    }
    return nv;
}

最小二乘：

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;
}

raim_fde完好性检测

static int raim_fde(const obsd_t *obs, int n, const double *rs,
                    const double *dts, const double *vare, const int *svh,
                    const nav_t *nav, const prcopt_t *opt, sol_t *sol,
                    double *azel, int *vsat, double *resp, char *msg)
{
    obsd_t *obs_e;
    sol_t sol_e={{0}};
    char tstr[32],name[16],msg_e[128];
    double *rs_e,*dts_e,*vare_e,*azel_e,*resp_e,rms_e,rms=100.0;
    int i,j,k,nvsat,stat=0,*svh_e,*vsat_e,sat=0;
    
    trace(3,"raim_fde: %s n=%2d\n",time_str(obs[0].time,0),n);
    
    if (!(obs_e=(obsd_t *)malloc(sizeof(obsd_t)*n))) return 0;
    rs_e = mat(6,n); dts_e = mat(2,n); vare_e=mat(1,n); azel_e=zeros(2,n);
    svh_e=imat(1,n); vsat_e=imat(1,n); resp_e=mat(1,n); 
    
    for (i=0;i<n;i++) {
        
        /* satellite exclution */
        for (j=k=0;j<n;j++) {
            if (j==i) continue;
            obs_e[k]=obs[j];
            matcpy(rs_e +6*k,rs +6*j,6,1);
            matcpy(dts_e+2*k,dts+2*j,2,1);
            vare_e[k]=vare[j];
            svh_e[k++]=svh[j];
        }
        /* estimate receiver position without a satellite */
        if (!estpos(obs_e,n-1,rs_e,dts_e,vare_e,svh_e,nav,opt,&sol_e,azel_e,
                    vsat_e,resp_e,msg_e)) {
            trace(3,"raim_fde: exsat=%2d (%s)\n",obs[i].sat,msg);
            continue;
        }
        for (j=nvsat=0,rms_e=0.0;j<n-1;j++) {
            if (!vsat_e[j]) continue;
            rms_e+=SQR(resp_e[j]);
            nvsat++;
        }
        if (nvsat<5) {
            trace(3,"raim_fde: exsat=%2d lack of satellites nvsat=%2d\n",
                  obs[i].sat,nvsat);
            continue;
        }
        rms_e=sqrt(rms_e/nvsat);
        
        trace(3,"raim_fde: exsat=%2d rms=%8.3f\n",obs[i].sat,rms_e);
        
        if (rms_e>rms) continue;
        
        /* save result */
        for (j=k=0;j<n;j++) {
            if (j==i) continue;
            matcpy(azel+2*j,azel_e+2*k,2,1);
            vsat[j]=vsat_e[k];
            resp[j]=resp_e[k++];
        }
        stat=1;
        *sol=sol_e;
        sat=obs[i].sat;
        rms=rms_e;
        vsat[i]=0;
        strcpy(msg,msg_e);
    }
    if (stat) {
        time2str(obs[0].time,tstr,2); satno2id(sat,name);
        trace(2,"%s: %s excluded by raim\n",tstr+11,name);
    }
    free(obs_e);
    free(rs_e ); free(dts_e ); free(vare_e); free(azel_e);
    free(svh_e); free(vsat_e); free(resp_e);
    return stat;
}

通过一个双重循环，逐一排除每颗卫星，重新估计接收机的位置，并计算每次估计的均方根误差。最终选择排除哪颗卫星能使定位误差最小，从而确定最可能的故障卫星。

estvel估计接收机速度（多普勒）

/* doppler residuals ---------------------------------------------------------*/
static int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
                  const nav_t *nav, const double *rr, const double *x,
                  const double *azel, const int *vsat, double *v, double *H)
{
    double lam,rate,pos[3],E[9],a[3],e[3],vs[3],cosel;
    int i,j,nv=0;
    
    trace(3,"resdop  : n=%d\n",n);
    
    ecef2pos(rr,pos); xyz2enu(pos,E);
    
    for (i=0;i<n&&i<MAXOBS;i++) {
        
        lam=nav->lam[obs[i].sat-1][0];
        
        if (obs[i].D[0]==0.0||lam==0.0||!vsat[i]||norm(rs+3+i*6,3)<=0.0) {
            continue;
        }
        /* line-of-sight vector in ecef */
        cosel=cos(azel[1+i*2]);
        a[0]=sin(azel[i*2])*cosel;
        a[1]=cos(azel[i*2])*cosel;
        a[2]=sin(azel[1+i*2]);
        matmul("TN",3,1,3,1.0,E,a,0.0,e);
        
        /* satellite velocity relative to receiver in ecef */
        for (j=0;j<3;j++) vs[j]=rs[j+3+i*6]-x[j];
        
        /* range rate with earth rotation correction */
        rate=dot(vs,e,3)+OMGE/CLIGHT*(rs[4+i*6]*rr[0]+rs[1+i*6]*x[0]-
                                      rs[3+i*6]*rr[1]-rs[  i*6]*x[1]);
        
        /* doppler residual */
        v[nv]=-lam*obs[i].D[0]-(rate+x[3]-CLIGHT*dts[1+i*2]);
        
        /* design matrix */
        for (j=0;j<4;j++) H[j+nv*4]=j<3?-e[j]:1.0;
        
        nv++;
    }
    return nv;
}

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;
}

本文为学习笔记，如有错误，欢迎指正~