rtklib---RTK代码流程

dsx08

已于 2024-03-17 21:30:55 修改

阅读量2.2k

点赞数 24

CC 4.0 BY-SA版权

分类专栏： GNSS 文章标签： c语言

于 2024-03-15 17:31:51 首次发布

本文链接：https://blog.csdn.net/qq_58309598/article/details/136742739

GNSS 专栏收录该内容

6 篇文章

订阅专栏

该博客主要用C语言实现RTK相关计算。介绍了整体框架，包括relpos函数、rtkinit初始化及nx和na求法，还有中间信息文件输出；阐述基准站单站残差计算，涉及主函数、多种改正及残差计算；最后说明了共识卫星的查找方法，即找出基准站和流动站共同观测到的卫星。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

1 整体框架

1.1 relpos( )

rs---卫星位置/速度
dts---卫星钟/钟差
var---方差
y---残差
e---设计矩阵x/y/z系数
azel---高度角方位角
v---残差
H---设计矩阵x/y/z/钟等全部系数
R---方差-协方差阵
xp---KF中间量

static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
                  const nav_t *nav)
{
    prcopt_t *opt=&rtk->opt;
    gtime_t time=obs[0].time;
    double *rs,*dts,*var,*y,*e,*azel,*freq,*v,*H,*R,*xp,*Pp,*xa,*bias,dt;
    int i,j,f,n=nu+nr,ns,ny,nv,sat[MAXSAT],iu[MAXSAT],ir[MAXSAT],niter;
    int info,vflg[MAXOBS*NFREQ*2+1],svh[MAXOBS*2];
    int stat=rtk->opt.mode<=PMODE_DGPS?SOLQ_DGPS:SOLQ_FLOAT;
    int nf=opt->ionoopt==IONOOPT_IFLC?1:opt->nf;
    
    trace(3,"relpos  : nx=%d nu=%d nr=%d\n",rtk->nx,nu,nr);
    
    dt=timediff(time,obs[nu].time);
    
    rs=mat(6,n); dts=mat(2,n); var=mat(1,n); y=mat(nf*2,n); e=mat(3,n);
    azel=zeros(2,n); freq=zeros(nf,n);
    
    for (i=0;i<MAXSAT;i++) {
        rtk->ssat[i].sys=satsys(i+1,NULL);
        for (j=0;j<NFREQ;j++) rtk->ssat[i].vsat[j]=0;
        for (j=1;j<NFREQ;j++) rtk->ssat[i].snr [j]=0;
    }
    /* satellite positions/clocks 卫星位置/钟*/
    satposs(time,obs,n,nav,opt->sateph,rs,dts,var,svh);
    
    /* UD (undifferenced) residuals for base station 基准站非差残差*/
    if (!zdres(1,obs+nu,nr,rs+nu*6,dts+nu*2,var+nu,svh+nu,nav,rtk->rb,opt,1,
               y+nu*nf*2,e+nu*3,azel+nu*2,freq+nu*nf)) {
        errmsg(rtk,"initial base station position error\n");
        
        free(rs); free(dts); free(var); free(y); free(e); free(azel);
        free(freq);
        return 0;
    }
    /* time-interpolation of residuals (for post-processing) */
    if (opt->intpref) {
        dt=intpres(time,obs+nu,nr,nav,rtk,y+nu*nf*2);
    }
    /* select common satellites between rover and base-station 选择基准站和流动站共识卫星 */
    if ((ns=selsat(obs,azel,nu,nr,opt,sat,iu,ir))<=0) {
        errmsg(rtk,"no common satellite\n");
        
        free(rs); free(dts); free(var); free(y); free(e); free(azel);
        free(freq);
        return 0;
    }
    /* temporal update of states 状态一步预测--卡尔曼 */
    udstate(rtk,obs,sat,iu,ir,ns,nav);
    
    trace(4,"x(0)="); tracemat(4,rtk->x,1,NR(opt),13,4);
    
    xp=mat(rtk->nx,1); Pp=zeros(rtk->nx,rtk->nx); xa=mat(rtk->nx,1);
    matcpy(xp,rtk->x,rtk->nx,1);
    
    ny=ns*nf*2+2;
    v=mat(ny,1); H=zeros(rtk->nx,ny); R=mat(ny,ny); bias=mat(rtk->nx,1);
    
    /* add 2 iterations for baseline-constraint moving-base */
    niter=opt->niter+(opt->mode==PMODE_MOVEB&&opt->baseline[0]>0.0?2:0);
    
    for (i=0;i<niter;i++) {
        /* UD (undifferenced) residuals for rover 流动站非差残差*/
        if (!zdres(0,obs,nu,rs,dts,var,svh,nav,xp,opt,0,y,e,azel,freq)) {
            errmsg(rtk,"rover initial position error\n");
            stat=SOLQ_NONE;
            break;
        }
        /* DD (double-differenced) residuals and partial derivatives 站星双差和设计矩阵*/
        if ((nv=ddres(rtk,nav,dt,xp,Pp,sat,y,e,azel,freq,iu,ir,ns,v,H,R,
                      vflg))<1) {
            errmsg(rtk,"no double-differenced residual\n");
            stat=SOLQ_NONE;
            break;
        }
        /* Kalman filter measurement update 卡尔曼滤波*/
        matcpy(Pp,rtk->P,rtk->nx,rtk->nx);
        if ((info=filter(xp,Pp,H,v,R,rtk->nx,nv))) {
            errmsg(rtk,"filter error (info=%d)\n",info);
            stat=SOLQ_NONE;
            break;
        }
        trace(4,"x(%d)=",i+1); tracemat(4,xp,1,NR(opt),13,4);
    }


    if (stat!=SOLQ_NONE&&zdres(0,obs,nu,rs,dts,var,svh,nav,xp,opt,0,y,e,azel,
                               freq)) {
        
        /* post-fit residuals for float solution 后验残差 */
        nv=ddres(rtk,nav,dt,xp,Pp,sat,y,e,azel,freq,iu,ir,ns,v,NULL,R,vflg);
        
        /* validation of float solution 后验残差检验*/
        if (valpos(rtk,v,R,vflg,nv,4.0)) {
            
            /* update state and covariance matrix 更新状态和转移矩阵*/
            matcpy(rtk->x,xp,rtk->nx,1);
            matcpy(rtk->P,Pp,rtk->nx,rtk->nx);
            
            /* update ambiguity control struct */
            rtk->sol.ns=0;
            for (i=0;i<ns;i++) for (f=0;f<nf;f++) {
                if (!rtk->ssat[sat[i]-1].vsat[f]) continue;
                rtk->ssat[sat[i]-1].lock[f]++;
                rtk->ssat[sat[i]-1].outc[f]=0;
                if (f==0) rtk->sol.ns++; /* valid satellite count by L1 */
            }
            /* lack of valid satellites */
            if (rtk->sol.ns<4) stat=SOLQ_NONE;
        }
        else stat=SOLQ_NONE;
    }
//后面就是存储结果了
    .....
    .....
    .....
}

1.2 rtkinit( ) ---初始化

initialize RTK control struct

int nx,na; /* number of float states/fixed states 浮点解和固定解状态量个数*/

double *x, *P; /* float states and their covariance 浮点状态存储和方差*/

extern void rtkinit(rtk_t *rtk, const prcopt_t *opt)
{
    sol_t sol0={{0}};
    ambc_t ambc0={{{0}}};
    ssat_t ssat0={0};
    int i;
    
    trace(3,"rtkinit :\n");
    
    rtk->sol=sol0;
    for (i=0;i<6;i++) rtk->rb[i]=0.0;
    rtk->nx=opt->mode<=PMODE_FIXED?NX(opt):pppnx(opt); //NX比NR多NB个
    rtk->na=opt->mode<=PMODE_FIXED?NR(opt):pppnx(opt);
    rtk->tt=0.0;
    //根据nx、na申请内存。rtklib用的都是最大卫星数，会造成不必要的空间浪费
    rtk->x=zeros(rtk->nx,1);
    rtk->P=zeros(rtk->nx,rtk->nx);
    rtk->xa=zeros(rtk->na,1);
    rtk->Pa=zeros(rtk->na,rtk->na);
    rtk->nfix=rtk->neb=0;
    for (i=0;i<MAXSAT;i++) {
        rtk->ambc[i]=ambc0;
        rtk->ssat[i]=ssat0;
    }
    for (i=0;i<MAXERRMSG;i++) rtk->errbuf[i]=0;
    rtk->opt=*opt;
}

1.2.1 nx和na的求法

先不考虑PPP

    rtk->nx=opt->mode<=PMODE_FIXED?NX(opt):pppnx(opt); //NX比NR多NB个
    rtk->na=opt->mode<=PMODE_FIXED?NR(opt):pppnx(opt);

可知，nx比na多NB( ) 。这就是两者的差别

#define NR(opt) (NP(opt)+NI(opt)+NT(opt)+NL(opt))
#define NX(opt) (NR(opt)+NB(opt))

当<=PMODE_DGPS时，说明就是伪距单点或差分，不用模糊度固定，那么就是0

MAXSAT*NF(opt))，所有卫星*每个卫星参与解算频点数。其中MAXSAT是所有系统卫星的数量。假如总得卫星是200颗，虽然你只能观测到40颗，那MAXSAT仍为200。这种做法，省去了统计观测卫星数麻烦，但是占用大量内存。NF( )是参与解算的频点数量( L1,L1+2,L1+2+5等)。

#define NB(opt) ((opt)->mode<=PMODE_DGPS?0:MAXSAT*NF(opt))

无电离层组合，那就是1；非差，是参与的频点数

#define NF(opt) ((opt)->ionoopt==IONOOPT_IFLC?1:(opt)->nf)

int nf; /* number of frequencies (1:L1,2:L1+L2,3:L1+L2+L5) */

是否为动态模型。3--position 9--位置、速度、加速度

#define NP(opt) ((opt)->dynamics==0?3:9)

IONOOPT_EST是估计电离层，长基线适用。最大卫星数，每个频率的电离层延迟不一样，但是不同频率有相关性，所以只需要每颗卫星第1频点就可

#define NI(opt) ((opt)->ionoopt!=IONOOPT_EST?0:MAXSAT)

对流层。不是估计--0；是估计--（根据估计的方法）2/6

#define NT(opt) ((opt)->tropopt<TROPOPT_EST?0:((opt)->tropopt<TROPOPT_ESTG?2:6))

启用GLONASS？

#define NL(opt) ((opt)->glomodear!=2?0:NFREQGLO)

1.3 中间信息文件(.stat)输出

outsolstat(rtk)-----＞rtkoutstat( )

卫星的中间过程信息，ssat结构体中的。高度角、方位角、伪距残差啥的。

一些中间的位置、速度参数、电离层对流层啥。以后用的时候再看

2 基准站单站残差计算--zdres( )

2.1 主函数--zdres( )

基准站非差残差
nu是流动站个数，流动站和基准站观测值都在obs[ ]中，前半是流动，后半是基准，所以我们处理基准站数据需要移动指针
obs+nu，将指针移到第一个基准站
这都是一维仿多维，所以移下标就有nu*2，nu*nf*2

nf是选择的频点
nu*nf*2--每个卫星每个频点，×2是因为有伪距和载波

zdres(1,obs+nu,nr,rs+nu*6,dts+nu*2,var+nu,svh+nu,nav,rtk->rb,opt,1,
y+nu*nf*2,e+nu*3,azel+nu*2,freq+nu*nf)

/*
index 和天线改正有关
n 卫星数量
rs 卫星位置和速度
dts 卫星钟和钟差
var 方差
svh 卫星健康状态
nav 星历
rr 接收机位置(一般概略位置)xyz
y 残差
e 设计矩阵XYZ
freq 选择的频点
base 基准站--1 ；流动站---0

*/

static int zdres(int base, const obsd_t *obs, int n, const double *rs,
                 const double *dts, const double *var, const int *svh,
                 const nav_t *nav, const double *rr, const prcopt_t *opt,
                 int index, double *y, double *e, double *azel, double *freq)
{
    double r,rr_[3],pos[3],dant[NFREQ]={0},disp[3];
    double zhd,zazel[]={0.0,90.0*D2R};
    int i,nf=NF(opt);
    
    trace(3,"zdres   : n=%d\n",n);
    //初始化残差
    for (i=0;i<n*nf*2;i++) y[i]=0.0;
    
    if (norm(rr,3)<=0.0) return 0; /* no receiver position，没有接收机位置就返回 */
    //复制一份，因为后面在固体潮改正时会有改动
    for (i=0;i<3;i++) rr_[i]=rr[i];
    
    /* 
        earth tide correction 固体海洋潮改正
        基准站和流动站相近，影响一样，作差可消除。所以RTK中一般不改正
        PPP要改
    */
    if (opt->tidecorr) {
        tidedisp(gpst2utc(obs[0].time),rr_,opt->tidecorr,&nav->erp,
                 opt->odisp[base],disp);
        for (i=0;i<3;i++) rr_[i]+=disp[i];//修正xyz方向坐标
    }
    ecef2pos(rr_,pos);
    //遍历每一颗卫星
    for (i=0;i<n;i++) {
        /* compute geometric-range and azimuth/elevation angle */
        if ((r=geodist(rs+i*6,rr_,e+i*3))<=0.0) continue; //与概略位置距离
        if (satazel(pos,e+i*3,azel+i*2)<opt->elmin) continue;//高度角方位角
        
        /* excluded satellite? */
        if (satexclude(obs[i].sat,var[i],svh[i],opt)) continue;
        
        /* satellite clock-bias 参考先验残差计算公式*/
        r+=-CLIGHT*dts[i*2];
        
        /* troposphere delay model (hydrostatic)对流层 很少改正，理由同上 */
        zhd=tropmodel(obs[0].time,pos,zazel,0.0);
        r+=tropmapf(obs[i].time,pos,azel+i*2,NULL)*zhd;
        
        /* 
            receiver antenna phase center correction 天线相位中心改正 
            antdel[0]是流动站天线中心偏差 [1]是基准站天线中心偏差
            对于RTK天线改正可有可无，PPP是一定要有的
        */
        antmodel(opt->pcvr+index,opt->antdel[index],azel+i*2,opt->posopt[1],
                 dant);
        
        /* 
            UD phase/code residual for satellite 
            卫星残差计算
            针对的是卫星的每个频点的载波和伪距(2*nf)
            dant    每个频点的距离改正
        */
        zdres_sat(base,r,obs+i,nav,azel+i*2,dant,opt,y+i*nf*2,freq+i*nf);
    }
    
    return 1;
}

2.2 固体/海洋潮改正--tidedisp

RTK中一般不作要求,PPP要改

返回值是disp，displacement by earth tides (ecef) (m) 返回--xyz方向下的改正

2.3 对流层电离层改正

中短基线，基准站和流动站很近，作差可消除。这里暂时不讨论

2.4 天线相位中心改正--antmodel

antdel[0]是流动站天线中心偏差 [1]是基准站天线中心偏差
对于RTK天线改正可有可无，PPP是一定要有的

* compute antenna offset by antenna phase center parameters
* args   : pcv_t *pcv       I   antenna phase center parameters 天线相位中心参数
*          double *del      I   antenna delta {e,n,u} (m)  天线相位中心相对于标志点的enu方向偏差
*          double *azel     I   azimuth/elevation for receiver {az,el} (rad) 高度角方位角
*          int     opt      I   option (0:only offset,1:offset+pcv)
*          double *dant     O   range offsets for each frequency (m) 每个频点的距离偏移。因为不同频率的天线偏差不一样(码偏差)

extern void antmodel(const pcv_t *pcv, const double *del, const double *azel,int opt,double *dant)

注意返回，dant。每个频点的天线相位改正，因为不同频率不一样（码偏差）

2.5 残差计算--zdres_sa

我们注意到，如果是无电离层组合，默认使用的是第1/2频点的观测值。如果我们想做L1+L3的无电离层组合，需要自行改写代码

只讨论下面电离层非差组合的代码

在这个函数中遍历了卫星的所有频点。我们知道在主函数zdres( )中遍历的是所有卫星，所以最后处理的是每颗卫星的所有频点

        //遍历每一个频点
        for (i=0;i<nf;i++) {
            if ((freq[i]=sat2freq(obs->sat,obs->code[i],nav))==0.0) continue; //频点的频率
            
            /* check SNR mask 信噪比检验 */
            if (testsnr(base,i,azel[1],obs->SNR[i]*SNR_UNIT,&opt->snrmask)) {
                continue;
            }
            /* 
                residuals = observable - pseudorange 残差计算
                残差=观测-概略位置伪距。？？？残差应该是真实值和观测值之差。这个好奇怪，以后再说
                2*nf是因为有载波和伪距
                y[]前半部分存载波，后半部分是伪距残差
            */
            if (obs->L[i]!=0.0) y[i   ]=obs->L[i]*CLIGHT/freq[i]-r-dant[i];//载波以波长存储，所以有C/frq(速度/频率)
            if (obs->P[i]!=0.0) y[i+nf]=obs->P[i]               -r-dant[i];
        }

3 共识卫星--selsat

基准站和流动站共同观测到的卫星。每个卫星都有编号sat，我们只需找到哪个卫星编号在基准站和流动站中都出现过即可。

iu，ir分别记录共识卫星sat在观测值obs[ ]（流动站和基准站的下标）

static int selsat(const obsd_t *obs, double *azel, int nu, int nr,
                  const prcopt_t *opt, int *sat, int *iu, int *ir)
{
    int i,j,k=0;
    
    trace(3,"selsat  : nu=%d nr=%d\n",nu,nr);
    /*
        obs[i]前面是流动站，后面是基准站。这个函数是要找流动站和基准站共同观测到的卫星
        [i]从流动站变量，[j]从基准站遍历。i--,j--都是在sat不相等时在调整
        当找到共同的卫星sat相等，sat[k]负责记录该共同卫星编号(sat)
        iu[k]记录卫星在流动站的索引(在obs中，obs[i])
        iu[k]记录卫星在基准站的索引(在obs中，obs[j])
    */


    for (i=0,j=nu;i<nu&&j<nu+nr;i++,j++) {
        if      (obs[i].sat<obs[j].sat) j--;
        else if (obs[i].sat>obs[j].sat) i--;
        else if (azel[1+j*2]>=opt->elmin) { /* elevation at base station */
            sat[k]=obs[i].sat; iu[k]=i; ir[k++]=j;
            trace(4,"(%2d) sat=%3d iu=%2d ir=%2d\n",k-1,obs[i].sat,i,j);
        }
    }
    return k;
}