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

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:先验残差,具体可见estpos()函数详解,后文会说明

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

2 估计接收机位置

pntpos()函数下,解算出卫星位置速度,就解算接收机位置。先验残差推导,相关理论知识见下

单点定位技术理论推导-CSDN博客

2.1 流程图

2.2 estpos()函数

 需要注意:rtklib中的矩阵是按列存的,和我们日常所使用的呈转置关系

/*  没有const,一般是输出
    obs  观测值
    n    卫星数量
    rs   卫星位置和速度(x/y/z/vx/vy/vz)
    dts  钟和钟piao!!
    vare 卫星位置和钟的方差
    svh  卫星健康
    nav  星历
    azel 高度角/方位角
    vast 卫星是否可用
    resp 卫星先验残差
    msg  错误信息(Debug使用)

static int estpos(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)
{
    //NX 4+3,4是(xyz钟),3是其他系统??
    double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig;
    int i,j,k,info,stat,nv,ns;
    
    trace(3,"estpos  : n=%d\n",n);
    //v 先验残差    H 设计矩阵  var 方差
    v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1);
    //数组x就是接收机概略位置,赋予rr前三个元素(xyz)(0,0,0)
    for (i=0;i<3;i++) x[i]=sol->rr[i];
    //迭代,使得概略位置逐渐精确
    //MAXITR最大迭代次数
    for (i=0;i<MAXITR;i++) {
        
        /* pseudorange residuals 伪距残差*/
        nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
                   &ns);
        //观测值(卫星数/方程数)少于未知数(xyz+系统钟),就无解直接退出
        if (nv<NX) {
            sprintf(msg,"lack of valid sats ns=%d",nv);
            break;
        }
        /* weight by variance */
        for (j=0;j<nv;j++) {
            sig=sqrt(var[j]);
            v[j]/=sig;
            for (k=0;k<NX;k++) H[k+j*NX]/=sig;
        }
        /* least square estimation 最小二乘法 */
        if ((info=lsq(H,v,NX,nv,dx,Q))) {
            sprintf(msg,"lsq error info=%d",info);
            break;
        }
        for (j=0;j<NX;j++) x[j]+=dx[j]; //更新位置
        
        //收敛判断,当dx[]变化少于一定值
        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) */
            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=(unsigned char)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;
            }
            free(v); free(H); free(var);
            
            return stat;
        }
    }
    if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);
    
    free(v); free(H); free(var);
    
    return 0;
}

2.3 rescode( )

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);
    //rr[]局部变量--接收机概略位置,赋予给rr[]
    //最开始概略位置rr[]={0,0,0} 钟也是0
    for (i=0;i<3;i++) rr[i]=x[i]; dtr=x[3];
    //xyz-->BLH 坐标转换
    ecef2pos(rr,pos); //这里并不是改正rr[]的值,而是产生的一个pos[]这里面是BLH,rr[]是xyz,都表示同一个位置
    //遍历所有卫星
    for (i=*ns=0;i<n&&i<MAXOBS;i++) {
        vsat[i]=0; azel[i*2]=azel[1+i*2]=resp[i]=0.0; //azel有高度角和方位角,所有*2
        
        if (!(sys=satsys(obs[i].sat,NULL))) continue; //判断是那个导航系统GPS/GLOS/BDL
        
        /* reject duplicated observation data 去掉重复观测数据*/
        //依据的是sat是否相同,sat是卫星编号
        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 
           geodist()卫星与概率位置距离r计算+方向余弦e计算
           satazel()高度角/方位角计算
        */
        if ((r=geodist(rs+i*6,rr,e))<=0.0||
            satazel(pos,e,azel+i*2)<opt->elmin) continue;
        
        /* psudorange with code bias correction */
        //码偏差修正,P是经过修正的伪距
        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 */
        //dtr是接收机钟的概略。不懂!!为什么减去dtr
        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 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)++; //resp收的是先验残差 
        
        /* error variance */
        //方差 精度,30分钟!!
        //varerr()观测值误差   vare卫星位置误差和钟差  vmeans 码偏差修正方差  vion 电离层  vtrp对流层
        //var[]伪距方差,都是方差加和
        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;
}

设计矩阵构建

每行前三列都是dx,dy,dz的系数,第四列系数是1.

后面几列是其他系统的相对于GPS钟的钟差

mask[ ]标记是否有该卫星系统的值

添加约数

循环四次(对应四个导航系统)GPS/GLA/BDS/GLO。根据mask[ ]标记,在没有值的那一列,填个1

2.4 geodist( )函数

* args   : double *rs       I   satellilte position (ecef at transmission) (m)
*          double *rr       I   receiver position (ecef at reception) (m)
*          double *e        O   line-of-sight vector (ecef)

e是单位向量,表示卫星与概率位置的方向

地球自转修正借鉴如下公式

{
    double r;
    int i;
    //norm()距离计算,卫星距地心距离,判断是否小于地球半径。小于地球半径,说明就不是卫星
    if (norm(rs,3)<RE_WGS84) return -1.0;
    for (i=0;i<3;i++) e[i]=rs[i]-rr[i]; //Xs-Xo
    r=norm(e,3);//卫星与概略位置距离
    for (i=0;i<3;i++) e[i]/=r;//方向余弦计算
    return r+OMGE*(rs[0]*rr[1]-rs[1]*rr[0])/CLIGHT;//后面是地球自转改正,这里只是把卫地距换成了卫星与概略位置的距离
}

rs[0]是卫星x

rr[1]是概略位置y,依次类推

2.5 satazel( ) 高度角方位角

* args   : double *pos      I   geodetic position {lat,lon,h} (rad,m)  卫星位置的BLH
*          double *e        I   receiver-to-satellilte unit vevtor (ecef)
*          double *azel     IO  azimuth/elevation {az,el} (rad) (NULL: no output)

extern double satazel(const double *pos, const double *e, double *azel)
{
    double az=0.0,el=PI/2.0,enu[3];
    //图示计算方法
    if (pos[2]>-RE_WGS84) {
        ecef2enu(pos,e,enu);//xyz-->enu,产生了enu[],pos[]值并没有变
        az=dot(enu,enu,2)<1E-12?0.0:atan2(enu[0],enu[1]);//方位角
        if (az<0.0) az+=2*PI;
        el=asin(enu[2]);//高度角
    }
    if (azel) {azel[0]=az; azel[1]=el;}
    return el;
}

2.6 ionocorr( )电离层修正

根据配置,选择相应的模型进行修正

* args   : gtime_t time     I   time
*          nav_t  *nav      I   navigation data 星历,其中有电离层参数
*          int    sat       I   satellite number
*          double *pos      I   receiver position {lat,lon,h} (rad|m) 接收机位置
*          double *azel     I   azimuth/elevation angle {az,el} (rad)
*          int    ionoopt   I   ionospheric correction option (IONOOPT_???) 电离层模型--配置项
*          double *ion      O   ionospheric delay (L1) (m) 输出电离层
*          double *var      O   ionospheric delay (L1) variance (m^2) 电离层方差

extern int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos,
                    const double *azel, int ionoopt, double *ion, double *var)
{
    trace(4,"ionocorr: time=%s opt=%d sat=%2d pos=%.3f %.3f azel=%.3f %.3f\n",
          time_str(time,3),ionoopt,sat,pos[0]*R2D,pos[1]*R2D,azel[0]*R2D,
          azel[1]*R2D);
    
    /* broadcast model */
    if (ionoopt==IONOOPT_BRDC) {
        *ion=ionmodel(time,nav->ion_gps,pos,azel);
        *var=SQR(*ion*ERR_BRDCI);
        return 1;
    }
    /* sbas ionosphere model */
    if (ionoopt==IONOOPT_SBAS) {
        return sbsioncorr(time,nav,pos,azel,ion,var);
    }
    /* ionex tec model */
    if (ionoopt==IONOOPT_TEC) {
        return iontec(time,nav,pos,azel,1,ion,var);
    }
    /* qzss broadcast model */
    if (ionoopt==IONOOPT_QZS&&norm(nav->ion_qzs,8)>0.0) {
        *ion=ionmodel(time,nav->ion_qzs,pos,azel);
        *var=SQR(*ion*ERR_BRDCI);
        return 1;
    }
    /* lex ionosphere model */
    if (ionoopt==IONOOPT_LEX) {
        return lexioncorr(time,nav,pos,azel,ion,var);
    }
    *ion=0.0;
    *var=ionoopt==IONOOPT_OFF?SQR(ERR_ION):0.0;
    return 1;
}

2.7 tropcorr( ) 对流层

同上

2.8 prange( )伪距码偏差修正

略,还不太懂

2.9 varerr( ) 方差

观测值误差

varr=SQR(opt->err[0])*(SQR(opt->err[1])+SQR(opt->err[2])/sin(el)); //观测值误差

针对这个,我们通常采用如下定义观测值误差

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); //不同系统,权重是不一样的
    varr=SQR(opt->err[0])*(SQR(opt->err[1])+SQR(opt->err[2])/sin(el)); //观测值误差
    if (opt->ionoopt==IONOOPT_IFLC) varr*=SQR(3.0); /* iono-free */
    return SQR(fact)*varr;
}

2.10 lsq( )最小二乘法

solving normal equation (x=(A*A')^-1*A*y)  这里的计算公式不同,是因为rtklib的矩阵按列存储,所以和计算公式相反

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.11 raim_fde( )接收机自主性检验

遍历所有卫星,一个一个卫星挨个删除,再执行单点定位,后验残差最小的作为定位结果。

这个方法可以除去粗差较大卫星的影响。缺点是只能检测单个不良卫星

2.12 valsol( )检验定位结果准确

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);
    //判断精度,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;
}

3 估计接收机速度

3.1 estvel( )

思路和单点定位差不多,构建设计矩阵、残差阵。使用最小二乘逼近速度

 

/* estimate receiver velocity ------------------------------------------------*/
static void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
                   const nav_t *nav, const prcopt_t *opt, sol_t *sol,
                   const double *azel, const int *vsat)
{
    double x[4]={0},dx[4],Q[16],*v,*H;
    int i,j,nv;
    
    trace(3,"estvel  : n=%d\n",n);
    
    v=mat(n,1); H=mat(4,n);
    
    for (i=0;i<MAXITR;i++) {
        
        /* doppler residuals */
        //re-->卫星速度   x-->接收机速度,从0开始,和单点定位一样,逐渐逼近
        if ((nv=resdop(obs,n,rs,dts,nav,sol->rr,x,azel,vsat,v,H))<4) {
            break;
        }
        /* least square estimation 最小二乘,参数估计速度变化 */
        if (lsq(H,v,4,nv,dx,Q)) break;
        
        for (j=0;j<4;j++) x[j]+=dx[j];
        //达到一定精度时,填充到sol结构体中。
        if (norm(dx,4)<1E-6) {
            for (i=0;i<3;i++) sol->rr[i+3]=x[i];
            //其实后面还有,就是xx,xy,yy等协方差
            break;
        }
    }
    free(v); free(H);
}

3.2 resdop( )

由高度角方位角计算设计矩阵--方向余弦

/* 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]);//高度角 cos
        a[0]=sin(azel[i*2])*cosel;//OA*sin(方位角) A`A``  e
        a[1]=cos(azel[i*2])*cosel;//n
        a[2]=sin(azel[1+i*2]);    //u
        matmul("TN",3,1,3,1.0,E,a,0.0,e); //a是当地enu设计矩阵。enu转ecef的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]); //加负号 (v-vs)-->-(vs-v)
        
        /* design matrix 设计矩阵*/
        for (j=0;j<4;j++) H[j+nv*4]=j<3?-e[j]:1.0;
        
        nv++;
    }
    return nv;
}

  • 21
    点赞
  • 37
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
### 回答1: 您好!我可以用中文回答您的问题。 Kalman滤波器是一种常用的控制理论中的滤波器,用于估计具有噪声的系统的状态。它在工程和科学领域中都得到了广泛的应用,如航天、导航、机器人等。 Kalman滤波器的基本思想是根据系统的动态方程和测量方程,通过贝叶斯定理来估计系统的状态。它可以将不准确的测量值和先前的状态估计值进行加权平均,从而得到更精确的状态估计值。 关于Kalman单点定位算法,这是一种用于对移动对象进行定位算法。它结合了GPS测量和惯性导航测量,通过对测量值进行加权平均来提高定位精度。 希望能够帮助您了解Kalman滤波器和单点定位算法。如果您有其他问题,欢迎随时向我提出。 ### 回答2: Kalman单点定位算法是一种常用的滤波算法,用于估计物体或者车辆的位置。它通过结合测量值和系统模型,可以准确地估计出物体的实际位置,并可以追踪位置的变化。 Kalman单点定位算法的基本原理是将物体的位置看作一个状态变量,并假设这个状态变量受到测量误差和系统噪声的影响。然后,通过线性方程组来描述系统的动态模型,并利用观测方程来描述测量模型。通过这两个方程,可以得到物体位置的预测值和测量值之间的协方差矩阵。 在实际应用中,Kalman单点定位算法可以应用于各种领域,例如车辆导航系统、无人机自主飞行等。它的优点是能够估计物体位置的不确定性,并且可以根据测量值的准确性自动调整权重,使得估计结果更加准确可靠。 值得注意的是,Kalman单点定位算法需要事先了解系统的动态模型和测量模型,并对噪声进行合理的建模。如果模型不准确或者噪声较大,算法的估计结果可能会存在误差。因此,在实际应用中,需要根据具体情况进行参数调优和误差修正,以获得更好的定位精度。 总之,Kalman单点定位算法是一种基于滤波的定位算法,能够准确估计物体的位置,并且具有自适应能力。它在导航、控制和自主系统等领域有广泛的应用。 ### 回答3: 卡尔曼单点定位算法(Kalman Single Point Positioning Algorithm)是一种基于卡尔曼滤波器的定位算法。它适用于利用已知的观测数据,通过测量值和预测值之间的比较,进行位置估计和优化的过程。 卡尔曼单点定位算法的基本原理是通过建立状态模型和观测模型,并使用卡尔曼滤波器对系统状态进行估计和校正。在定位过程中,系统状态包括位置、速度、加速度等参数,观测数据则由GPS或其他定位系统提供。通过对观测数据和预测值进行加权平均,可以得到更准确的位置估计结果。 具体来说,卡尔曼滤波器通过预测-校正的方式对系统状态进行更新。预测过程通过利用系统的动力学方程对当前状态进行预测,并估计预测误差的协方差。校正过程则通过对观测数据进行加权平均,根据观测数据的精度和预测误差的协方差来更新状态估计值和协方差。 卡尔曼单点定位算法具有以下几个优点: 1. 高精度:通过将观测数据和预测值进行加权平均,可以得到更准确的位置估计结果。 2. 实时性:卡尔曼滤波器的计算效率较高,可以实时更新状态估计值。 3. 自适应性:卡尔曼滤波器可以根据观测数据和预测值的准确性自动调整权重,适应不同的定位环境。 卡尔曼单点定位算法在实际应用中被广泛使用,尤其在无人机、自动驾驶、导航等领域具有重要意义。它通过利用已有的观测数据进行位置估计和优化,可以提高定位精度和可靠性。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值