disccorr.cpp

linux的gps应用

// fit a polynomial to the GF range, and change the units of -gfr(P2) and gfp(L2)
// to cycles of wl21 (=5.4cm)
int GDCPass::prepareGFdata(void) throw(Exception)
{
try {
   bool first;
   int i,nbeg,nend;
   unsigned ndeg;

   // decide on the degree of fit
   nbeg = SegList.begin()->nbeg;
   nend = SegList.begin()->nend;
   ndeg = 2 + int(0.5 + (nend-nbeg+1)*cfg(DT)/3000.0);
   if(ndeg > 6) ndeg = 6;
   //if(ndeg > int(cfg(GFPolyMaxDegree))) ndeg = int(cfg(GFPolyMaxDegree));
   if(ndeg < 2) ndeg = 2;

   // global fit to the gfr
   GFPassFit.Reset(ndeg);

   for(first=true,i=nbeg; i <= nend; i++) {
      if(!(data[i].flag & OK)) continue;

      // 'change the bias' in the GFP by changing units, also
      // slip fixing in the WL may have changed the values of GFP
      if(first) {
         //if(fabs(data[i].L2 - SegList.begin()->bias2)/wl21 > 10.) {
         //   SegList.begin()->bias2 = data[i].L2;
         //}
         SegList.begin()->bias2 /= wl21;
         first = false;
      }

      // 'change the arrays'
      // change units on the GFP and the GFR
      data[i].P2 /= wl21;                    // gfr (cycles of wl21)
      data[i].L2 /= wl21;                    // gfp (cycles of wl21)

      // compute polynomial fit
      GFPassFit.Add(data[i].P2,data[i].ndt);

      // 'change the data'
      // save in L1                          // gfp+gfr residual (cycles of wl21)
      // ?? data[i].L1 = data[i].L2 - data[i].P2 - SegList.begin()->bias2;
      data[i].L1 = data[i].L2 - data[i].P2;
   }

   if(GFPassFit.isSingular()) {
      log << "Polynomial fit to GF range is singular! .. abort." << endl;
      return Singular;
   }

   return ReturnOK;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
catch(exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

//------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------
// detect slips in the geometry-free phase
int GDCPass::detectGFslips(void) throw(Exception)
{
try {
   int i,iret;
   double bias;
   list<Segment>::iterator it;

   // places first difference of GF in A1 - 'change the arrays' A1
   if( (iret = detectObviousSlips("GF"))) return iret;

   GFPassStats.Reset();
   for(it=SegList.begin(); it != SegList.end(); it++) {
      // save for debiasing below
      // TD what if this segment deleted?
      if(it == SegList.begin()) bias = it->bias2;

      // compute stats on dGF/dt
      for(i=it->nbeg; i <= it->nend; i++) {
         if(!(data[i].flag & OK)) continue;

         // compute first-diff stats in meters
         // skip the first point in a segment - it is an obvious GF slip
         if(i > it->nbeg) GFPassStats.Add(A1[i]*wl21);

         // if a gross GF slip was found, must remove bias in L1=GF(R-P)
         // in all subsequent segments
         if(it != SegList.begin()) data[i].L1 += bias - it->bias2;
      }

      // delete segments if sigma too high?

      // check number of good points
      if(it->npts < int(cfg(MinPts))) {
         deleteSegment(it,"insufficient data in segment");
         continue;
      }

      // fit polynomial to GFR in each segment
      // compute (1stD of) fit residual GFP-fit(GFR) -> A1 - 'change the arrays' A1
      // delete segment if polynomial is singular - probably due to too little data
      if( (iret = GFphaseResiduals(it))) {
         //return iret;
         deleteSegment(it,"polynomial fit to GF residual failed");
         continue;
      }
   }

   // 'change the arrays'
   // at this point:
   // L1 = GFP+GFR in cycles, by prepareGFdata()
   // L2 = GFP in cycles, by prepareGFdata()
   // P1 = wlbias
   // P2 = GFR in cycles, by prepareGFdata() 
   // A1 = GFP-(local fit) OR its 1stD, by GFphaseResiduals()
   //      (was 1stD of GFP+GFR (in L1), by firstDifferences())
   // A2 = 1stD of GFP (in L2), by firstDifferences()
   if( (iret = detectGFsmallSlips())) return iret;

   // delete all segments that are too small
   for(it=SegList.begin(); it != SegList.end(); it++) {
      if(it->npts < int(cfg(MinPts)))
         deleteSegment(it,"insufficient data in segment");
   }

   if(cfg(Debug) >= 4) dumpSegments("GFD",2,true);

   return ReturnOK;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
catch(exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

//------------------------------------------------------------------------------------
// for each segment, fit a polynomial to the gfr, then compute and store the
// residual of fit
int GDCPass::GFphaseResiduals(list<Segment>::iterator& it) throw(Exception)
{
try {
   int i,j,ndeg,nprev;
   double fit,rbias,prev,tmp;
   Stats<double> rofStats;

   // decide on the degree of fit
   ndeg = 2 + int(0.5 + (it->nend-it->nbeg+1)*cfg(DT)/3000.0);
   //if(ndeg > int(cfg(GFPolyMaxDegree))) ndeg = int(cfg(GFPolyMaxDegree));
   if(ndeg > 6) ndeg = 6;
   if(ndeg < 2) ndeg = 2;

   it->PF.Reset(ndeg);     // for fit to GF range

   for(i=it->nbeg; i <= it->nend; i++) {
      if(!(data[i].flag & OK)) continue;
      it->PF.Add(data[i].P2,data[i].ndt);
   }

   if(it->PF.isSingular()) {     // this should never happen
      log << "Polynomial fit to GF range is singular in segment " << it->nseg
         << "! .. abort." << endl;
      return Singular;
   }

   // now compute the residual of fit
   rbias = prev = 0.0;
   rofStats.Reset();
   for(i=it->nbeg; i <= it->nend; i++) {
      if(!(data[i].flag & OK)) continue;
      
      // TD? Use whole pass for small segments?
      //fit = GFPassFit.Evaluate(data[i].ndt);     // use fit to gfr for whole pass
      fit = it->PF.Evaluate(data[i].ndt);

      // all (fit, resid, gfr and gfp) are in cycles of wl21 (5.4cm)

      // compute gfp-(fit to gfr), store in A1 - 'change the arrays' A1 and A2
      // OR let's try first difference of residual of fit
      A1[i] = data[i].L2 - it->bias2 - fit; // residual: phase - fit to range
      if(rbias == 0.0) { rbias = A1[i]; nprev = data[i].ndt - 1; }
      A1[i] -= rbias;                       // debias residual for plots

         // compute stats on residual of fit
      rofStats.Add(A1[i]);

      if(1) { // 1stD of residual - remember A1 has just been debiased
         tmp = A1[i];
         A1[i] -= prev;       // diff with previous epoch's
         A1[i] /= (data[i].ndt - nprev);
         prev = tmp;          // store residual for next point
         nprev = data[i].ndt;
      }
      
      // store fit in A2
      //A2[i] = fit;                         // fit to gfr (cycles of wl21)
      // store raw residual GFP-GFR (cycles of wl21) in A2
      //A2[i] = data[i].L2 - it->bias2 - data[i].P2;
   }

   // TD? need this? use this?
   //log << "GFDsum " << GDCUnique << " " << sat << " " << it->nseg << " " << ndeg
   //   << " " << it->nbeg << " " << it->npts << " " << it->nend
   //   << " " << rofStats.N() << fixed << setprecision(3)
   //   << " " << rofStats.Minimum()
   //   << " " << rofStats.Maximum()
   //   << " " << rofStats.Average()
   //   << " " << rofStats.StdDev() << endl;

   return ReturnOK;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
catch(exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

//------------------------------------------------------------------------------------
// detect small slips in the geometry-free phase
// TD outliers at the beginning or end of the segment....
int GDCPass::detectGFsmallSlips(void) throw(Exception)
{
try {
   const int width=int(cfg(GFSlipWidth));
   int i,j,iplus,inew,ifirst,nok;
   list<Segment>::iterator it;
   Stats<double> pastStats,futureStats;

   // loop over segments
   for(it=SegList.begin(); it != SegList.end(); it++) {

      if(it->npts < 2*width+1) continue;

      // Cartoon of the GF 'two-pane moving window'
      //          point of interest:|
      // windows:     'past window' | 'future window'
      // stats  :        pastStats  | futureStats  (5 pts in each window)
      // data   : ... x (x x x x x) x (x x x x x) x ...
      //                 |          |          |
      // indexes:        j          i        iplus

      deque<int> pastIndex, futureIndex;
      pastStats.Reset();
      futureStats.Reset();
      i = inew = ifirst = -1;
      nok = 0;                          // recount

      // loop over points in the segment
      for(iplus=it->nbeg; iplus<=it->nend+width; iplus++) {

         // ignore bad points
         if(iplus <= it->nend && !(data[iplus].flag & OK)) continue;
         if(ifirst == -1) ifirst = iplus;

         // pop the new i from the future
         if(futureIndex.size() == width || iplus > it->nend) {
            inew = futureIndex.front();
            futureIndex.pop_front();
            futureStats.Subtract(A1[inew]);
            nok++;
         }

         // put iplus into the future deque
         if(iplus <= it->nend) {
            futureIndex.push_back(iplus);
            futureStats.Add(A1[iplus]);
         }
         else
            futureIndex.push_back(-1);

         // check for outliers
         // we now have:
         //                (  past   )     ( future  )
         // data   : ... x (x x x x x) x x (x x x x x) x ...
         //                            | |          |
         // indexes:                   i inew     iplus
         // outlier if: (i,inew) = opposite signs but ~= large magnitude
         // if found, mark i bad and replace A1(inew) = A1(inew)+A1(i)
         if(foundGFoutlier(i,inew,pastStats,futureStats)) {
            // check that i was not marked a slip in the last iteration
            // if so, let inew be the slip and i the outlier
            if(data[i].flag & DETECT) {
               //log << "Warning - marking a slip point BAD in GF detect small "
               //   << GDCUnique << " " << sat
               //   << " " << time(i).printf(outFormat) << " " << i << endl;
               data[inew].flag = data[i].flag;
               it->nbeg = inew;
            }
            data[i].flag = BAD;
            A1[inew] += A1[i];
            learn["points deleted: GF outlier"]++;
            i = inew;
            nok--;
         }

         // pop last from past
         if(pastIndex.size() == width) {
            j = pastIndex.front();
            pastIndex.pop_front();
            pastStats.Subtract(A1[j]);
         }

         // move i into the past
         if(i > -1) {
            pastIndex.push_back(i);
            pastStats.Add(A1[i]);
         }

         // return to original state
         i = inew;

         // test for slip .. foundGF...prints to log
         if(foundGFsmallSlip(i,it->nseg,it->nend,it->nbeg,
            pastIndex,futureIndex,pastStats,futureStats)) {

            // create a new segment
            it->npts = nok-1;
            it = createSegment(it,i,"GF slip small");
            nok = 1;

            // mark it
            data[i].flag |= GFDETECT;

            // TD print the "possible GF slip" and timetag here - see WLS
         }

      }  // end loop over points in the pass
      it->npts = nok;

   }  // end loop over segments

   return ReturnOK;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
catch(exception& e) { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

//------------------------------------------------------------------------------------
bool GDCPass::foundGFoutlier(int i, int inew, 
   Stats<double>& pastSt, Stats<double>& futureSt)
   throw(Exception)
{
try {
   if(i < 0 || inew < 0) return false;
   double pmag = A1[i]; // -pastSt.Average();
   double fmag = A1[inew]; // -futureSt.Average();
   double var = sqrt(pastSt.Variance() + futureSt.Variance());

   if(cfg(Debug) >= 7) log << "GFoutlier " << GDCUnique
      << " " << sat << " " << setw(3) << inew
      << " " << time(inew).printf(outFormat)
      << fixed << setprecision(3)
      << " mags: " << pmag << " ~=? " << -fmag
      //<< "; dmag/mag: " << 2*fabs((pmag+fmag)/(pmag-fmag)) << " <? " << 0.3
      << "; mag/noise: " << fabs(pmag)/var <<" & "<< fabs(fmag)/var << " >? " << 5;

   if(pmag * fmag >= 0)                               // opposite signs
      { if(cfg(Debug) >= 7) log << endl; return false; }

   //if(fabs(pmag+fmag) > 0.15*fabs(pmag-fmag))         // approx equal magnitude
      //{ if(cfg(Debug) >= 7) log << endl; return false; }

   if(fabs(pmag) < cfg(GFSlipOutlier)*var ||
      fabs(fmag) < cfg(GFSlipOutlier)*var)   // and large
   {
      if(cfg(Debug) >= 7) log << endl;
      return false;
   }

   if(cfg(Debug) >= 7) log << " possible GF outlier" << endl;