单点定位算法流程和编程实践

1?单点定位整体框架、输入输出、配置、卫星位置计算

1.1 rtkpos( )函数

功能：处理单历元的所有卫星观测值数据（流动站+基准站）

参数：

[1] rtk_t *rtk

[2] const obsd_t *obs?每个历元所有的观测值

[3] int n? 卫星数量。也是结构体数组obs下标大小

[4] const nav_t *nav? 星历

首先备份配置参数，因为后面使用时会改。之后统计基准站和流动站个数，记录上一历元的时间，进行流动站单点定位pntpos()，然后计算两历元的时差。最后选择不同的定位模式进行解算、输出

extern int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
{
    prcopt_t *opt=&rtk->opt; //先复制一份配置，因为后面要改
    sol_t solb={{0}};
    gtime_t time;
    int i,nu,nr;
    char msg[128]="";
    
    trace(3,"rtkpos  : time=%s n=%d\n",time_str(obs[0].time,3),n);
    trace(4,"obs=\n"); traceobs(4,obs,n);
    /*trace(5,"nav=\n"); tracenav(5,nav);*/
    
    /* set base staion position */
    if (opt->refpos<=3&&opt->mode!=PMODE_SINGLE&&opt->mode!=PMODE_MOVEB) {
        for (i=0;i<6;i++) rtk->rb[i]=i<3?opt->rb[i]:0.0;
    }
    /* count rover/base station observations */
    for (nu=0;nu   <n&&obs[nu   ].rcv==1;nu++) ; //流动站个数统计
    for (nr=0;nu+nr<n&&obs[nu+nr].rcv==2;nr++) ; //基准站个数统计
    
    time=rtk->sol.time; /* previous epoch */ //上一历元的时间，时间的获取是在ontpos()这个函数里
    
    /* rover position by single point positioning 流动站单点定位 */
    if (!pntpos(obs,nu,nav,&rtk->opt,&rtk->sol,NULL,rtk->ssat,msg)) {
        errmsg(rtk,"point pos error (%s)\n",msg);
        
        if (!rtk->opt.dynamics) {
            outsolstat(rtk);
            return 0;
        }
    }
    //得出两个历元的时间差
    //rtk->tt，time difference between current and previous
    if (time.time!=0) rtk->tt=timediff(rtk->sol.time,time);

    //下面是选择定位的模式
    /* single point positioning 单点定位*/ //如果是单点定位，那么我们之前就解算过了，直接输出即可
    if (opt->mode==PMODE_SINGLE) {
        outsolstat(rtk);
        return 1;
    }
    /* precise point positioning 精密单点定位*/
    if (opt->mode>=PMODE_PPP_KINEMA) {
        pppos(rtk,obs,nu,nav);
        pppoutsolstat(rtk,statlevel,fp_stat);
        return 1;
    }
    /* check number of data of base station and age of differential */
    if (nr==0) {
        errmsg(rtk,"no base station observation data for rtk\n");
        outsolstat(rtk);
        return 1;
    }
    if (opt->mode==PMODE_MOVEB) { /*  moving baseline 基线 */
        
        /* estimate position/velocity of base station */
        if (!pntpos(obs+nu,nr,nav,&rtk->opt,&solb,NULL,NULL,msg)) {
            errmsg(rtk,"base station position error (%s)\n",msg);
            return 0;
        }
        rtk->sol.age=(float)timediff(rtk->sol.time,solb.time);
        
        if (fabs(rtk->sol.age)>TTOL_MOVEB) {
            errmsg(rtk,"time sync error for moving-base (age=%.1f)\n",rtk->sol.age);
            return 0;
        }
        for (i=0;i<6;i++) rtk->rb[i]=solb.rr[i];
        
        /* time-synchronized position of base station */
        for (i=0;i<3;i++) rtk->rb[i]+=rtk->rb[i+3]*rtk->sol.age;
    }
    else {
        rtk->sol.age=(float)timediff(obs[0].time,obs[nu].time);
        
        if (fabs(rtk->sol.age)>opt->maxtdiff) {
            errmsg(rtk,"age of differential error (age=%.1f)\n",rtk->sol.age);
            outsolstat(rtk);
            return 1;
        }
    }
    /* relative potitioning */
    relpos(rtk,obs,nu,nr,nav);
    outsolstat(rtk);
    
    return 1;
}

1.2?pntpos( )函数

解算接收机(流动站接收机)位置、速度、钟差
* args ? : obsd_t *obs ? ? ?I ? observation data 观测值
* ? ? ? ? ?int ? ?n ? ? ? ? I ? number of observation data 卫星数
* ? ? ? ? ?nav_t ?*nav ? ? ?I ? navigation data 星历
* ? ? ? ? ?prcopt_t *opt ? ?I ? processing options 配置参数
* ? ? ? ? ?sol_t ?*sol ? ? ?IO ?solution
* ? ? ? ? ?double *azel ? ? IO ?azimuth/elevation angle (rad) (NULL: no output) 方位角/高度角
* ? ? ? ? ?ssat_t *ssat ? ? IO ?satellite status ? ? ? ? ? ? ?(NULL: no output) 卫星状态
* ? ? ? ? ?char ? *msg ? ? ?O ? error message for error exit

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)
{
    prcopt_t opt_=*opt;
    double *rs,*dts,*var,*azel_,*resp; //定义的指针，为了后面申请内存使用，二维数组。
    int i,stat,vsat[MAXOBS]={0},svh[MAXOBS];
    //先设置没有输出
    sol->stat=SOLQ_NONE;
    //判断有没有卫星，没有直接返回
    if (n<=0) {strcpy(msg,"no observation data"); return 0;}
    
    trace(3,"pntpos  : tobs=%s n=%d\n",time_str(obs[0].time,3),n);
    //获取该历元时间。因为都是这个历元的数据，下标无所谓
    sol->time=obs[0].time; msg[0]='\0';
    //申请内存，数组
    //mat() malloc一段内存。每个数组含义，后面有解释
    rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n);
    //如果不是单点定位，启用电离层模型纠正
    if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */
#if 0
        opt_.sateph =EPHOPT_BRDC;
#endif
        opt_.ionoopt=IONOOPT_BRDC;
        opt_.tropopt=TROPOPT_SAAS;
    }
    /* satellite positons, velocities and clocks  计算卫星位置、速度和时间*/
    satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh);
    
    /* estimate receiver position with pseudorange  评价精度*/
    stat=estpos(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
    
    /* 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);
    }
    /* estimate receiver velocity with doppler */
    if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat);
    
    if (azel) {
        for (i=0;i<n*2;i++) azel[i]=azel_[i];
    }
    //
    if (ssat) {
        for (i=0;i<MAXSAT;i++) {
            ssat[i].vs=0;
            ssat[i].azel[0]=ssat[i].azel[1]=0.0;
            ssat[i].resp[0]=ssat[i].resc[0]=0.0;
            ssat[i].snr[0]=0;
        }
        for (i=0;i<n;i++) {
            ssat[obs[i].sat-1].azel[0]=azel_[  i*2];
            ssat[obs[i].sat-1].azel[1]=azel_[1+i*2];
            ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0];
            if (!vsat[i]) continue;
            ssat[obs[i].sat-1].vs=1;
            ssat[obs[i].sat-1].resp[0]=resp[i];
        }
    }
    free(rs); free(dts); free(var); free(azel_); free(resp);
    return stat;
}

1.3 mat( )函数

malloc( )申请内存。在pntpos()函数中，申请的是m*n大小的空间，二维数组。但我们需要清除，这其实是内存的一段连续空间

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

?n是卫星数量。推测rs是每颗卫星包含的6种信息，dts是卫星包含的2种信息.....

rs：x、y、z、vx、vy、vz??satellite positions and velocities

dts：钟、钟piao? ?satellite clocks

var：方差，精度。?sat position and clock error variances (m^2)

azel_：高度角/方位角

resp：sat health flag (-1:correction not available)

extern double *mat(int n, int m)
{
    double *p;
    
    if (n<=0||m<=0) return NULL;
    if (!(p=(double *)malloc(sizeof(double)*n*m))) {
        fatalerr("matrix memory allocation error: n=%d,m=%d\n",n,m);
    }
    return p;
}

1.4 satposs()函数

计算的是卫星信号对应的卫星发射时刻的卫星位置、速度，所以我们得先计算出卫星发射时刻是多少。因为传时间参数--teph是接收机接收时刻，我们需要使用伪距去推算卫星发射时刻。

• 卫星发射信号时刻，理论计算

[1]?卫星钟差假设没有，只考虑接收机钟差。但是(接收机钟差、真实卫地距、传播时间)我们不知道，需要把这个未知量代换

[2]?伪距观测方程

我们不考虑电离层、对流层、噪声影响。

[3]?推导，由伪距观测方方程。卫星钟差可以通过导航电文获得，伪距也有

extern void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,
                    int ephopt, double *rs, double *dts, double *var, int *svh)
{
    gtime_t time[MAXOBS]={{0}};
    double dt,pr;
    int i,j;
    //teph是时间。每包数据都有个时间，这个时间是接收机捕获观测值时，接收机的钟面时刻
    trace(3,"satposs : teph=%s n=%d ephopt=%d\n",time_str(teph,3),n,ephopt);
    
    for (i=0;i<n&&i<MAXOBS;i++) {
        for (j=0;j<6;j++) rs [j+i*6]=0.0;
        for (j=0;j<2;j++) dts[j+i*2]=0.0;
        var[i]=0.0; svh[i]=0;
        /* search any psuedorange 伪距 */
        //选择任意伪距值即可
        for (j=0,pr=0.0;j<NFREQ;j++) if ((pr=obs[i].P[j])!=0.0) break;
        
        if (j>=NFREQ) {
            trace(2,"no pseudorange %s sat=%2d\n",time_str(obs[i].time,3),obs[i].sat);
            continue;
        }
        //计算卫星发射时刻

        /* transmission time by satellite clock */
        time[i]=timeadd(obs[i].time,-pr/CLIGHT); //pr是伪距
        /* satellite clock bias by broadcast ephemeris  通过广播星历获取卫星钟钟差*/
        if (!ephclk(time[i],teph,obs[i].sat,nav,&dt)) {
            trace(2,"no broadcast clock %s sat=%2d\n",time_str(time[i],3),obs[i].sat);
            continue;
        }
        time[i]=timeadd(time[i],-dt); //至此，我们已经得出卫星发射时刻
        
        /* satellite position and clock at transmission time 卫星信号发射时刻，卫星位置计算 */
        if (!satpos(time[i],teph,obs[i].sat,ephopt,nav,rs+i*6,dts+i*2,var+i,
                    svh+i)) {
            trace(2,"no ephemeris %s sat=%2d\n",time_str(time[i],3),obs[i].sat);
            continue;
        }
        /* if no precise clock available, use broadcast clock instead */
        if (dts[i*2]==0.0) {
            if (!ephclk(time[i],teph,obs[i].sat,nav,dts+i*2)) continue;
            dts[1+i*2]=0.0;
            *var=SQR(STD_BRDCCLK);
        }
    }
    for (i=0;i<n&&i<MAXOBS;i++) {
        trace(4,"%s sat=%2d rs=%13.3f %13.3f %13.3f dts=%12.3f var=%7.3f svh=%02X\n",
              time_str(time[i],6),obs[i].sat,rs[i*6],rs[1+i*6],rs[2+i*6],
              dts[i*2]*1E9,var[i],svh[i]);
    }
}

1.5 satpos()

卫星位置和钟解算。我们只针对广播星历进行介绍，ephpos()

1.6?ephpos()

两次调用eph2pos()，计算卫星位置。第一次调用是卫星信号发射时刻的卫星位置，第二次调用是间隔tt事件后的位置。位移差，除以tt就得出速度

eph2pos()是一个接口，broadcast ephemeris to satellite position and clock bias。由广播星历得出卫星位置和钟差

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);//调用两次，求解tt时间间隔的位置。之后rst-rs得出位移变化，再除以tt求出速度。后面这个是1ms后的位置
        *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; //速度计算。i+3，对应着[x,y,z,vx,vy,vz] 从vx开始
    dts[1]=(dtst[0]-dts[0])/tt; //钟piao计算
    
    return 1;
}

?