【manage_amb_LAMBDA】模糊度固定函数解析
模糊度固定函数_manage_amb_LAMBDA解析
文章目录
- 模糊度固定函数_manage_amb_LAMBDA解析
- 一、函数功能描述
- 二、函数功能分析
-
- 1.变量解析
- 2.逐块分析
- 三、总结
一、函数功能描述
rtkpos.c：函数注释如下,
/* resolve integer ambiguity by LAMBDA using partial fix techniques and multiple attempts -----------------------*/
从上述注释可知，使用部分固定技术和多次尝试两种方法进行LAMBDA算法实现。
那么，本文重点对这两种技术的实现进行讲解。
二、函数功能分析
推荐大家阅读源码的时候，使用Source Insight软件。
1.变量解析
先贴出来该函数的局部变量，
int i,f,lockc[NFREQ],ar=0,excflag=0,arsats[MAXOBS]={0};
int gps1=-1,glo1=-1,sbas1=-1,gps2,glo2,sbas2,nb,rerun,dly;
float ratio1,var=0,posvar=0;
（1）上述变量ar，ar是一个计数器，表示上次进行AR（模糊度解算）可用卫星数量，在以下代码块中使用（出现两次）。
/* if no fix on previous sample and enough sats, exclude next sat in list */
if (rtk->sol.prev_ratio2<rtk->sol.thres&&rtk->nb_ar>=rtk->opt.mindropsats) {
/* find and count sats used last time for AR */
for (f=0;f<nf;f++) for (i=0;i<ns;i++)
if (rtk->ssat[sat[i]-1].vsat[f] && rtk->ssat[sat[i]-1].lock[f]>=0 && rtk->ssat[sat[i]-1].azel[1]>=rtk->opt.elmin) {
arsats[ar++]=i;
}
if (rtk->excsat<ar) {
i=sat[arsats[rtk->excsat]];
for (f=0;f<nf;f++) {
lockc[f]=rtk->ssat[i-1].lock[f]; /* save lock count */
/* remove sat from AR long enough to enable hold if stays fixed */
rtk->ssat[i-1].lock[f]=-rtk->nb_ar;
}
trace(3,"AR: exclude sat %d\n",i);
excflag=1;
} else rtk->excsat=0; /* exclude none and reset to beginning of list */
}
（2）上述变量excflag，从变量定义可知，是排除卫星的标志，也出现两次，除了在（1）中出现，还在以下代码中出现。
/* restore excluded sat if still no fix or significant increase in ar ratio */
if (excflag && (rtk->sol.ratio<rtk->sol.thres) && (rtk->sol.ratio<(1.5*rtk->sol.prev_ratio2))) {
i=sat[arsats[rtk->excsat++]];
for (f=0;f<nf;f++) rtk->ssat[i-1].lock[f]=lockc[f];
trace(3,"AR: restore sat %d\n",i);
}
（3）上述变量arsats，表示前一历元的共视卫星数的索引信息，从索引信息和共视卫星sat可知卫星的svid，变量出现三次，分别在（1）和（2）的代码块中。
（4）上述变量nb，表示当前历元使用的模糊度数量，与之对比的是nb_ar，表示前一历元使用的模糊度数量。
（5）上述变量rerun，是重新运行的标志，总是把它和return看混，其实变量定义成re_run也好点，不易混淆。
（6）上述变量dly，是控制出问题的卫星过几个历元再参与模糊度的一个变量，和stagger是一个意思吧，用于调节卫星的lock，进而影响是否参与模糊度解算。
（7）上述变量posvar，是位置方差，即state状态（xyz）rtk->x[0]，rtk->x[1]，rtk->x[2]对应的rtk->P[0]，rtk->P[1+rtk->nx]，rtk->P[2+2*rtk->nx]的均值。代码如下，
/* calc position variance, will skip AR if too high to avoid false fix */
for (i=0;i<3;i++) posvar+=rtk->P[i+i*rtk->nx];
posvar/=3.0; /* maintain compatibility with previous code */
trace(3,"posvar=%.6f\n",posvar);
2.逐块分析
/* skip AR if don't meet criteria */
if (rtk->opt.mode<=PMODE_DGPS||rtk->opt.modear==ARMODE_OFF||
rtk->opt.thresar[0]<1.0||posvar>rtk->opt.thresar[1]) {
trace(3,"Skip AR\n");
rtk->sol.ratio=0.0;
rtk->sol.prev_ratio1=rtk->sol.prev_ratio2=0.0;
rtk->nb_ar=0;
return 0;
}
（1）上述代码是对进入AR的准则做一些判断，如果模式是DGPS或SINGLE，AR模式选择OFF，配置opt中的ar阈值小于1.0，位置方差大于配置opt中的方差阈值，出现四种其中之一的话，就会跳过AR，我们要关注的是次数被重置的成员是哪些。
/* if no fix on previous sample and enough sats, exclude next sat in list */
if (rtk->sol.prev_ratio2<rtk->sol.thres&&rtk->nb_ar>=rtk->opt.mindropsats) {
/* find and count sats used last time for AR */
for (f=0;f<nf;f++) for (i=0;i<ns;i++)
if (rtk->ssat[sat[i]-1].vsat[f] && rtk->ssat[sat[i]-1].lock[f]>=0 && rtk->ssat[sat[i]-1].azel[1]>=rtk->opt.elmin) {
arsats[ar++]=i;
}
if (rtk->excsat<ar) {
i=sat[arsats[rtk->excsat]];
for (f=0;f<nf;f++) {
lockc[f]=rtk->ssat[i-1].lock[f]; /* save lock count */
/* remove sat from AR long enough to enable hold if stays fixed */
rtk->ssat[i-1].lock[f]=-rtk->nb_ar;
}
trace(3,"AR: exclude sat %d\n",i);
excflag=1;
} else rtk->excsat=0; /* exclude none and reset to beginning of list */
}
（2）上述代码是对前一历元没有fix解，但是参与模糊度的卫星数又足够多，这时候就要进行剔星的操作。具体实现是把卫星的索引信息放在arsats数组中，根据要剔除的索引信息（rtk->excsat）的卫星进行remove操作，移除卫星的操作通过卫星载波锁定数lock来控制，同时对excflag置为1。在rtk结构体的成员excsat中有说明，其实我觉得改成idx_excsat更好理解这个变量。
int excsat; /* index of next satellite to be excluded for partial ambiguity resolution */
/* first attempt to resolve ambiguities */
nb=resamb_LAMBDA(rtk,bias,xa,gps1,glo1,sbas1);
ratio1=rtk->sol.ratio;
（3）进行第一次LAMBDA尝试，将ratio值赋给中间变量ratio1。
/* reject bad satellites if AR filtering enabled */
if (rtk->opt.arfilter) {
rerun=0;
/* if results are much poorer than previous epoch or dropped below ar ratio thresh, remove new sats */
if (nb>=0 && rtk->sol.prev_ratio2>=rtk->sol.thres && ((rtk->sol.ratio<rtk->sol.thres) ||
(rtk->sol.ratio<rtk->opt.thresar[0]*1.1 && rtk->sol.ratio<rtk->sol.prev_ratio1/2.0))) {
trace(3,"low ratio: check for new sat\n");
dly=2;
for (i=0;i<ns;i++) for (f=0;f<nf;f++) {
if (rtk->ssat[sat[i]-1].fix[f]!=2) continue;
/* check for new sats */
if (rtk->ssat[sat[i]-1].lock[f]==0) {
trace(3,"remove sat %d:%d lock=%d\n",sat[i],f,rtk->ssat[sat[i]-1].lock[f]);
rtk->ssat[sat[i]-1].lock[f]=-rtk->opt.minlock-dly; /* delay use of this sat with stagger */
dly+=2; /* stagger next try of new sats */
rerun=1;
}
}
}
/* rerun if filter removed any sats */
if (rerun) {
trace(3,"rerun AR with new sat removed\n");
/* try again with new sats removed */
nb=resamb_LAMBDA(rtk,bias,xa,gps1,glo1,sbas1);
}
}
（4）上述代码表示如果ar滤波打开的话，则进行剔星的操作，注意注释中的reject bad satellites ，表明剔除的卫星可能不止一颗。从ratio的值作为条件进行判断是否进入if语句，如果进入的话，则对共视星进行for循环判断，先对fix进行判断，fix=1表示float，fix=2表示fix，fix=3表示hold，lock=0表示这颗卫星是新上星，这个是根据模糊度解算去判断的，可以看看resamb_LAMBDA中ddidx的实现。对新上星的lock进行赋值，如果新上星不止一颗，那么要通过dly进行错开，间隔为2个历元，后处理时可以通过打印ddres中的打印信息分析，如果该卫星一直存在问题，会发现lock是在-2和-1来回变化。
trace(3,"sat=%3d-%3d %s%d v=%13.3f R=%9.6f %9.6f icb=%9.3f lock=%5d x=%9.3f\n",sat[i],
sat[j],code?"P":"L",frq+1,v[nv],Ri[nv],Rj[nv],icb,
rtk->ssat[sat[j]-1].lock[frq],rtk->x[IB(sat[j],frq,&rtk->opt)]);
如果rerun标志位被置起来。那么就要再次进行LAMBDA尝试（第二次）。
/* if fix-and-hold gloarmode enabled, re-run AR with final gps/glo settings if differ from above */
if ((rtk->opt.navsys&SYS_GLO) && rtk->opt.glomodear==GLO_ARMODE_FIXHOLD && rtk->sol.ratio<rtk->sol.thres) {
glo2=sbas2=0;
/* turn off gpsmode if not enabled and got good fix (used for debug and eval only) */
gps2=rtk->opt.gpsmodear==0&&rtk->sol.ratio>=rtk->sol.thres?0:1;
/* if modes changed since initial AR run or haven't run yet,re-run with new modes */
if (glo1!=glo2||gps1!=gps2)
nb=resamb_LAMBDA(rtk,bias,xa,gps2,glo2,sbas2);
}
（5）上述代码表示如果选择的卫星系统中有glonass并且模糊度解算模式时HLOD，但是ratio值小于设置的阈值，那么再次进行LAMBDA尝试（第三次）。
/* restore excluded sat if still no fix or significant increase in ar ratio */
if (excflag && (rtk->sol.ratio<rtk->sol.thres) && (rtk->sol.ratio<(1.5*rtk->sol.prev_ratio2))) {
i=sat[arsats[rtk->excsat++]];
for (f=0;f<nf;f++) rtk->ssat[i-1].lock[f]=lockc[f];
trace(3,"AR: restore sat %d\n",i);
}
（6）上述代码是针对第一次LAMBDA尝试没有成功提高ratio时，对（2）中的卫星恢复如初，把lockc保存的剔除卫星前的lock计数重新赋值过来，ratio值没有提升，说明剔除的卫星没有啥问题，有问题的卫星还没有找到，没找到咋办呢，继续下一个索引的卫星呗，即rtk->excsat++，真的建议把这个成员excsat改成idx_excsat。
三、总结
从上述分析可以，该函数从两方面来入手提高模糊度的固定率，最精髓的是这处代码。
/* reject bad satellites if AR filtering enabled */
if (rtk->opt.arfilter) {
rerun=0;
/* if results are much poorer than previous epoch or dropped below ar ratio thresh, remove new sats */
if (nb>=0 && rtk->sol.prev_ratio2>=rtk->sol.thres && ((rtk->sol.ratio<rtk->sol.thres) ||
(rtk->sol.ratio<rtk->opt.thresar[0]*1.1 && rtk->sol.ratio<rtk->sol.prev_ratio1/2.0))) {
trace(3,"low ratio: check for new sat\n");
dly=2;
for (i=0;i<ns;i++) for (f=0;f<nf;f++) {
if (rtk->ssat[sat[i]-1].fix[f]!=2) continue;
/* check for new sats */
if (rtk->ssat[sat[i]-1].lock[f]==0) {
trace(3,"remove sat %d:%d lock=%d\n",sat[i],f,rtk->ssat[sat[i]-1].lock[f]);
rtk->ssat[sat[i]-1].lock[f]=-rtk->opt.minlock-dly; /* delay use of this sat with stagger */
dly+=2; /* stagger next try of new sats */
rerun=1;
}
}
}
/* rerun if filter removed any sats */
if (rerun) {
trace(3,"rerun AR with new sat removed\n");
/* try again with new sats removed */
nb=resamb_LAMBDA(rtk,bias,xa,gps1,glo1,sbas1);
}
}
调试的时候，多关注下lock成员变量。