From 5d7ed1ad5423be03ef0ee53805021725c0b38127 Mon Sep 17 00:00:00 2001 From: rtklibexplorer Date: Mon, 1 Aug 2022 12:04:44 -0600 Subject: [PATCH] Modifications for Google Decimeter Challenge 2022 --- app/winapp/rtkpost/postmain.cpp | 14 +++++++------- app/winapp/rtkpost/postmain.h | 2 +- app/winapp/rtkpost/postopt.cpp | 20 +++++++++++--------- app/winapp/rtkpost/postopt.dfm | 8 ++++---- app/winapp/rtkpost/postopt.h | 4 ++-- src/options.c | 6 +++--- src/pntpos.c | 3 ++- src/postpos.c | 21 +++++++++++++++++---- src/ppp.c | 2 +- src/rtkcmn.c | 3 ++- src/rtklib.h | 7 +++---- src/rtkpos.c | 25 ++++++++++++++----------- 12 files changed, 67 insertions(+), 48 deletions(-) diff --git a/app/winapp/rtkpost/postmain.cpp b/app/winapp/rtkpost/postmain.cpp index 967aa8584..5f7bca195 100644 --- a/app/winapp/rtkpost/postmain.cpp +++ b/app/winapp/rtkpost/postmain.cpp @@ -118,7 +118,7 @@ __fastcall TMainForm::TMainForm(TComponent* Owner) DynamicModel=IonoOpt=TropOpt=RovAntPcv=RefAntPcv=AmbRes=0; RovPosType=RefPosType=0; OutCntResetAmb=5; LockCntFixAmb=5; FixCntHoldAmb=10; - MaxAgeDiff=30.0; RejectThres=30.0; RejectGdop=30.0; + MaxAgeDiff=30.0; RejectPhase=30.0; RejectCode=30.0; MeasErrR1=MeasErrR2=MeasErrR5=100.0; MeasErr2=0.004; MeasErr3=0.003; MeasErr4=1.0; SatClkStab=1E-11; ValidThresAR=3.0; ValidThresARMin=3.0; ValidThresARMax=3.0; RovAntE=RovAntN=RovAntU=RefAntE=RefAntN=RefAntU=0.0; @@ -905,8 +905,8 @@ int __fastcall TMainForm::GetOption(prcopt_t &prcopt, solopt_t &solopt, prcopt.thresslip=SlipThres; prcopt.thresdop=DopThres; prcopt.maxtdiff =MaxAgeDiff; - prcopt.maxgdop =RejectGdop; - prcopt.maxinno =RejectThres; + prcopt.maxinno[1]=RejectCode; + prcopt.maxinno[0]=RejectPhase; prcopt.varholdamb=VarHoldAmb; prcopt.gainholdamb=GainHoldAmb; prcopt.outsingle=OutputSingle; @@ -1260,10 +1260,10 @@ void __fastcall TMainForm::LoadOpt(void) SlipThres =ini->ReadFloat ("opt","slipthres", 0.05); DopThres =ini->ReadFloat ("opt","dopthres", 0.0); MaxAgeDiff =ini->ReadFloat ("opt","maxagediff", 30.0); - RejectThres =ini->ReadFloat ("opt","rejectthres", 30.0); + RejectPhase =ini->ReadFloat ("opt","rejectphase", 5.0); VarHoldAmb =ini->ReadFloat ("opt","varholdamb", 0.1); GainHoldAmb =ini->ReadFloat ("opt","gainholdamb", 0.01); - RejectGdop =ini->ReadFloat ("opt","rejectgdop", 30.0); + RejectCode =ini->ReadFloat ("opt","rejectcode", 30.0); ARIter =ini->ReadInteger("opt","ariter", 1); NumIter =ini->ReadInteger("opt","numiter", 1); MinFixSats =ini->ReadInteger("opt","minfixsats", 4); @@ -1484,8 +1484,8 @@ void __fastcall TMainForm::SaveOpt(void) ini->WriteFloat ("opt","slipthres", SlipThres ); ini->WriteFloat ("opt","dopthres", DopThres ); ini->WriteFloat ("opt","maxagediff", MaxAgeDiff ); - ini->WriteFloat ("opt","rejectgdop", RejectGdop ); - ini->WriteFloat ("opt","rejectthres", RejectThres ); + ini->WriteFloat ("opt","rejectcode", RejectCode ); + ini->WriteFloat ("opt","rejectphase", RejectPhase ); ini->WriteFloat ("opt","varholdamb", VarHoldAmb ); ini->WriteFloat ("opt","gainholdamb", GainHoldAmb ); ini->WriteInteger("opt","ariter", ARIter ); diff --git a/app/winapp/rtkpost/postmain.h b/app/winapp/rtkpost/postmain.h index 8f79c7739..78981d4fb 100644 --- a/app/winapp/rtkpost/postmain.h +++ b/app/winapp/rtkpost/postmain.h @@ -202,7 +202,7 @@ class TMainForm : public TForm int SbasCorr,SbasCorr1,SbasCorr2,SbasCorr3,SbasCorr4,TimeDecimal; int SolStatic,SbasSat,MapFunc; int PosOpt[6]; - double ElMask,MaxAgeDiff,RejectThres,VarHoldAmb,GainHoldAmb,RejectGdop; + double ElMask,MaxAgeDiff,VarHoldAmb,GainHoldAmb,RejectCode,RejectPhase; double MeasErrR1,MeasErrR2,MeasErrR5,MeasErr2,MeasErr3,MeasErr4,MeasErr5; double MeasErr6,MeasErr7,MeasErr8; double SatClkStab,RovAntE,RovAntN,RovAntU,RefAntE,RefAntN,RefAntU; diff --git a/app/winapp/rtkpost/postopt.cpp b/app/winapp/rtkpost/postopt.cpp index 0a59ec6d1..991bf0caa 100644 --- a/app/winapp/rtkpost/postopt.cpp +++ b/app/winapp/rtkpost/postopt.cpp @@ -391,8 +391,8 @@ void __fastcall TOptDialog::GetOpt(void) ElMaskAR ->Text =s.sprintf("%.0f",MainForm->ElMaskAR); ElMaskHold ->Text =s.sprintf("%.0f",MainForm->ElMaskHold); MaxAgeDiff ->Text =s.sprintf("%.1f",MainForm->MaxAgeDiff); - RejectGdop ->Text =s.sprintf("%.1f",MainForm->RejectGdop); - RejectThres ->Text =s.sprintf("%.1f",MainForm->RejectThres); + RejectCode ->Text =s.sprintf("%.1f",MainForm->RejectCode); + RejectPhase ->Text =s.sprintf("%.1f",MainForm->RejectPhase); VarHoldAmb ->Text =s.sprintf("%.4f",MainForm->VarHoldAmb); GainHoldAmb ->Text =s.sprintf("%.4f",MainForm->GainHoldAmb); SlipThres ->Text =s.sprintf("%.3f",MainForm->SlipThres); @@ -526,8 +526,8 @@ void __fastcall TOptDialog::SetOpt(void) MainForm->ElMaskAR =ElMaskAR ->Text.ToInt(); MainForm->ElMaskHold =ElMaskHold ->Text.ToInt(); MainForm->MaxAgeDiff =str2dbl(MaxAgeDiff ->Text); - MainForm->RejectGdop =str2dbl(RejectGdop ->Text); - MainForm->RejectThres =str2dbl(RejectThres->Text); + MainForm->RejectCode =str2dbl(RejectCode ->Text); + MainForm->RejectPhase =str2dbl(RejectPhase->Text); MainForm->VarHoldAmb =str2dbl(VarHoldAmb->Text); MainForm->GainHoldAmb =str2dbl(GainHoldAmb->Text); MainForm->SlipThres =str2dbl(SlipThres ->Text); @@ -676,8 +676,8 @@ int ppp=PosMode->ItemIndex>=PMODE_PPP_KINEMA; ElMaskAR ->Text =s.sprintf("%.0f",prcopt.elmaskar*R2D); ElMaskHold ->Text =s.sprintf("%.0f",prcopt.elmaskhold*R2D); MaxAgeDiff ->Text =s.sprintf("%.1f",prcopt.maxtdiff ); - RejectGdop ->Text =s.sprintf("%.1f",prcopt.maxgdop ); - RejectThres ->Text =s.sprintf("%.1f",prcopt.maxinno ); + RejectCode ->Text =s.sprintf("%.1f",prcopt.maxinno[1] ); + RejectPhase ->Text =s.sprintf("%.1f",prcopt.maxinno[0] ); VarHoldAmb ->Text =s.sprintf("%.5f",prcopt.varholdamb); GainHoldAmb ->Text =s.sprintf("%.5f",prcopt.gainholdamb); SlipThres ->Text =s.sprintf("%.3f",prcopt.thresslip); @@ -837,8 +837,8 @@ int ppp=PosMode->ItemIndex>=PMODE_PPP_KINEMA; prcopt.elmaskar =str2dbl(ElMaskAR ->Text)*D2R; prcopt.elmaskhold=str2dbl(ElMaskHold->Text)*D2R; prcopt.maxtdiff =str2dbl(MaxAgeDiff ->Text); - prcopt.maxgdop =str2dbl(RejectGdop ->Text); - prcopt.maxinno =str2dbl(RejectThres->Text); + prcopt.maxinno[1]=str2dbl(RejectCode ->Text); + prcopt.maxinno[0]=str2dbl(RejectPhase->Text); prcopt.varholdamb=str2dbl(VarHoldAmb->Text); prcopt.gainholdamb=str2dbl(GainHoldAmb->Text); prcopt.thresslip=str2dbl(SlipThres ->Text); @@ -958,7 +958,8 @@ void __fastcall TOptDialog::UpdateEnable(void) SlipThres ->Enabled=rtk||ppp; DopThres ->Enabled=rtk||ppp; MaxAgeDiff ->Enabled=rel; - RejectThres ->Enabled=rel||ppp; + RejectPhase ->Enabled=rel||ppp; + RejectCode ->Enabled=rel||ppp; VarHoldAmb ->Enabled=ar; GainHoldAmb ->Enabled=ar&&AmbRes->ItemIndex==3; //ARIter ->Enabled=ppp; @@ -1134,3 +1135,4 @@ void __fastcall TOptDialog::BtnFreqClick(TObject *Sender) + diff --git a/app/winapp/rtkpost/postopt.dfm b/app/winapp/rtkpost/postopt.dfm index ee09005a2..51ef310df 100644 --- a/app/winapp/rtkpost/postopt.dfm +++ b/app/winapp/rtkpost/postopt.dfm @@ -498,9 +498,9 @@ object OptDialog: TOptDialog object Label11: TLabel Left = 24 Top = 170 - Width = 176 + Width = 179 Height = 13 - Caption = 'Reject Threshold of GDOP/Innov (m)' + Caption = 'Outlier Threshold for Code/Phase (m)' end object Label37: TLabel Left = 25 @@ -609,7 +609,7 @@ object OptDialog: TOptDialog TabOrder = 9 Text = '30' end - object RejectThres: TEdit + object RejectPhase: TEdit Left = 325 Top = 167 Width = 75 @@ -682,7 +682,7 @@ object OptDialog: TOptDialog TabOrder = 6 Text = '0' end - object RejectGdop: TEdit + object RejectCode: TEdit Left = 248 Top = 167 Width = 75 diff --git a/app/winapp/rtkpost/postopt.h b/app/winapp/rtkpost/postopt.h index df880a39a..1920c6730 100644 --- a/app/winapp/rtkpost/postopt.h +++ b/app/winapp/rtkpost/postopt.h @@ -59,7 +59,7 @@ class TOptDialog : public TForm TEdit *ElMaskAR; TEdit *SlipThres; TEdit *MaxAgeDiff; - TEdit *RejectThres; + TEdit *RejectPhase; TEdit *NumIter; TTabSheet *TabSheet3; TLabel *LabelSolFormat; @@ -175,7 +175,7 @@ class TOptDialog : public TForm TButton *BtnBLQFile; TEdit *MeasErrR2; TEdit *ElMaskHold; - TEdit *RejectGdop; + TEdit *RejectCode; TLabel *Label39; TLabel *Label40; TComboBox *IntpRefObs; diff --git a/src/options.c b/src/options.c index 61f65b4f8..a31b2e390 100644 --- a/src/options.c +++ b/src/options.c @@ -113,9 +113,9 @@ EXPORT opt_t sysopts[]={ {"pos2-maxage", 1, (void *)&prcopt_.maxtdiff, "s" }, {"pos2-syncsol", 3, (void *)&prcopt_.syncsol, SWTOPT }, {"pos2-slipthres", 1, (void *)&prcopt_.thresslip, "m" }, - {"pos2-dopthres", 1, (void *)&prcopt_.thresdop, "m" }, - {"pos2-rejionno", 1, (void *)&prcopt_.maxinno, "m" }, - {"pos2-rejgdop", 1, (void *)&prcopt_.maxgdop, "" }, + {"pos2-dopthres", 1, (void *)&prcopt_.thresdop, "m" }, + {"pos2-rejionno", 1, (void *)&prcopt_.maxinno[0], "m" }, + {"pos2-rejcode", 1, (void *)&prcopt_.maxinno[1], "m" }, {"pos2-niter", 0, (void *)&prcopt_.niter, "" }, {"pos2-baselen", 1, (void *)&prcopt_.baseline[0],"m" }, {"pos2-basesig", 1, (void *)&prcopt_.baseline[1],"m" }, diff --git a/src/pntpos.c b/src/pntpos.c index 99a49c7f1..c6e5c894f 100644 --- a/src/pntpos.c +++ b/src/pntpos.c @@ -44,6 +44,7 @@ #define ERR_CBIAS 0.3 /* code bias error Std (m) */ #define REL_HUMI 0.7 /* relative humidity for Saastamoinen model */ #define MIN_EL (5.0*D2R) /* min elevation for measurement error (rad) */ +# define MAX_GDOP 30 /* max gdop for valid solution */ /* pseudorange measurement error variance ------------------------------------*/ static double varerr(const prcopt_t *opt, const ssat_t *ssat, const obsd_t *obs, double el, int sys) @@ -383,7 +384,7 @@ static int valsol(const double *azel, const int *vsat, int n, ns++; } dops(ns,azels,opt->elmin,dop); - if (dop[0]<=0.0||dop[0]>opt->maxgdop) { + if (dop[0]<=0.0||dop[0]>MAX_GDOP) { sprintf(msg,"gdop error nv=%d gdop=%.1f",nv,dop[0]); return 0; } diff --git a/src/postpos.c b/src/postpos.c index c5bef2977..56df5427c 100644 --- a/src/postpos.c +++ b/src/postpos.c @@ -242,6 +242,7 @@ static int inputobs(obsd_t *obs, int solq, const prcopt_t *popt) { gtime_t time={0}; int i,nu,nr,n=0; + double dt,dt_next; trace(3,"\ninfunc : revs=%d iobsu=%d iobsr=%d isbs=%d\n",revs,iobsu,iobsr,isbs); @@ -254,12 +255,18 @@ static int inputobs(obsd_t *obs, int solq, const prcopt_t *popt) if (!revs) { /* input forward data */ if ((nu=nextobsf(&obss,&iobsu,1))<=0) return -1; if (popt->intpref) { + /* interpolate nearest timestamps */ for (;(nr=nextobsf(&obss,&iobsr,2))>0;iobsr+=nr) if (timediff(obss.data[iobsr].time,obss.data[iobsu].time)>-DTTOL) break; } else { - for (i=iobsr;(nr=nextobsf(&obss,&i,2))>0;iobsr=i,i+=nr) - if (timediff(obss.data[i].time,obss.data[iobsu].time)>DTTOL) break; + /* find closest timestamp */ + dt=timediff(obss.data[i].time,obss.data[iobsu].time); + for (i=iobsr;(nr=nextobsf(&obss,&i,2))>0;iobsr=i,i+=nr) { + dt_next=timediff(obss.data[i].time,obss.data[iobsu].time); + if (fabs(dt_next)>fabs(dt)) break; + dt=dt_next; + } } nr=nextobsf(&obss,&iobsr,2); if (nr<=0) { @@ -287,12 +294,18 @@ static int inputobs(obsd_t *obs, int solq, const prcopt_t *popt) else { /* input backward data */ if ((nu=nextobsb(&obss,&iobsu,1))<=0) return -1; if (popt->intpref) { + /* interpolate nearest timestamps */ for (;(nr=nextobsb(&obss,&iobsr,2))>0;iobsr-=nr) if (timediff(obss.data[iobsr].time,obss.data[iobsu].time)0;iobsr=i,i-=nr) - if (timediff(obss.data[i].time,obss.data[iobsu].time)<-DTTOL) break; + /* find closest timestamp */ + dt=timediff(obss.data[i].time,obss.data[iobsu].time); + for (i=iobsr;(nr=nextobsb(&obss,&i,2))>0;iobsr=i,i-=nr) { + dt_next=timediff(obss.data[i].time,obss.data[iobsu].time); + if (fabs(dt_next)>fabs(dt)) break; + dt=dt_next; + } } nr=nextobsb(&obss,&iobsr,2); for (i=0;imaxinno>0.0&&fabs(v[nv])>opt->maxinno) { + if (!post&&opt->maxinno[code]>0.0&&fabs(v[nv])>opt->maxinno[code]) { trace(2,"outlier (%d) rejected %s sat=%2d %s%d res=%9.4f el=%4.1f\n", post,str,sat,code?"P":"L",frq+1,v[nv],azel[1+i*2]*R2D); exc[i]=1; rtk->ssat[sat-1].rejc[frq]++; diff --git a/src/rtkcmn.c b/src/rtkcmn.c index 540700141..f4786977d 100644 --- a/src/rtkcmn.c +++ b/src/rtkcmn.c @@ -216,7 +216,8 @@ const prcopt_t prcopt_default={ /* defaults processing options */ 5E-12, /* sclkstab */ {3.0,0.25,0.0,1E-9,1E-5,3.0,3.0,0.0}, /* thresar */ 0.0,0.0,0.05,0, /* elmaskar,elmaskhold,thresslip,thresdop, */ - 0.1,0.01,30.0,5.0,30.0, /* varholdamb,gainholdamb,maxtdif,maxinno,maxgdop */ + 0.1,0.01,30.0, /* varholdamb,gainholdamb,maxtdif */ + {5.0,30.0}, /* maxinno {phase,code} */ {0},{0},{0}, /* baseline,ru,rb */ {"",""}, /* anttype */ {{0}},{{0}},{0}, /* antdel,pcv,exsats */ diff --git a/src/rtklib.h b/src/rtklib.h index 4a55f273f..680803ef8 100644 --- a/src/rtklib.h +++ b/src/rtklib.h @@ -59,7 +59,7 @@ extern "C" { #define VER_RTKLIB "demo5" /* library version */ -#define PATCH_LEVEL "b34f.1" /* patch level */ +#define PATCH_LEVEL "gdsc_2022" /* patch level */ #define COPYRIGHT_RTKLIB \ "Copyright (C) 2007-2020 T.Takasu\nAll rights reserved." @@ -100,7 +100,7 @@ extern "C" { #define FREQ3_CMP 1.26852E9 /* BDS B3 frequency (Hz) */ #define EFACT_GPS 1.0 /* error factor: GPS */ -#define EFACT_GLO 1.5 /* error factor: GLONASS */ +#define EFACT_GLO 0.75 /* error factor: GLONASS */ #define EFACT_GAL 1.0 /* error factor: Galileo */ #define EFACT_QZS 1.0 /* error factor: QZSS */ #define EFACT_CMP 1.0 /* error factor: BeiDou */ @@ -1022,8 +1022,7 @@ typedef struct { /* processing options type */ double varholdamb; /* variance for fix-and-hold psuedo measurements (cycle^2) */ double gainholdamb; /* gain used for GLO and SBAS sats to adjust ambiguity */ double maxtdiff; /* max difference of time (sec) */ - double maxinno; /* reject threshold of innovation (m) */ - double maxgdop; /* reject threshold of gdop */ + double maxinno[2]; /* reject threshold of innovation for code and phase (m) */ double baseline[2]; /* baseline length constraint {const,sigma} (m) */ double ru[3]; /* rover position for fixed mode {x,y,z} (ecef) (m) */ double rb[3]; /* base position for relative mode {x,y,z} (ecef) (m) */ diff --git a/src/rtkpos.c b/src/rtkpos.c index a100a5918..f52454a26 100644 --- a/src/rtkpos.c +++ b/src/rtkpos.c @@ -404,10 +404,13 @@ static double varerr(int sat, int sys, double el, double snr_rover, double snr_b int nf=NF(opt),frq,code; frq=f%nf;code=feratio[frq]; - if (fact<=0.0) fact=opt->eratio[0]; - /* adjust variances for constellation */ + if (code) fact=opt->eratio[frq]; + /* else adjust variance between freqs */ + else fact=opt->eratio[frq]/opt->eratio[0]; + + /* adjust variance for constellation */ switch (sys) { case SYS_GPS: fact*=EFACT_GPS;break; case SYS_GLO: fact*=EFACT_GLO;break; @@ -487,7 +490,7 @@ static void udpos(rtk_t *rtk, double tt) } /* initialize position for first epoch */ if (norm(rtk->x,3)<=0.0) { - trace(3,"rr=");tracemat(3,rtk->sol.rr,1,6,15,6); + trace(3,"rr_init=");tracemat(3,rtk->sol.rr,1,6,15,6); for (i=0;i<3;i++) initx(rtk,rtk->sol.rr[i],VAR_POS,i); if (rtk->opt.dynamics) { for (i=3;i<6;i++) initx(rtk,rtk->sol.rr[i],VAR_VEL,i); @@ -896,7 +899,7 @@ static void udstate(rtk_t *rtk, const obsd_t *obs, const int *sat, /* temporal update of position/velocity/acceleration */ udpos(rtk,tt); - + /* temporal update of ionospheric parameters */ if (rtk->opt.ionoopt>=IONOOPT_EST) { bl=baseline(rtk->x,rtk->rb,dr); @@ -1184,7 +1187,7 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt, con int ii,jj,frq,code; trace(3,"ddres : dt=%.4f ns=%d\n",dt,ns); - + /* bl=distance from base to rover, dr=x,y,z components */ bl=baseline(x,rtk->rb,dr); /* translate ecef pos to geodetic pos */ @@ -1325,9 +1328,9 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt, con /* if residual too large, flag as outlier */ /* adjust threshold by error stdev ratio unless one of the phase biases was just initialized*/ - threshadj=code||(P[ii+rtk->nx*ii]==SQR(rtk->opt.std[0]))|| - (P[jj+rtk->nx*jj]==SQR(rtk->opt.std[0]))?opt->eratio[frq]:1; - if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno*threshadj) { + threshadj=(P[ii+rtk->nx*ii]==SQR(rtk->opt.std[0]))|| + (P[jj+rtk->nx*jj]==SQR(rtk->opt.std[0]))?10:1; + if (fabs(v[nv])>opt->maxinno[code]*threshadj) { rtk->ssat[sat[j]-1].vsat[frq]=0; rtk->ssat[sat[j]-1].rejc[frq]++; errmsg(rtk,"outlier rejected (sat=%3d-%3d %s%d v=%.3f)\n", @@ -2019,7 +2022,7 @@ static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr, /* update state and covariance matrix from kalman filter update */ matcpy(rtk->x,xp,rtk->nx,1); matcpy(rtk->P,Pp,rtk->nx,rtk->nx); - + /* update valid satellite status for ambiguity control */ rtk->sol.ns=0; for (i=0;isol.stat=stat; - + return stat!=SOLQ_NONE; } /* initialize RTK control ------------------------------------------------------