diff --git a/src/options.c b/src/options.c
index 5241fd941abfdd79d8609cf580e5c0cae5672889..104b5596401ded8616bd8b6b37299a175d17e04d 100644
--- a/src/options.c
+++ b/src/options.c
@@ -47,6 +47,7 @@ static char snrmask_[NFREQ][1024];
#define EPHOPT "0:brdc,1:precise,2:brdc+sbas,3:brdc+ssrapc,4:brdc+ssrcom"
#define NAVOPT "1:gps+2:sbas+4:glo+8:gal+16:qzs+32:comp"
#define GAROPT "0:off,1:on,2:autocal,3:fix-and-hold"
+#define WEIGHTOPT "0:elevation,1:snr"
#define SOLOPT "0:llh,1:xyz,2:enu,3:nmea"
#define TSYOPT "0:gpst,1:utc,2:jst"
#define TFTOPT "0:tow,1:hms"
@@ -87,7 +88,7 @@ EXPORT opt_t sysopts[]={
{"pos2-armode", 3, (void *)&prcopt_.modear, ARMOPT },
{"pos2-gloarmode", 3, (void *)&prcopt_.glomodear, GAROPT },
{"pos2-bdsarmode", 3, (void *)&prcopt_.bdsmodear, SWTOPT },
- {"pos2-arfilter", 3, (void *)&prcopt_.arfilter, SWTOPT },
+ {"pos2-arfilter", 3, (void *)&prcopt_.arfilter, SWTOPT },
{"pos2-arthres", 1, (void *)&prcopt_.thresar[0], "" },
{"pos2-arthres1", 1, (void *)&prcopt_.thresar[1], "" },
{"pos2-arthres2", 1, (void *)&prcopt_.thresar[2], "" },
@@ -96,10 +97,10 @@ EXPORT opt_t sysopts[]={
{"pos2-varholdamb", 1, (void *)&prcopt_.varholdamb, "cyc^2"},
{"pos2-gainholdamb",1, (void *)&prcopt_.gainholdamb,"" },
{"pos2-arlockcnt", 0, (void *)&prcopt_.minlock, "" },
- {"pos2-minfixsats", 0, (void *)&prcopt_.minfixsats, "" },
- {"pos2-minholdsats",0, (void *)&prcopt_.minholdsats,"" },
+ {"pos2-minfixsats", 0, (void *)&prcopt_.minfixsats, "" },
+ {"pos2-minholdsats",0, (void *)&prcopt_.minholdsats,"" },
{"pos2-mindropsats",0, (void *)&prcopt_.mindropsats,"" },
- {"pos2-rcvstds", 3, (void *)&prcopt_.rcvstds, SWTOPT },
+ {"pos2-rcvstds", 3, (void *)&prcopt_.rcvstds, SWTOPT },
{"pos2-arelmask", 1, (void *)&elmaskar_, "deg" },
{"pos2-arminfix", 0, (void *)&prcopt_.minfix, "" },
{"pos2-armaxiter", 0, (void *)&prcopt_.armaxiter, "" },
@@ -132,12 +133,14 @@ EXPORT opt_t sysopts[]={
{"out-nmeaintv2", 1, (void *)&solopt_.nmeaintv[1],"s" },
{"out-outstat", 3, (void *)&solopt_.sstat, STSOPT },
+ {"stats-weightmode",3, (void *)&prcopt_.weightmode, WEIGHTOPT},
{"stats-eratio1", 1, (void *)&prcopt_.eratio[0], "" },
{"stats-eratio2", 1, (void *)&prcopt_.eratio[1], "" },
{"stats-errphase", 1, (void *)&prcopt_.err[1], "m" },
{"stats-errphaseel",1, (void *)&prcopt_.err[2], "m" },
{"stats-errphasebl",1, (void *)&prcopt_.err[3], "m/10km"},
{"stats-errdoppler",1, (void *)&prcopt_.err[4], "Hz" },
+ {"stats-snrmax", 1, (void *)&prcopt_.err[5], "dB.Hz"},
{"stats-stdbias", 1, (void *)&prcopt_.std[0], "m" },
{"stats-stdiono", 1, (void *)&prcopt_.std[1], "m" },
{"stats-stdtrop", 1, (void *)&prcopt_.std[2], "m" },
diff --git a/src/pntpos.c b/src/pntpos.c
index bbf8d90a8286ae84c079b77852ef40973890dccd..630cee04a8153c133a56170d6a2e56895f8ccc26 100644
--- a/src/pntpos.c
+++ b/src/pntpos.c
@@ -23,6 +23,7 @@
/* constants -----------------------------------------------------------------*/
#define SQR(x) ((x)*(x))
+#define MAX(x,y) ((x)>=(y)?(x):(y))
#define NX (4+3) /* # of estimated parameters */
@@ -35,13 +36,32 @@
#define REL_HUMI 0.7 /* relative humidity for saastamoinen model */
/* pseudorange measurement error variance ------------------------------------*/
-static double varerr(const prcopt_t *opt, double el, int sys)
+static double varerr(const prcopt_t *opt, double el, double snr_rover, int sys)
{
- double fact,varr;
- fact=sys==SYS_GLO?EFACT_GLO:(sys==SYS_SBS?EFACT_SBS:EFACT_GPS);
- varr=SQR(opt->err[0])*(SQR(opt->err[1])+SQR(opt->err[2])/sin(el));
- if (opt->ionoopt==IONOOPT_IFLC) varr*=SQR(3.0); /* iono-free */
- return SQR(fact)*varr;
+ double a, b, snr_max;
+ double fact = opt->err[0];
+ double sinel = sin(el);
+
+ switch (sys) {
+ case SYS_GPS: fact *= EFACT_GPS; break;
+ case SYS_GLO: fact *= EFACT_GLO; break;
+ case SYS_SBS: fact *= EFACT_SBS; break;
+ default: fact *= EFACT_GPS; break;
+ }
+
+ a = fact * opt->err[1];
+ b = fact * opt->err[2];
+ snr_max = opt->err[5];
+
+ /* note: SQR(3.0) is approximated scale factor for error variance
+ in the case of iono-free combination */
+ fact = (opt->ionoopt == IONOOPT_IFLC) ? SQR(3.0) : 1.0;
+ switch (opt->weightmode) {
+ case WEIGHTOPT_ELEVATION: return fact * ( SQR(a) + SQR(b / sinel) );
+ case WEIGHTOPT_SNR : return fact * SQR(a) * pow(10, 0.1 * MAX(snr_max - snr_rover, 0));
+ ;
+ default: return 0;
+ }
}
/* get tgd parameter (m) -----------------------------------------------------*/
static double gettgd(int sat, const nav_t *nav)
@@ -198,10 +218,11 @@ extern int tropcorr(gtime_t time, const nav_t *nav, const double *pos,
static int rescode(int iter, const obsd_t *obs, int n, const double *rs,
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const double *x, const prcopt_t *opt,
- double *v, double *H, double *var, double *azel, int *vsat,
- double *resp, int *ns)
+ const ssat_t *ssat, double *v, double *H, double *var,
+ double *azel, int *vsat, double *resp, int *ns)
{
double r,dion,dtrp,vmeas,vion,vtrp,rr[3],pos[3],dtr,e[3],P,lam_L1;
+ double snr_rover = (ssat) ? 0.25 * ssat->snr_rover[0] : opt->err[5];
int i,j,nv=0,sys,mask[4]={0};
trace(3,"resprng : n=%d\n",n);
@@ -261,7 +282,7 @@ static int rescode(int iter, const obsd_t *obs, int n, const double *rs,
vsat[i]=1; resp[i]=v[nv]; (*ns)++;
/* error variance */
- var[nv++]=varerr(opt,azel[1+i*2],sys)+vare[i]+vmeas+vion+vtrp;
+ var[nv++]=varerr(opt,azel[1+i*2],snr_rover,sys)+vare[i]+vmeas+vion+vtrp;
trace(4,"sat=%2d azel=%5.1f %4.1f res=%7.3f sig=%5.3f\n",obs[i].sat,
azel[i*2]*R2D,azel[1+i*2]*R2D,resp[i],sqrt(var[nv-1]));
@@ -308,8 +329,8 @@ static int valsol(const double *azel, const int *vsat, int n,
/* estimate receiver position ------------------------------------------------*/
static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
const double *vare, const int *svh, const nav_t *nav,
- const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
- double *resp, char *msg)
+ const prcopt_t *opt, const ssat_t *ssat, sol_t *sol, double *azel,
+ int *vsat, double *resp, char *msg)
{
double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig;
int i,j,k,info,stat,nv,ns;
@@ -323,7 +344,7 @@ static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
for (i=0;i<MAXITR;i++) {
/* pseudorange residuals */
- nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
+ nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,ssat,v,H,var,azel,vsat,resp,
&ns);
if (nv<NX) {
@@ -376,8 +397,8 @@ static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
/* raim fde (failure detection and exclution) -------------------------------*/
static int raim_fde(const obsd_t *obs, int n, const double *rs,
const double *dts, const double *vare, const int *svh,
- const nav_t *nav, const prcopt_t *opt, sol_t *sol,
- double *azel, int *vsat, double *resp, char *msg)
+ const nav_t *nav, const prcopt_t *opt, const ssat_t *ssat,
+ sol_t *sol, double *azel, int *vsat, double *resp, char *msg)
{
obsd_t *obs_e;
sol_t sol_e={{0}};
@@ -403,7 +424,7 @@ static int raim_fde(const obsd_t *obs, int n, const double *rs,
svh_e[k++]=svh[j];
}
/* estimate receiver position without a satellite */
- if (!estpos(obs_e,n-1,rs_e,dts_e,vare_e,svh_e,nav,opt,&sol_e,azel_e,
+ if (!estpos(obs_e,n-1,rs_e,dts_e,vare_e,svh_e,nav,opt,ssat,&sol_e,azel_e,
vsat_e,resp_e,msg_e)) {
trace(3,"raim_fde: exsat=%2d (%s)\n",obs[i].sat,msg);
continue;
@@ -556,6 +577,15 @@ extern int pntpos(const obsd_t *obs, int n, const nav_t *nav,
rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n);
+ if (ssat) {
+ for (i=0;i<MAXSAT;i++) {
+ ssat[i].snr_rover[0]=0;
+ ssat[i].snr_base[0] =0;
+ }
+ for (i=0;i<n;i++)
+ ssat[obs[i].sat-1].snr_rover[0]=obs[i].SNR[0];
+ }
+
if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */
#if 0
opt_.sateph =EPHOPT_BRDC;
@@ -567,11 +597,11 @@ extern int pntpos(const obsd_t *obs, int n, const nav_t *nav,
satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh);
/* estimate receiver position with pseudorange */
- stat=estpos(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
+ stat=estpos(obs,n,rs,dts,var,svh,nav,&opt_,ssat,sol,azel_,vsat,resp,msg);
/* raim fde */
if (!stat&&n>=6&&opt->posopt[4]) {
- stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
+ stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,ssat,sol,azel_,vsat,resp,msg);
}
/* estimate receiver velocity with doppler */
if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat);
@@ -584,12 +614,10 @@ extern int pntpos(const obsd_t *obs, int n, const nav_t *nav,
ssat[i].vs=0;
ssat[i].azel[0]=ssat[i].azel[1]=0.0;
ssat[i].resp[0]=ssat[i].resc[0]=0.0;
- ssat[i].snr[0]=0;
}
for (i=0;i<n;i++) {
ssat[obs[i].sat-1].azel[0]=azel_[ i*2];
ssat[obs[i].sat-1].azel[1]=azel_[1+i*2];
- ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0];
if (!vsat[i]) continue;
ssat[obs[i].sat-1].vs=1;
ssat[obs[i].sat-1].resp[0]=resp[i];
diff --git a/src/ppp.c b/src/ppp.c
index bc48d2e9e60fc40e6f644fc047c03af4ffc44178..82317db56dd84811dac839105a7bdf88ea90a6c9 100644
--- a/src/ppp.c
+++ b/src/ppp.c
@@ -318,19 +318,39 @@ static int model_phw(gtime_t time, int sat, const char *type, int opt,
return 1;
}
/* measurement error variance ------------------------------------------------*/
-static double varerr(int sat, int sys, double el, int freq, int type,
- const prcopt_t *opt)
+static double varerr(int sat, int sys, double el, double snr_rover, int freq,
+ int type, const prcopt_t *opt)
{
- double fact=1.0,sinel=sin(el);
+ double a, b, snr_max;
+ double fact = 1.0;
+ double sinel = sin(el);
- if (type==1) fact*=opt->eratio[freq==0?0:1];
- fact*=sys==SYS_GLO?EFACT_GLO:(sys==SYS_SBS?EFACT_SBS:EFACT_GPS);
+ if (type == 1) fact *= opt->eratio[(freq == 0) ? 0 : 1];
- if (sys==SYS_GPS||sys==SYS_QZS) {
- if (freq==2) fact*=EFACT_GPS_L5; /* GPS/QZS L5 error factor */
+ switch (sys) {
+ case SYS_GPS: fact *= EFACT_GPS; break;
+ case SYS_GLO: fact *= EFACT_GLO; break;
+ case SYS_SBS: fact *= EFACT_SBS; break;
+ default: fact *= EFACT_GPS; break;
+ }
+
+ if ( (sys == SYS_GPS) || (sys == SYS_QZS) ) {
+ if (freq==2) fact *= EFACT_GPS_L5; /* GPS/QZS L5 error factor */
+ }
+
+ a = fact * opt->err[1];
+ b = fact * opt->err[2];
+ snr_max = opt->err[5];
+
+ /* note: SQR(3.0) is approximated scale factor for error variance
+ in the case of iono-free combination */
+ fact = (opt->ionoopt == IONOOPT_IFLC) ? SQR(3.0) : 1.0;
+ switch (opt->weightmode) {
+ case WEIGHTOPT_ELEVATION: return fact * ( SQR(a) + SQR(b / sinel) );
+ case WEIGHTOPT_SNR : return fact * SQR(a) * pow(10, 0.1 * MAX(snr_max - snr_rover, 0));
+ ;
+ default: return 0;
}
- if (opt->ionoopt==IONOOPT_IFLC) fact*=3.0;
- return SQR(fact*opt->err[1])+SQR(fact*opt->err[2]/sinel);
}
/* initialize state and covariance -------------------------------------------*/
static void initx(rtk_t *rtk, double xi, double var, int i)
@@ -1038,7 +1058,7 @@ static int ppp_res(int post, const obsd_t *obs, int n, const double *rs,
else rtk->ssat[sat-1].resp[j/2]=v[nv];
/* variance */
- var[nv]=varerr(obs[i].sat,sys,azel[1+i*2],j/2,j%2,opt)+
+ var[nv]=varerr(obs[i].sat,sys,azel[1+i*2],0.25*rtk->ssat[sat-1].snr_rover[j/2],j/2,j%2,opt)+
vart+SQR(C)*vari+var_rs[i];
if (sys==SYS_GLO&&j%2==1) var[nv]+=VAR_GLO_IFB;
@@ -1126,7 +1146,8 @@ static void update_stat(rtk_t *rtk, const obsd_t *obs, int n, int stat)
rtk->sol.dtr[1]=rtk->x[IC(1,opt)]-rtk->x[IC(0,opt)];
for (i=0;i<n&&i<MAXOBS;i++) for (j=0;j<opt->nf;j++) {
- rtk->ssat[obs[i].sat-1].snr[j]=obs[i].SNR[j];
+ rtk->ssat[obs[i].sat-1].snr_rover[j]=obs[i].SNR[j];
+ rtk->ssat[obs[i].sat-1].snr_base[j] =0;
}
for (i=0;i<MAXSAT;i++) for (j=0;j<opt->nf;j++) {
if (rtk->ssat[i].slip[j]&3) rtk->ssat[i].slipc[j]++;
@@ -1170,8 +1191,12 @@ extern void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel=zeros(2,n);
- for (i=0;i<MAXSAT;i++) for (j=0;j<opt->nf;j++) rtk->ssat[i].fix[j]=0;
-
+ for (i=0;i<MAXSAT;i++) for (j=0;j<opt->nf;j++) {
+ rtk->ssat[i].fix[j]=0;
+ rtk->ssat[obs[i].sat-1].snr_rover[j]=obs[i].SNR[j];
+ rtk->ssat[obs[i].sat-1].snr_base[j] =0;
+ }
+
/* temporal update of ekf states */
udstate_ppp(rtk,obs,n,nav);
diff --git a/src/rtkcmn.c b/src/rtkcmn.c
index 8013b936bf533348d64bee2c75eeeb89ae5e4490..7316af5124384baa66bcd0dbaf68affc0c618490 100644
--- a/src/rtkcmn.c
+++ b/src/rtkcmn.c
@@ -197,8 +197,9 @@ const prcopt_t prcopt_default={ /* defaults processing options */
0,1,0,0,1,0, /* rcvstds,armaxiter,estion,esttrop,dynamics,tidecorr */
1,0,0,0,0, /* niter,codesmooth,intpref,sbascorr,sbassatsel */
0,0, /* rovpos,refpos */
+ WEIGHTOPT_ELEVATION, /* weightmode */
{300.0,300.0,300.0}, /* eratio[] */
- {100.0,0.003,0.003,0.0,1.0},/* err[] */
+ {100.0,0.003,0.003,0.0,1.0,52.0}, /* err[] */
{30.0,0.03,0.3}, /* std[] */
{1E-4,1E-3,1E-4,1E-1,1E-2,0.0}, /* prn[] */
5E-12, /* sclkstab */
diff --git a/src/rtklib.h b/src/rtklib.h
index 80176d7d3fd5ea8566a6cb942332201904430558..988df473f3e3c7f5003918f6304a33e06b3adc7d 100644
--- a/src/rtklib.h
+++ b/src/rtklib.h
@@ -397,6 +397,9 @@ extern "C" {
#define EPHOPT_SSRCOM 4 /* ephemeris option: broadcast + SSR_COM */
#define EPHOPT_LEX 5 /* ephemeris option: QZSS LEX ephemeris */
+#define WEIGHTOPT_ELEVATION 0 /* weighting option: elevation */
+#define WEIGHTOPT_SNR 1 /* weighting option: snr */
+
#define ARMODE_OFF 0 /* AR mode: off */
#define ARMODE_CONT 1 /* AR mode: continuous */
#define ARMODE_INST 2 /* AR mode: instantaneous */
@@ -1092,11 +1095,13 @@ typedef struct { /* processing options type */
int refpos; /* base position for relative mode */
/* (0:pos in prcopt, 1:average of single pos, */
/* 2:read from file, 3:rinex header, 4:rtcm pos) */
+ int weightmode; /* weighting option (WEIGHTOPT_??) */
double eratio[NFREQ]; /* code/phase error ratio */
- double err[5]; /* measurement error factor */
+ double err[6]; /* measurement error factor */
/* [0]:reserved */
/* [1-3]:error factor a/b/c of phase (m) */
/* [4]:doppler frequency (hz) */
+ /* [5]: snr max value (dB.Hz) */
double std[3]; /* initial-state std [0]bias,[1]iono [2]trop */
double prn[6]; /* process-noise std [0]bias,[1]iono [2]trop [3]acch [4]accv [5] pos */
double sclkstab; /* satellite clock stability (sec/sec) */
@@ -1209,9 +1214,10 @@ typedef struct { /* satellite status type */
double azel[2]; /* azimuth/elevation angles {az,el} (rad) */
double resp[NFREQ]; /* residuals of pseudorange (m) */
double resc[NFREQ]; /* residuals of carrier-phase (m) */
- double icbias[NFREQ]; /* glonass IC bias (cycles) */
+ double icbias[NFREQ]; /* glonass IC bias (cycles) */
unsigned char vsat[NFREQ]; /* valid satellite flag */
- unsigned char snr [NFREQ]; /* signal strength (0.25 dBHz) */
+ unsigned char snr_rover [NFREQ]; /* rover signal strength (0.25 dBHz) */
+ unsigned char snr_base [NFREQ]; /* base signal strength (0.25 dBHz) */
unsigned char fix [NFREQ]; /* ambiguity fix flag (1:fix,2:float,3:hold) */
unsigned char slip[NFREQ]; /* cycle-slip flag */
unsigned char half[NFREQ]; /* half-cycle valid flag */
diff --git a/src/rtkpos.c b/src/rtkpos.c
index ee3e84f8e310cfdca7a6d89ccc8cc461dbecb074..c2f38ef730ac79895101b9fdd9845e365bddfe91 100644
--- a/src/rtkpos.c
+++ b/src/rtkpos.c
@@ -49,6 +49,7 @@
#define SQR(x) ((x)*(x))
#define SQRT(x) ((x)<=0.0||(x)!=(x)?0.0:sqrt(x))
#define MIN(x,y) ((x)<=(y)?(x):(y))
+#define MAX(x,y) ((x)>=(y)?(x):(y))
#define ROUND(x) (int)floor((x)+0.5)
#define VAR_POS SQR(30.0) /* initial variance of receiver pos (m^2) */
@@ -356,7 +357,7 @@ static void outsolstat(rtk_t *rtk,const nav_t *nav)
k=IB(i+1,j,&rtk->opt);
fprintf(fp_stat,"$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d,%.2f,%.6f,%.5f,%.5f\n",
week,tow,id,j+1,ssat->azel[0]*R2D,ssat->azel[1]*R2D,
- ssat->resp[j],ssat->resc[j],ssat->vsat[j],ssat->snr[j]*0.25,
+ ssat->resp[j],ssat->resc[j],ssat->vsat[j],ssat->snr_rover[j]*0.25,
ssat->fix[j],ssat->slip[j]&3,ssat->lock[j],ssat->outc[j],
ssat->slipc[j],ssat->rejc[j],dgps?0:rtk->x[k],
dgps?0:rtk->P[k+k*rtk->nx],nav->lam[i][j],ssat->icbias[j]);
@@ -397,26 +398,37 @@ static double gfobs_L1L5(const obsd_t *obs, int i, int j, const double *lam)
double pi=sdobs(obs,i,j,0)*lam[0],pj=sdobs(obs,i,j,2)*lam[2];
return pi==0.0||pj==0.0?0.0:pi-pj;
}
+
/* single-differenced measurement error variance -----------------------------*/
-static double varerr(int sat, int sys, double el, double bl, double dt, int f,
- const prcopt_t *opt, const obsd_t *obs)
+static double varerr(int sat, int sys, double el, double snr_rover, double snr_base,
+ double bl, double dt, int f, const prcopt_t *opt, const obsd_t *obs)
{
- double a,b,c=opt->err[3]*bl/1E4,d=CLIGHT*opt->sclkstab*dt,fact=1.0;
- double sinel=sin(el);
- int i=sys==SYS_GLO?1:(sys==SYS_GAL?2:0),nf=NF(opt),frq,code;
+ double a, b, c = opt->err[3] * bl / 1E4;
+ double snr_max = opt->err[5];
+ double d = CLIGHT * opt->sclkstab * dt;
+ double fact = 1.0;
+ double sinel = sin(el);
+ int i, nf = NF(opt), frq, code;
frq=f%nf;code=f<nf?0:1;
+
+ switch (sys) {
+ case SYS_GPS: i = 0; break;
+ case SYS_GLO: i = 1; break;
+ case SYS_GAL: i = 2; break;
+ default: i = 0; break;
+ }
/* extended error model, not currently used */
- if (code&&opt->exterr.ena[0]) { /* code */
- a=opt->exterr.cerr[i][ frq*2];
- b=opt->exterr.cerr[i][1+frq*2];
- if (sys==SYS_SBS) {a*=EFACT_SBS; b*=EFACT_SBS;}
+ if (code && opt->exterr.ena[0]) { /* code */
+ a = opt->exterr.cerr[i][ frq * 2];
+ b = opt->exterr.cerr[i][1+frq * 2];
+ if (sys == SYS_SBS) { a *= EFACT_SBS; b *= EFACT_SBS; }
}
- else if (!code&&opt->exterr.ena[1]) { /* phase */
- a=opt->exterr.perr[i][ frq*2];
- b=opt->exterr.perr[i][1+frq*2];
- if (sys==SYS_SBS) {a*=EFACT_SBS; b*=EFACT_SBS;}
+ else if (!code && opt->exterr.ena[1]) { /* phase */
+ a = opt->exterr.perr[i][ frq * 2];
+ b = opt->exterr.perr[i][1+frq * 2];
+ if (sys == SYS_SBS) {a *= EFACT_SBS; b *= EFACT_SBS;}
}
else { /* normal error model */
if (opt->rcvstds&& obs->qualL[frq]!='\0'&&obs->qualP[frq]!='\0') {
@@ -424,12 +436,28 @@ static double varerr(int sat, int sys, double el, double bl, double dt, int f,
if (code) fact=opt->eratio[frq]*obs->qualP[frq];
else fact=obs->qualL[frq];
} else if (code) fact=opt->eratio[frq]; /* use err ratio only */
- if (fact<=0.0) fact=opt->eratio[0];
- fact*=sys==SYS_GLO?EFACT_GLO:(sys==SYS_SBS?EFACT_SBS:EFACT_GPS);
- a=fact*opt->err[1];
- b=fact*opt->err[2];
+ if (fact <= 0.0) fact = opt->eratio[0];
+ switch (sys) {
+ case SYS_GPS: fact *= EFACT_GPS; break;
+ case SYS_GLO: fact *= EFACT_GLO; break;
+ case SYS_SBS: fact *= EFACT_SBS; break;
+ default: fact *= EFACT_GPS; break;
+ }
+
+ a = fact * opt->err[1];
+ b = fact * opt->err[2];
+ }
+
+ /* note: SQR(3.0) is approximated scale factor for error variance
+ in the case of iono-free combination */
+ fact = (opt->ionoopt == IONOOPT_IFLC) ? SQR(3.0) : 1.0;
+ switch (opt->weightmode) {
+ case WEIGHTOPT_ELEVATION: return fact * 2.0 * ( SQR(a) + SQR(b / sinel) + SQR(c) ) + SQR(d);
+ case WEIGHTOPT_SNR : return fact * ( SQR(a) *
+ (pow(10, 0.1 * MAX(snr_max - snr_rover, 0)) +
+ pow(10, 0.1 * MAX(snr_max - snr_base, 0))) + SQR(c) ) + SQR(d);
+ default: return 0;
}
- return 2.0*(opt->ionoopt==IONOOPT_IFLC?3.0:1.0)*(a*a+b*b/sinel/sinel+c*c)+d*d;
}
/* baseline length -----------------------------------------------------------*/
static double baseline(const double *ru, const double *rb, double *dr)
@@ -891,7 +919,10 @@ static void udstate(rtk_t *rtk, const obsd_t *obs, const int *sat,
udpos(rtk,tt);
/* temporal update of ionospheric parameters */
- if (rtk->opt.ionoopt>=IONOOPT_EST) {
+ if ( (rtk->opt.ionoopt == IONOOPT_EST) || (rtk->opt.ionoopt == IONOOPT_TEC) ||
+ (rtk->opt.ionoopt == IONOOPT_QZS) || (rtk->opt.ionoopt == IONOOPT_LEX) ||
+ (rtk->opt.ionoopt == IONOOPT_STEC) )
+ {
bl=baseline(rtk->x,rtk->rb,dr);
udion(rtk,tt,bl,sat,ns);
}
@@ -1202,7 +1233,10 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt, con
- only used if kalman filter contains states for ION and TROP delays
usually insignificant for short baselines (<10km)*/
for (i=0;i<ns;i++) {
- if (opt->ionoopt>=IONOOPT_EST) {
+ if ( (rtk->opt.ionoopt == IONOOPT_EST) || (rtk->opt.ionoopt == IONOOPT_TEC) ||
+ (rtk->opt.ionoopt == IONOOPT_QZS) || (rtk->opt.ionoopt == IONOOPT_LEX) ||
+ (rtk->opt.ionoopt == IONOOPT_STEC) )
+ {
im[i]=(ionmapf(posu,azel+iu[i]*2)+ionmapf(posr,azel+ir[i]*2))/2.0;
}
if (opt->tropopt>=TROPOPT_EST) {
@@ -1220,7 +1254,7 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt, con
/* find reference satellite with highest elevation, set to i */
for (i=-1,j=0;j<ns;j++) {
sysi=rtk->ssat[sat[j]-1].sys;
- if (!test_sys(sysi,m) || sysi==SYS_SBS) continue;
+ if (!test_sys(sysi,m) || sysi==SYS_SBS) 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;
}
@@ -1336,9 +1370,15 @@ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt, con
continue;
}
/* single-differenced measurement error variances */
- Ri[nv]=varerr(sat[i],sysi,azel[1+iu[i]*2],bl,dt,f,opt,&obs[iu[i]]);
- Rj[nv]=varerr(sat[j],sysj,azel[1+iu[j]*2],bl,dt,f,opt,&obs[iu[j]]);
-
+ Ri[nv] = varerr(sat[i], sysi, azel[1+iu[i]*2],
+ 0.25 * rtk->ssat[sat[i]-1].snr_rover[frq],
+ 0.25 * rtk->ssat[sat[i]-1].snr_base[frq],
+ bl, dt, f, opt,&obs[iu[i]]);
+ Rj[nv] = varerr(sat[j], sysj, azel[1+iu[j]*2],
+ 0.25 * rtk->ssat[sat[j]-1].snr_rover[frq],
+ 0.25 * rtk->ssat[sat[j]-1].snr_base[frq],
+ bl, dt, f, opt,&obs[iu[j]]);
+
/* set valid data flags */
if (opt->mode>PMODE_DGPS) {
if (!code) rtk->ssat[sat[i]-1].vsat[frq]=rtk->ssat[sat[j]-1].vsat[frq]=1;
@@ -1910,9 +1950,12 @@ static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
/* init satellite status arrays */
for (i=0;i<MAXSAT;i++) {
- rtk->ssat[i].sys=satsys(i+1,NULL); /* gps system */
- for (j=0;j<NFREQ;j++) rtk->ssat[i].vsat[j]=0; /* valid satellite */
- for (j=1;j<NFREQ;j++) rtk->ssat[i].snr [j]=0;
+ rtk->ssat[i].sys=satsys(i+1,NULL); /* gps system */
+ for (j=0;j<NFREQ;j++) {
+ rtk->ssat[i].vsat[j]=0; /* valid satellite */
+ rtk->ssat[i].snr_rover[j]=0;
+ rtk->ssat[i].snr_base[j] =0;
+ }
}
/* compute satellite positions, velocities and clocks */
satposs(time,obs,n,nav,opt->sateph,rs,dts,var,svh);
@@ -1943,6 +1986,13 @@ static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
trace(4,"x(0)="); tracemat(4,rtk->x,1,NR(opt),13,4);
+ for (i=0;i<ns;i++) for (j=0;j<nf;j++) {
+
+ /* snr of base and rover receiver */
+ rtk->ssat[sat[i]-1].snr_rover[j]=obs[iu[i]].SNR[j];
+ rtk->ssat[sat[i]-1].snr_base[j] =obs[ir[i]].SNR[j];
+ }
+
/* initialize Pp,xa to zero, xp to rtk->x */
xp=mat(rtk->nx,1); Pp=zeros(rtk->nx,rtk->nx); xa=mat(rtk->nx,1);
matcpy(xp,rtk->x,rtk->nx,1);
@@ -2116,11 +2166,6 @@ static int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
rtk->ssat[obs[i].sat-1].pt[obs[i].rcv-1][j]=obs[i].time;
rtk->ssat[obs[i].sat-1].ph[obs[i].rcv-1][j]=obs[i].L[j];
}
- for (i=0;i<ns;i++) for (j=0;j<nf;j++) {
-
- /* output snr of rover receiver */
- rtk->ssat[sat[i]-1].snr[j]=obs[iu[i]].SNR[j];
- }
for (i=0;i<MAXSAT;i++) for (j=0;j<nf;j++) {
/* Don't lose track of which sats were used to try and resolve the ambiguities */
/* if (rtk->ssat[i].fix[j]==2&&stat!=SOLQ_FIX) rtk->ssat[i].fix[j]=1; */
diff --git a/src/solution.c b/src/solution.c
index 1fffddf27500d3eab5d24ac1829b65555b083cc0..5fc7a64c87ac75956939cb0817b123783f066c42 100644
--- a/src/solution.c
+++ b/src/solution.c
@@ -1405,7 +1405,7 @@ extern int outnmea_gsv(unsigned char *buff, const sol_t *sol,
if (satsys(sats[k],&prn)==SYS_SBS) prn+=33-MINPRNSBS;
az =ssat[sats[k]-1].azel[0]*R2D; if (az<0.0) az+=360.0;
el =ssat[sats[k]-1].azel[1]*R2D;
- snr=ssat[sats[k]-1].snr[0]*0.25;
+ snr=ssat[sats[k]-1].snr_rover[0]*0.25;
p+=sprintf(p,",%02d,%02.0f,%03.0f,%02.0f",prn,el,az,snr);
}
else p+=sprintf(p,",,,,");
@@ -1430,7 +1430,7 @@ extern int outnmea_gsv(unsigned char *buff, const sol_t *sol,
satsys(sats[k],&prn); prn+=64; /* 65-99 */
az =ssat[sats[k]-1].azel[0]*R2D; if (az<0.0) az+=360.0;
el =ssat[sats[k]-1].azel[1]*R2D;
- snr=ssat[sats[k]-1].snr[0]*0.25;
+ snr=ssat[sats[k]-1].snr_rover[0]*0.25;
p+=sprintf(p,",%02d,%02.0f,%03.0f,%02.0f",prn,el,az,snr);
}
else p+=sprintf(p,",,,,");
@@ -1455,7 +1455,7 @@ extern int outnmea_gsv(unsigned char *buff, const sol_t *sol,
satsys(sats[k],&prn); /* 1-36 */
az =ssat[sats[k]-1].azel[0]*R2D; if (az<0.0) az+=360.0;
el =ssat[sats[k]-1].azel[1]*R2D;
- snr=ssat[sats[k]-1].snr[0]*0.25;
+ snr=ssat[sats[k]-1].snr_rover[0]*0.25;
p+=sprintf(p,",%02d,%02.0f,%03.0f,%02.0f",prn,el,az,snr);
}
else p+=sprintf(p,",,,,");