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