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