mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-16 04:05:46 +00:00
Code Issues Releases Wiki Activity

Remove some warnings in rtkpos and make it more readable

Browse Source
This commit is contained in:
Carles Fernandez 2017-04-30 09:08:19 +02:00
parent 9323360526
commit cd87e070a4
2 changed files with 687 additions and 702 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

137
src/algorithms/libs/rtklib/rtklib_rtkpos.cc
View File

@ -380,10 +380,10 @@ double gfobs_L1L5(const obsd_t *obs, int i, int j, const double *lam)
/* single-differenced measurement error variance -----------------------------*/
double varerr(int sat, int sys, double el, double bl, double dt, int f,
double varerr(int sat __attribute((unused)), int sys, double el, double bl, double dt, int f,
const prcopt_t *opt)
{
double a, b, c = opt->err[3]*bl/1E4, d = SPEED_OF_LIGHT*opt->sclkstab*dt, fact = 1.0;
double a, b, c = opt->err[3]*bl/1e4, d = SPEED_OF_LIGHT * opt->sclkstab*dt, fact = 1.0;
double sinel = sin(el);
int i = sys == SYS_GLO ? 1 : (sys == SYS_GAL ? 2 : 0), nf = NF_RTK(opt);
@ -392,13 +392,13 @@ double varerr(int sat, int sys, double el, double bl, double dt, int f,
{ /* code */
a = opt->exterr.cerr[i][ (f-nf)*2];
b = opt->exterr.cerr[i][1+(f-nf)*2];
if (sys == SYS_SBS) {a*=EFACT_SBS; b*=EFACT_SBS;}
if (sys == SYS_SBS) {a *= EFACT_SBS; b *= EFACT_SBS;}
}
else if (f<nf && opt->exterr.ena[1])
{ /* phase */
a = opt->exterr.perr[i][ f*2];
b = opt->exterr.perr[i][1+f*2];
if (sys == SYS_SBS) {a*=EFACT_SBS; b*=EFACT_SBS;}
if (sys == SYS_SBS) {a *= EFACT_SBS; b *= EFACT_SBS;}
}
else
{ /* normal error model */
@ -559,7 +559,7 @@ void udion(rtk_t *rtk, double tt, double bl, const int *sat, int ns)
/* temporal update of tropospheric parameters --------------------------------*/
void udtrop(rtk_t *rtk, double tt, double bl)
void udtrop(rtk_t *rtk, double tt, double bl __attribute((unused)))
{
int i, j, k;
@ -575,12 +575,12 @@ void udtrop(rtk_t *rtk, double tt, double bl)
if (rtk->opt.tropopt >= TROPOPT_ESTG)
{
for (k = 0;k<2;k++) initx(rtk, 1E-6, VAR_GRA, ++j);
for (k = 0;k<2;k++) initx(rtk, 1e-6, VAR_GRA, ++j);
}
}
else
{
rtk->P[j+j*rtk->nx]+=std::pow(rtk->opt.prn[2], 2.0)*tt;
rtk->P[j+j*rtk->nx] += std::pow(rtk->opt.prn[2], 2.0)*tt;
if (rtk->opt.tropopt >= TROPOPT_ESTG)
{
@ -728,30 +728,30 @@ void detslp_dop(rtk_t *rtk, const obsd_t *obs, int i, int rcv,
{
/* detection with doppler disabled because of clock-jump issue (v.2.3.0) */
#if 0
int f,sat=obs[i].sat;
int f,sat = obs[i].sat;
double tt,dph,dpt,lam,thres;
trace(3,"detslp_dop: i=%d rcv=%d\n",i,rcv);
for (f=0;f<rtk->opt.nf;f++)
for (f = 0;f<rtk->opt.nf;f++)
{
if (obs[i].L[f]==0.0||obs[i].D[f]==0.0||rtk->ph[rcv-1][sat-1][f]==0.0)
if (obs[i].L[f] == 0.0 || obs[i].D[f] == 0.0 || rtk->ph[rcv-1][sat-1][f] == 0.0)
{
continue;
}
if (fabs(tt=timediff(obs[i].time,rtk->pt[rcv-1][sat-1][f]))<DTTOL) continue;
if ((lam=nav->lam[sat-1][f])<=0.0) continue;
if (fabs(tt = timediff(obs[i].time,rtk->pt[rcv-1][sat-1][f]))<DTTOL) continue;
if ((lam = nav->lam[sat-1][f])<=0.0) continue;
/* cycle slip threshold (cycle) */
thres=MAXACC*tt*tt/2.0/lam+rtk->opt.err[4]*fabs(tt)*4.0;
thres = MAXACC*tt*tt/2.0/lam+rtk->opt.err[4]*fabs(tt)*4.0;
/* phase difference and doppler x time (cycle) */
dph=obs[i].L[f]-rtk->ph[rcv-1][sat-1][f];
dpt=-obs[i].D[f]*tt;
dph = obs[i].L[f]-rtk->ph[rcv-1][sat-1][f];
dpt = -obs[i].D[f]*tt;
if (fabs(dph-dpt)<=thres) continue;
rtk->slip[sat-1][f]|=1;
rtk->slip[sat-1][f]| = 1;
errmsg(rtk,"slip detected (sat=%2d rcv=%d L%d=%.3f %.3f thres=%.3f)\n",
sat,rcv,f+1,dph,dpt,thres);
@ -772,7 +772,7 @@ void udbias(rtk_t *rtk, double tt, const obsd_t *obs, const int *sat,
for (i = 0;i<ns;i++)
{
/* detect cycle slip by LLI */
for (f = 0;f<rtk->opt.nf;f++) rtk->ssat[sat[i]-1].slip[f]&=0xFC;
for (f = 0;f<rtk->opt.nf;f++) rtk->ssat[sat[i]-1].slip[f] &= 0xFC;
detslp_ll(rtk, obs, iu[i], 1);
detslp_ll(rtk, obs, ir[i], 2);
@ -1133,7 +1133,7 @@ double prectrop(gtime_t time, const double *pos, int r,
/* glonass inter-channel bias correction -------------------------------------*/
double gloicbcorr(int sat1, int sat2, const prcopt_t *opt, double lam1,
double gloicbcorr(int sat1 __attribute((unused)), int sat2 __attribute((unused)), const prcopt_t *opt, double lam1,
double lam2, int f)
{
double dfreq;
@ -1436,12 +1436,10 @@ int ddmat(rtk_t *rtk, double *D)
for (m = 0;m<4;m++)
{ /* m=0:gps/qzs/sbs, 1:glo, 2:gal, 3:bds */
nofix = (m == 1 && rtk->opt.glomodear == 0) || (m == 3 && rtk->opt.bdsmodear == 0);
for (f = 0, k = na;f<nf;f++, k+=MAXSAT)
{
for (i = k;i<k+MAXSAT;i++)
{
#if 0
@ -1485,12 +1483,12 @@ int ddmat(rtk_t *rtk, double *D)
}
trace(5, "D=\n"); tracemat(5, D, nx, na+nb, 2, 0);
return nb;
}
}
/* restore single-differenced ambiguity --------------------------------------*/
void restamb(rtk_t *rtk, const double *bias, int nb, double *xa)
{
/* restore single-differenced ambiguity --------------------------------------*/
void restamb(rtk_t *rtk, const double *bias, int nb __attribute((unused)), double *xa)
{
int i, n, m, f, index[MAXSAT], nv = 0, nf = NF_RTK(&rtk->opt);
trace(3, "restamb :\n");
@ -1500,7 +1498,6 @@ int ddmat(rtk_t *rtk, double *D)
for (m = 0;m<4;m++) for (f = 0;f<nf;f++)
{
for (n = i = 0;i<MAXSAT;i++)
{
if (!test_sys(rtk->ssat[i].sys, m) || rtk->ssat[i].fix[f] != 2)
@ -1518,12 +1515,11 @@ int ddmat(rtk_t *rtk, double *D)
xa[index[i]] = xa[index[0]]-bias[nv++];
}
}
}
}
/* hold integer ambiguity ----------------------------------------------------*/
void holdamb(rtk_t *rtk, const double *xa)
{
/* hold integer ambiguity ----------------------------------------------------*/
void holdamb(rtk_t *rtk, const double *xa)
{
double *v, *H, *R;
int i, n, m, f, info, index[MAXSAT], nb = rtk->nx-rtk->na, nv = 0, nf = NF_RTK(&rtk->opt);
@ -1533,7 +1529,6 @@ int ddmat(rtk_t *rtk, double *D)
for (m = 0;m<4;m++) for (f = 0;f<nf;f++)
{
for (n = i = 0;i<MAXSAT;i++)
{
if (!test_sys(rtk->ssat[i].sys, m) || rtk->ssat[i].fix[f] != 2 ||
@ -1545,7 +1540,8 @@ int ddmat(rtk_t *rtk, double *D)
rtk->ssat[i].fix[f] = 3; /* hold */
}
/* constraint to fixed ambiguity */
for (i = 1;i<n;i++) {
for (i = 1;i<n;i++)
{
v[nv] = (xa[index[0]]-xa[index[i]])-(rtk->x[index[0]]-rtk->x[index[i]]);
H[index[0]+nv*rtk->nx] = 1.0;
@ -1566,12 +1562,11 @@ int ddmat(rtk_t *rtk, double *D)
free(R);
}
free(v); free(H);
}
}
/* resolve integer ambiguity by LAMBDA ---------------------------------------*/
int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa)
{
/* resolve integer ambiguity by LAMBDA ---------------------------------------*/
int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa)
{
prcopt_t *opt = &rtk->opt;
int i, j, ny, nb, info, nx = rtk->nx, na = rtk->na;
double *D, *DP, *y, *Qy, *b, *db, *Qb, *Qab, *QQ, s[2];
@ -1610,7 +1605,6 @@ int ddmat(rtk_t *rtk, double *D)
/* lambda/mlambda integer least-square estimation */
if (!(info = lambda(nb, 2, y+na, Qb, b, s)))
{
trace(4, "N(1)="); tracemat(4, b , 1, nb, 10, 3);
trace(4, "N(2)="); tracemat(4, b+nb, 1, nb, 10, 3);
@ -1663,16 +1657,15 @@ int ddmat(rtk_t *rtk, double *D)
free(b); free(db); free(Qb); free(Qab); free(QQ);
return nb; /* number of ambiguities */
}
}
/* validation of solution ----------------------------------------------------*/
int valpos(rtk_t *rtk, const double *v, const double *R, const int *vflg,
/* validation of solution ----------------------------------------------------*/
int valpos(rtk_t *rtk, const double *v, const double *R, const int *vflg,
int nv, double thres)
{
{
#if 0
prcopt_t *opt=&rtk->opt;
double vv=0.0;
prcopt_t *opt = &rtk->opt;
double vv = 0.0;
#endif
double fact = thres*thres;
int i, stat = 1, sat1, sat2, type, freq;
@ -1695,31 +1688,29 @@ int ddmat(rtk_t *rtk, double *D)
#if 0 /* omitted v.2.4.0 */
if (stat&&nv>NP(opt))
{
/* chi-square validation */
for (i=0;i<nv;i++) vv+=v[i]*v[i]/R[i+i*nv];
for (i = 0; i<nv; i++) vv += v[i]*v[i]/R[i+i*nv];
if (vv>chisqr[nv-NP(opt)-1])
if (vv > chisqr[nv-NP(opt)-1])
{
errmsg(rtk,"residuals validation failed (nv=%d np=%d vv=%.2f cs=%.2f)\n",
nv,NP(opt),vv,chisqr[nv-NP(opt)-1]);
stat=0;
nv, NP(opt), vv, chisqr[nv-NP(opt)-1]);
stat = 0;
}
else
{
trace(3,"valpos : validation ok (%s nv=%d np=%d vv=%.2f cs=%.2f)\n",
rtk->tstr,nv,NP(opt),vv,chisqr[nv-NP(opt)-1]);
rtk->tstr, nv, NP(opt), vv, chisqr[nv-NP(opt)-1]);
}
}
#endif
return stat;
}
}
/* relative positioning ------------------------------------------------------*/
int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
/* relative positioning ------------------------------------------------------*/
int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
const nav_t *nav)
{
{
prcopt_t *opt = &rtk->opt;
gtime_t time = obs[0].time;
double *rs, *dts, *var, *y, *e, *azel, *v, *H, *R, *xp, *Pp, *xa, *bias, dt;
@ -1807,14 +1798,12 @@ int ddmat(rtk_t *rtk, double *D)
}
if (stat != SOLQ_NONE && zdres(0, obs, nu, rs, dts, svh, nav, xp, opt, 0, y, e, azel))
{
/* post-fit residuals for float solution */
nv = ddres(rtk, nav, dt, xp, Pp, sat, y, e, azel, iu, ir, ns, v, NULL, R, vflg);
/* validation of float solution */
if (valpos(rtk, v, R, vflg, nv, 4.0))
{
/* update state and covariance matrix */
matcpy(rtk->x, xp, rtk->nx, 1);
matcpy(rtk->P, Pp, rtk->nx, rtk->nx);
@ -1852,17 +1841,14 @@ int ddmat(rtk_t *rtk, double *D)
/* resolve integer ambiguity by LAMBDA */
else if (stat != SOLQ_NONE && resamb_LAMBDA(rtk, bias, xa)>1)
{
if (zdres(0, obs, nu, rs, dts, svh, nav, xa, opt, 0, y, e, azel))
{
/* post-fit reisiduals for fixed solution */
nv = ddres(rtk, nav, dt, xa, NULL, sat, y, e, azel, iu, ir, ns, v, NULL, R, vflg);
/* validation of fixed solution */
if (valpos(rtk, v, R, vflg, nv, 4.0))
{
/* hold integer ambiguity */
if (++rtk->nfix >= rtk->opt.minfix &&
rtk->opt.modear == ARMODE_FIXHOLD)
@ -1873,6 +1859,7 @@ int ddmat(rtk_t *rtk, double *D)
}
}
}
/* save solution status */
if (stat == SOLQ_FIX)
{
@ -1905,7 +1892,6 @@ int ddmat(rtk_t *rtk, double *D)
}
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];
}
@ -1920,17 +1906,16 @@ int ddmat(rtk_t *rtk, double *D)
if (stat != SOLQ_NONE) rtk->sol.stat = stat;
return stat != SOLQ_NONE;
}
}
/* initialize rtk control ------------------------------------------------------
/* initialize rtk control ------------------------------------------------------
* initialize rtk control struct
* args : rtk_t *rtk IO rtk control/result struct
* prcopt_t *opt I positioning options (see rtklib.h)
* return : none
*-----------------------------------------------------------------------------*/
void rtkinit(rtk_t *rtk, const prcopt_t *opt)
{
void rtkinit(rtk_t *rtk, const prcopt_t *opt)
{
sol_t sol0 = { {0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0};
ambc_t ambc0 = {{ {0, 0}, {0, 0}, {0, 0}, {0, 0} }, {}, {}, {} , 0, {}};
ssat_t ssat0 = {0, 0, {0.0}, {0.0}, {0.0}, {'0'}, {'0'}, {'0'}, {'0'}, {'0'}, {}, {}, {}, {}, 0.0, 0.0, 0.0, 0.0, {{0,0},{0,0}}, {{},{}}};
@ -1955,16 +1940,16 @@ int ddmat(rtk_t *rtk, double *D)
}
for (i = 0;i<MAXERRMSG;i++) rtk->errbuf[i] = 0;
rtk->opt = *opt;
}
}
/* free rtk control ------------------------------------------------------------
/* free rtk control ------------------------------------------------------------
* free memory for rtk control struct
* args : rtk_t *rtk IO rtk control/result struct
* return : none
*-----------------------------------------------------------------------------*/
void rtkfree(rtk_t *rtk)
{
void rtkfree(rtk_t *rtk)
{
trace(3, "rtkfree :\n");
rtk->nx = rtk->na = 0;
@ -1972,10 +1957,10 @@ int ddmat(rtk_t *rtk, double *D)
free(rtk->P ); rtk->P = NULL;
free(rtk->xa); rtk->xa = NULL;
free(rtk->Pa); rtk->Pa = NULL;
}
}
/* precise positioning ---------------------------------------------------------
/* precise positioning ---------------------------------------------------------
* input observation data and navigation message, compute rover position by
* precise positioning
* args : rtk_t *rtk IO rtk control/result struct
@ -2033,8 +2018,8 @@ int ddmat(rtk_t *rtk, double *D)
* notes : before calling function, base station position rtk->sol.rb[] should
* be properly set for relative mode except for moving-baseline
*-----------------------------------------------------------------------------*/
int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
{
int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
{
prcopt_t *opt = &rtk->opt;
sol_t solb = { {0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0};
gtime_t time;
@ -2132,4 +2117,4 @@ int ddmat(rtk_t *rtk, double *D)
outsolstat(rtk);
return 1;
}
}

12
src/algorithms/libs/rtklib/rtklib_rtkpos.h
View File

@ -90,12 +90,12 @@ const double TTOL_MOVEB = (1.0+2*DTTOL);
#define IB_RTK(s,f,opt) (NR_RTK(opt)+MAXSAT*(f)+(s)-1) /* phase bias (s:satno,f:freq) */
static int resamb_WLNL(rtk_t *rtk, const obsd_t *obs, const int *sat,
const int *iu, const int *ir, int ns, const nav_t *nav,
const double *azel) {return 0;}
static int resamb_TCAR(rtk_t *rtk, const obsd_t *obs, const int *sat,
const int *iu, const int *ir, int ns, const nav_t *nav,
const double *azel) {return 0;}
static int resamb_WLNL(rtk_t *rtk __attribute((unused)), const obsd_t *obs __attribute((unused)), const int *sat __attribute((unused)),
const int *iu __attribute((unused)), const int *ir __attribute((unused)), int ns, const nav_t *nav __attribute((unused)),
const double *azel __attribute((unused))) {return 0;}
static int resamb_TCAR(rtk_t *rtk __attribute((unused)), const obsd_t *obs __attribute((unused)), const int *sat __attribute((unused)),
const int *iu __attribute((unused)), const int *ir __attribute((unused)), int ns __attribute((unused)), const nav_t *nav __attribute((unused)),
const double *azel __attribute((unused))) {return 0;}
/* global variables ----------------------------------------------------------*/
static int statlevel = 0; /* rtk status output level (0:off) */