小蓝xlanll

这个屌丝很懒，什么也没留下！

热门标签

ddres( ) 组站星双差方程和设计矩阵_rtklib ddres

作者：小蓝xlanll | 2024-04-21 18:37:54

踩

rtklib ddres

1 ddres( )参数介绍

rtklib中进行的单频解算

双差观测值，单差的模糊度单频点双差
DD (double-differenced) phase/code residuals ------------------------------
x 模糊度
P 方差-协方差阵
sat 共识卫星列表
ns 共识卫星数量
y 残差阵,数量：ns*nf*2(每颗卫星的每个频率，×2是伪距和载波)
e dx dy dz设计矩阵,数量：ns*3
freq 频率,数量：ns*nf 每个频点的伪距和载波频率一样，而且伪距也用不到频率，是载波求波长时用的
v 完整残差阵(包括了模糊度)
H dx dy dz N1 N2 N2等模糊度设计矩阵
R 方差-协方差阵


static int ddres(rtk_t *rtk, const nav_t *nav, double dt, const double *x,
                 const double *P, const int *sat, double *y, double *e,
                 double *azel, double *freq, const int *iu, const int *ir,
                 int ns, double *v, double *H, double *R, int *vflg)

2 代码解读

计算基线的长度，计算基准站和流动站方差时会用到


    bl=baseline(x,rtk->rb,dr);
    ecef2pos(x,posu); ecef2pos(rtk->rb,posr);

Ri是参考卫星方差，RJ是非参考卫星方差。用于后面计算双差后的观测值方差--协方差阵

tropu、tropr等是对流层、电离层参数，在此我们不作讨论


    Ri=mat(ns*nf*2+2,1); Rj=mat(ns*nf*2+2,1); im=mat(ns,1);
    tropu=mat(ns,1); tropr=mat(ns,1); dtdxu=mat(ns,3); dtdxr=mat(ns,3);

初始化伪距和载波相位残差为0，每个卫星每个频点的，用于以后记录双差残差

rtk->ssat[i]；访问到了每颗卫星

double resp[NFREQ]; residuals of pseudorange (m)
double resc[NFREQ]; residuals of carrier-phase (m)

MAXSAT；最大卫星数，不管观没观测到，都初始化。这也造成了空间上的浪费

NFREQ；最大频率


    for (i=0;i<MAXSAT;i++) for (j=0;j<NFREQ;j++) {
        rtk->ssat[i].resp[j]=rtk->ssat[i].resc[j]=0.0;
    }

对流层和电离层处理，暂不考虑


    for (i=0;i<ns;i++) {
        if (opt->ionoopt>=IONOOPT_EST) {
            im[i]=(ionmapf(posu,azel+iu[i]*2)+ionmapf(posr,azel+ir[i]*2))/2.0;
        }
        if (opt->tropopt>=TROPOPT_EST) {
            tropu[i]=prectrop(rtk->sol.time,posu,0,azel+iu[i]*2,opt,x,dtdxu+i*3);
            tropr[i]=prectrop(rtk->sol.time,posr,1,azel+ir[i]*2,opt,x,dtdxr+i*3);
        }
    }

之后就进入一个大循环，最外层是先遍历每个卫星系统

for (m=0;m<6;m++)    /* m=0:GPS/SBS,1:GLO,2:GAL,3:BDS,4:QZS,5:IRN */

二重循环，根据所选择的定位模式，进行相应的频点遍历。nf*2是因为每个系统的每个频点(*2 包括伪距和载波)

只利用伪距观测值，nf---2*nf，因为伪距残差是在y[]后半

是载波(动态定位模式及以后)，那我们从0--2*nf，因为同时使用伪距和载波残差

for (f=opt->mode>PMODE_DGPS?0:nf;f<nf*2;f++)

ns---共识卫星数量

这个就是找参考卫星的，在这里是把高度角最高的那个作为参考卫星。我们也可以改写代码，设置其他的规则。参考卫星下标 i


        for (i=-1,j=0;j<ns;j++) {
            sysi=rtk->ssat[sat[j]-1].sys;
            if (!test_sys(sysi,m)) continue;//当前共识卫星的系统和当前遍历的系统是否一样
            if (!validobs(iu[j],ir[j],f,nf,y)) continue; //评价是否可以
            if (i<0||azel[1+iu[j]*2]>=azel[1+iu[i]*2]) i=j;//选择排序的那种,i最终是参考卫星的index
        }

又是一个大循环，遍历所有的共识卫星，这次是要计算双差残差和设计矩阵。组双差的话，两台接收机是要都观测到相同的卫星的，所以需要共识卫星处理。

以下代码均是在此循环中执行


for (j=0;j<ns;j++) {
 
}

两个不同的卫星，所以i≠j

freq[f%nf+iu[i]*nf]；取频率

f区间为[0,2*nf]，所以取余%

freqi，freqj会在电离层、对流层用到


            if (i==j) continue;
            sysi=rtk->ssat[sat[i]-1].sys;//参考卫星系统
            sysj=rtk->ssat[sat[j]-1].sys;//非参考卫星系统
            freqi=freq[f%nf+iu[i]*nf];
            freqj=freq[f%nf+iu[j]*nf];
            if (!test_sys(sysj,m)) continue;//判断和当前遍历系统一样否
            if (!validobs(iu[j],ir[j],f,nf,y)) continue;//检验数据有效yy[]

设计矩阵初始化，nx为待估量的个数


            if (H) {
                Hi=H+nv*rtk->nx;
                for (k=0;k<rtk->nx;k++) Hi[k]=0.0;
            }

求双差残差，

(y[f+iu[i]*nf*2]-y[f+ir[i]*nf*2]) 共识卫星站间单差，是参考卫星i

y[f+iu[j]*nf*2]-y[f+ir[j]*nf*2] 共识卫星站间单差，是参考卫星j

两个一减，就是双差残差了

 v[nv]=(y[f+iu[i]*nf*2]-y[f+ir[i]*nf*2])-(y[f+iu[j]*nf*2]-y[f+ir[j]*nf*2]);

设计矩阵，e[ ]是dx dy dz的系数，已在站间单差函数中求得，我们只考虑单频，e[ ]中只有单频的信息

e[]是dx dy dz设计矩阵，大小：流动站观测到的卫星+基准站观测到的卫星。
因为在relpos()函数中，站间单差[先是算基准站，后是流动站]，设计矩阵都填到e里面了。所以大小如此

设计矩阵=非参考卫星-参考卫星

每次循环都会产生一组双差dx dy dz的系数。但最最外面还有一个频率循环，那么不同频的dx dy dz的系数是一样的吗？这我还不知道

这两个会是一样吗？留到以后回答，或者有谁知道

我的猜想是可能一样，因为由计算系数的公式可知，这个与卫星位置和接收机概略位置有关


            if (H) {
                for (k=0;k<3;k++) {
                    Hi[k]=-e[k+iu[i]*3]+e[k+iu[j]*3];
                }
            }

IB()不懂是什么。哪个卫星哪个频点的模糊度位置索引？？

看上去像是索引，但为什么x[ ] Hi[ ]都可以用。一个是设计矩阵位置，一个是对应模糊度位置啥的


                if (opt->ionoopt!=IONOOPT_IFLC) {
                    v[nv]-=CLIGHT/freqi*x[IB(sat[i],f,opt)]-
                           CLIGHT/freqj*x[IB(sat[j],f,opt)];
                    if (H) {
                        Hi[IB(sat[i],f,opt)]= CLIGHT/freqi;
                        Hi[IB(sat[j],f,opt)]=-CLIGHT/freqj;
                    }
                }
                else {
                    //根据站星双差方差，还要减去(模糊度的derT)，见理论。而在y[]的计算中，还没有考虑模糊度，在这里给补上
                    //x[] 模糊度
                    v[nv]-=x[IB(sat[i],f,opt)]-x[IB(sat[j],f,opt)];
                    if (H) {
                        Hi[IB(sat[i],f,opt)]= 1.0; //IB() 哪个卫星哪个频点的模糊度位置索引？？？不懂
                        Hi[IB(sat[j],f,opt)]=-1.0;
                    }
                }
            }

Hi[IB(sat[i],f,opt)]= 1.0 这里是在补设计矩阵，参考卫星系数为-1。而这里为1，后续应该在某个计算中会改掉

v[ ]是计算好的双差残差，将其存到ssat结构体中

之后检验残差是否在容许范围内 maxinno是设置的标准


            if (f<nf) rtk->ssat[sat[j]-1].resc[f   ]=v[nv];
            else      rtk->ssat[sat[j]-1].resp[f-nf]=v[nv];
            
            /* test innovation 检查残差是否符合标准 maxinno */
            if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno) {
                if (f<nf) {
                    rtk->ssat[sat[i]-1].rejc[f]++;
                    rtk->ssat[sat[j]-1].rejc[f]++;
                }
                errmsg(rtk,"outlier rejected (sat=%3d-%3d %s%d v=%.3f)\n",
                       sat[i],sat[j],f<nf?"L":"P",f%nf+1,v[nv]);
                continue;
            }

每颗卫星每个频率的观测值噪声(方差)


            Ri[nv]=varerr(sat[i],sysi,azel[1+iu[i]*2],bl,dt,f,opt);
            Rj[nv]=varerr(sat[j],sysj,azel[1+iu[j]*2],bl,dt,f,opt);

设置标志，这颗卫星这个频点的伪距/载波观测值有效。标记一下


            if (opt->mode>PMODE_DGPS) {
                if (f<nf) rtk->ssat[sat[i]-1].vsat[f]=rtk->ssat[sat[j]-1].vsat[f]=1;
            }
            else {
                rtk->ssat[sat[i]-1].vsat[f-nf]=rtk->ssat[sat[j]-1].vsat[f-nf]=1;
            }

独特的编码，由此码可知，双差的具体信息。哪两个卫星作差，载波还是伪距，第几个频率

vflg[nv++]=(sat[i]<<16)|(sat[j]<<8)|((f<nf?0:1)<<4)|(f%nf);

至此，共识卫星遍历结束

每个小频率有几颗卫星(残差有多少个)

nb[b]++;

至此，频点遍历结束

关于基线约束的


    if (opt->mode==PMODE_MOVEB&&constbl(rtk,x,P,v,H,Ri,Rj,nv)) {
        vflg[nv++]=3<<4;
        nb[b++]++;
    }

求双差后的协方差矩阵

ddcov(nb,b,Ri,Rj,nv,R);

3 ddcov( )

非对角线是Ri，对角线是Ri+Rj


static void ddcov(const int *nb, int n, const double *Ri, const double *Rj,
                  int nv, double *R)
{
    int i,j,k=0,b;
    
    trace(3,"ddcov   : n=%d\n",n);
    //nv*nv 矩阵嘛
    for (i=0;i<nv*nv;i++) R[i]=0.0;//初始化
    for (b=0;b<n;k+=nb[b++]) {
        
        for (i=0;i<nb[b];i++) for (j=0;j<nb[b];j++) {
            R[k+i+(k+j)*nv]=Ri[k+i]+(i==j?Rj[k+i]:0.0); //参考GNSS理论16.2讲。非对角线，参考卫星方差
        }
    }
    trace(5,"R=\n"); tracemat(5,R,nv,nv,8,6);
}

4 validobs()

评价也不太懂，为什么可以不用管伪距


    i--基准站共识卫星索引下标
    f--频点 0--nf 是载波 nf--2*nf 是伪距 
 
    当f<nf，载波频率残差  有载波，就不管伪距了？？？解：这是f(0--2*nf)的情况，随着循环进行伪距最终会遍历到(nf--2*nf)检查
        y[f+i*nf*2]!=0  检查基准站共识卫星的载波残差
        f<nf||(y[f-nf+i*nf*2]!=0.0&&y[f-nf+j*nf*2]!=0.0)  f-nf为真，短路原则
     
    当f>=nf,伪距残差,载波也要有，不然伪距不可用(注释)
        y[f+i*nf*2] 每颗共识卫星伪距观测
        y[f-nf+i*nf*2] 载波观测
*/
static int validobs(int i, int j, int f, int nf, double *y)
{
    /* if no phase observable, psudorange is also unusable */
    return y[f+i*nf*2]!=0.0&&y[f+j*nf*2]!=0.0&&
           (f<nf||(y[f-nf+i*nf*2]!=0.0&&y[f-nf+j*nf*2]!=0.0));
}

5 全部代码


static int ddres(rtk_t *rtk, const nav_t *nav, double dt, const double *x,
                 const double *P, const int *sat, double *y, double *e,
                 double *azel, double *freq, const int *iu, const int *ir,
                 int ns, double *v, double *H, double *R, int *vflg)
{
    prcopt_t *opt=&rtk->opt;
    double bl,dr[3],posu[3],posr[3],didxi=0.0,didxj=0.0,*im;
    double *tropr,*tropu,*dtdxr,*dtdxu,*Ri,*Rj,freqi,freqj,*Hi=NULL;
    int i,j,k,m,f,nv=0,nb[NFREQ*4*2+2]={0},b=0,sysi,sysj,nf=NF(opt);
    
    trace(3,"ddres   : dt=%.1f nx=%d ns=%d\n",dt,rtk->nx,ns);
    //计算基线长度
    bl=baseline(x,rtk->rb,dr);
    ecef2pos(x,posu); ecef2pos(rtk->rb,posr);
    /*
        Ri,Rj 基准站和流动站方差，计算以后协方差阵
        对一些中间变量进行初始化，将双差伪距残差和双差载波相位残差初始化为0
    */
    Ri=mat(ns*nf*2+2,1); Rj=mat(ns*nf*2+2,1); im=mat(ns,1);
    tropu=mat(ns,1); tropr=mat(ns,1); dtdxu=mat(ns,3); dtdxr=mat(ns,3);
    
    /*
        double resp[NFREQ]; residuals of pseudorange (m) 
        double resc[NFREQ]; residuals of carrier-phase (m) 
        将双差伪距残差和双差载波相位残差初始化为0
        每个卫星每个频点的
        为什么没有j<NFREQ*2,因为伪距和载波分为resp和resc了
    */
 
    for (i=0;i<MAXSAT;i++) for (j=0;j<NFREQ;j++) {
        rtk->ssat[i].resp[j]=rtk->ssat[i].resc[j]=0.0;
    }
    /* 
        compute factors of ionospheric and tropospheric delay 
        如果卡尔曼滤波中包含对流层状态量，调用prectrop函数计算基站和移动站的对流层延迟湿分量，存在tropu[i]和tropur[i]中
        如果卡尔曼滤波器包含电离层状态量，调用ionmapf函数分别计算基站和移动站处的投影函数
    */
    for (i=0;i<ns;i++) {
        if (opt->ionoopt>=IONOOPT_EST) {
            im[i]=(ionmapf(posu,azel+iu[i]*2)+ionmapf(posr,azel+ir[i]*2))/2.0;
        }
        if (opt->tropopt>=TROPOPT_EST) {
            tropu[i]=prectrop(rtk->sol.time,posu,0,azel+iu[i]*2,opt,x,dtdxu+i*3);
            tropr[i]=prectrop(rtk->sol.time,posr,1,azel+ir[i]*2,opt,x,dtdxr+i*3);
        }
    }
    /*  对各个系统以及载波相位、伪距分别进行循环处理
        双重循环，遍历每个系统的每个频点(*2 包括伪距和载波)
        哪种定位模式，是伪距，那我们从nf---2*nf，因为伪距残差是在y[]后半
        是载波(动态定位模式及以后)，那我们从0--2*nf，因为同时使用伪距和载波
    */
    for (m=0;m<6;m++) /* m=0:GPS/SBS,1:GLO,2:GAL,3:BDS,4:QZS,5:IRN */
    
    for (f=opt->mode>PMODE_DGPS?0:nf;f<nf*2;f++) {
        
        /* 
            search reference satellite with highest elevation
            选择最大的高度角卫星作为参考卫星
            当然，我们也可以自己设置规则
            ssat_t ssat[MAXSAT]; satellite status 
            所有的共识卫星(全系统)每次都会被遍历一遍，我们选出所需的系统，然后再
        */
        for (i=-1,j=0;j<ns;j++) {
            sysi=rtk->ssat[sat[j]-1].sys;
            if (!test_sys(sysi,m)) continue;//当前共识卫星的系统和当前遍历的系统是否一样
            if (!validobs(iu[j],ir[j],f,nf,y)) continue; //评价不懂
            if (i<0||azel[1+iu[j]*2]>=azel[1+iu[i]*2]) i=j;//选择排序的那种,i最终是参考卫星的index
        }
        if (i<0) continue;
        
        /* 
            make DD (double difference) 做双差，遍历共识卫星
            一个历元一个历元处理，输入流动站和基准站的数据
               流动站卫星残差    基准站卫星残差       每颗卫星每个频点的载波和伪距
            y[............... ................]
            站间单差，两个接收机(a,b)观测同一颗卫星(p)，作差(M1)。卫星p肯定是它们的共识卫星
            星间双差，参考卫星站间单差(M2),M1-M2
        */
        for (j=0;j<ns;j++) {
            if (i==j) continue;
            sysi=rtk->ssat[sat[i]-1].sys;//参考卫星系统
            sysj=rtk->ssat[sat[j]-1].sys;//流动
            freqi=freq[f%nf+iu[i]*nf];
            freqj=freq[f%nf+iu[j]*nf];
            if (!test_sys(sysj,m)) continue;//判断和当前遍历系统一样否
            if (!validobs(iu[j],ir[j],f,nf,y)) continue;//检验数据有效yy[]
            //设计矩阵初始化，nx---待解量的个数
            if (H) {
                Hi=H+nv*rtk->nx;
                for (k=0;k<rtk->nx;k++) Hi[k]=0.0;
            }
            /* 
                DD residual 作双差残差
                iu[i] 共识--参考卫星在基准站残差中的索引
                (y[f+iu[i]*nf*2]-y[f+ir[i]*nf*2]) 共识参考卫星站间单差
            */
            v[nv]=(y[f+iu[i]*nf*2]-y[f+ir[i]*nf*2])-
                  (y[f+iu[j]*nf*2]-y[f+ir[j]*nf*2]);
            
            /* partial derivatives by rover position 
                e[]是dx dy dz设计矩阵，大小：流动站观测到的卫星+基准站观测到的卫星。 
                因为在relpos()函数中，站间单差[先是算基准站，后是流动站]，设计矩阵都填到e里面了。所以大小如此
                设计矩阵=非参考卫星-参考卫星
                因为我们求得是流动站位置，所以只用iu[]即可
                至于不同频率,这里只有单频
            */
            if (H) {
                for (k=0;k<3;k++) {
                    Hi[k]=-e[k+iu[i]*3]+e[k+iu[j]*3];
                }
            }
            /* DD ionospheric delay term */
            if (opt->ionoopt==IONOOPT_EST) {
                didxi=(f<nf?-1.0:1.0)*im[i]*SQR(FREQ1/freqi);
                didxj=(f<nf?-1.0:1.0)*im[j]*SQR(FREQ1/freqj);
                v[nv]-=didxi*x[II(sat[i],opt)]-didxj*x[II(sat[j],opt)];
                if (H) {
                    Hi[II(sat[i],opt)]= didxi;
                    Hi[II(sat[j],opt)]=-didxj;
                }
            }
            /* DD tropospheric delay term */
            if (opt->tropopt==TROPOPT_EST||opt->tropopt==TROPOPT_ESTG) {
                v[nv]-=(tropu[i]-tropu[j])-(tropr[i]-tropr[j]);
                for (k=0;k<(opt->tropopt<TROPOPT_ESTG?1:3);k++) {
                    if (!H) continue;
                    Hi[IT(0,opt)+k]= (dtdxu[k+i*3]-dtdxu[k+j*3]);
                    Hi[IT(1,opt)+k]=-(dtdxr[k+i*3]-dtdxr[k+j*3]);
                }
            }
            /* DD phase-bias term 双差模糊度 */
            if (f<nf) {
                if (opt->ionoopt!=IONOOPT_IFLC) {
                    v[nv]-=CLIGHT/freqi*x[IB(sat[i],f,opt)]-
                           CLIGHT/freqj*x[IB(sat[j],f,opt)];
                    if (H) {
                        Hi[IB(sat[i],f,opt)]= CLIGHT/freqi;
                        Hi[IB(sat[j],f,opt)]=-CLIGHT/freqj;
                    }
                }
                else {
                    //根据站星双差方差，还要减去(模糊度的derT)，见理论。而在y[]的计算中，还没有考虑模糊度，在这里给补上
                    //x[] 模糊度
                    v[nv]-=x[IB(sat[i],f,opt)]-x[IB(sat[j],f,opt)];
                    if (H) {
                        Hi[IB(sat[i],f,opt)]= 1.0; //IB() 哪个卫星哪个频点的模糊度位置索引？？？不懂
                        Hi[IB(sat[j],f,opt)]=-1.0;
                    }
                }
            }
            //将计算好的残差存到ssat结构体
 
            if (f<nf) rtk->ssat[sat[j]-1].resc[f   ]=v[nv];
            else      rtk->ssat[sat[j]-1].resp[f-nf]=v[nv];
            
            /* test innovation 检查残差是否符合标准 maxinno */
            if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno) {
                if (f<nf) {
                    rtk->ssat[sat[i]-1].rejc[f]++;
                    rtk->ssat[sat[j]-1].rejc[f]++;
                }
                errmsg(rtk,"outlier rejected (sat=%3d-%3d %s%d v=%.3f)\n",
                       sat[i],sat[j],f<nf?"L":"P",f%nf+1,v[nv]);
                continue;
            }
            /* SD (single-differenced) measurement error variances 每颗卫星每个频率的观测值噪声(方差) */
            Ri[nv]=varerr(sat[i],sysi,azel[1+iu[i]*2],bl,dt,f,opt);
            Rj[nv]=varerr(sat[j],sysj,azel[1+iu[j]*2],bl,dt,f,opt);
            
            /* set valid data flags 设置标志，这个卫星这个频率有效 */
            if (opt->mode>PMODE_DGPS) {
                if (f<nf) rtk->ssat[sat[i]-1].vsat[f]=rtk->ssat[sat[j]-1].vsat[f]=1;
            }
            else {
                rtk->ssat[sat[i]-1].vsat[f-nf]=rtk->ssat[sat[j]-1].vsat[f-nf]=1;
            }
            trace(4,"sat=%3d-%3d %s%d v=%13.3f R=%8.6f %8.6f\n",sat[i],
                  sat[j],f<nf?"L":"P",f%nf+1,v[nv],Ri[nv],Rj[nv]);
            
            //标记一下，双差的具体信息。哪两个卫星作差，载波还是伪距，第几个频率
            vflg[nv++]=(sat[i]<<16)|(sat[j]<<8)|((f<nf?0:1)<<4)|(f%nf);
            nb[b]++;//每个小频率有几颗卫星(残差有多少个)
        }
        b++;//每个小频率
    }
    /* end of system loop */
    
    /* baseline length constraint for moving baseline 基线约束，以后看 */
    if (opt->mode==PMODE_MOVEB&&constbl(rtk,x,P,v,H,Ri,Rj,nv)) {
        vflg[nv++]=3<<4;
        nb[b++]++;
    }
    if (H) {trace(5,"H=\n"); tracemat(5,H,rtk->nx,nv,7,4);}
    
    /* DD measurement error covariance 协方差矩阵 */
    ddcov(nb,b,Ri,Rj,nv,R);
    
    free(Ri); free(Rj); free(im);
    free(tropu); free(tropr); free(dtdxu); free(dtdxr);
    
    return nv;
}

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/小蓝xlanll/article/detail/464546