disccorr.cpp

linux的gps应用

   // recompute WLstats, and break up the segments where slips are found
   if( (iret = detectWLsmallSlips())) 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("WLD");

   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 obvious slips by computing the first difference (of either WL or GFP)
// and looking for outliers. create new segments where there are slips
// which is either 'WL' or 'GF'.
int GDCPass::detectObviousSlips(string which) throw(Exception)
{
try {
      // TD determine from range noise // ~ 2*range noise/wl2
   const double WLobviousNwlLimit=cfg(WLobviousLimit)*cfg(WLSigma);
   const double GFobviousNwlLimit=cfg(GFobviousLimit)*cfg(GFVariation)/wl21;
   bool outlier;
   int i,j,iret,ibad,igood,nok;
   double limit,wlbias;
   list<Segment>::iterator it;

   // compute 1st differences of (WL bias, GFP-GFR) as which is (WL,GF)
   iret = firstDifferences(which);
   if(iret) return iret;

   if(cfg(Debug) >= 5) dumpSegments(string("D")+which,2,true); // DWL DGF

   // scan the first differences, eliminate outliers and
   // break into segments where there are WL slips.
   limit = (which == string("WL") ? WLobviousNwlLimit : GFobviousNwlLimit);
   it = SegList.begin();
   nok = 0;
   igood = -1;
   outlier = false;
   for(i=0; i<data.size(); i++) {
      if(i < it->nbeg) {
         outlier = false;
         continue;
      }
      if(i > it->nend) {                  // change segments
         if(outlier) {
            if(data[ibad].flag & OK) nok--;
            data[ibad].flag = BAD;
            learn[string("points deleted: ") + which + string(" slip outlier")]++;
            outlier = false;
         }
         it->npts = nok;
         // update nbeg and nend
         while(it->nbeg < it->nend
            && it->nbeg < data.size()
            && !(data[it->nbeg].flag & OK) ) it->nbeg++;
         while(it->nend > it->nbeg
            && it->nend > 0
            && !(data[it->nend].flag & OK) ) it->nend--;
         it++;
         if(it == SegList.end())
            return ReturnOK;
         nok = 0;
      }

      if(!(data[i].flag & OK))
         continue;
      nok++;                                 // nok = # good points in segment

      if(igood == -1) igood = i;             // igood is index of last good point

      if(fabs(A1[i]) > limit) {              // found an outlier (1st diff, cycles)
         outlier = true;
         ibad = i;                           // ibad is index of last bad point
      }
      else if(outlier) {                     // this point good, but not past one(s)
         for(j=igood+1; j<ibad; j++) {
            if(data[j].flag & OK)
               nok--;
            if(data[j].flag & DETECT)
               log << "Warning - found an obvious slip, "
                  << "but marking BAD a point already marked with slip "
                  << GDCUnique << " " << sat
                  << " " << time(j).printf(outFormat) << " " << j << endl;
            data[j].flag = BAD;             // mark all points between as bad
            learn[string("points deleted: ") + which + string(" slip outlier")]++;
         }

            // create a new segment, starting at the last outlier
         it->npts = nok-2;
         // WL slip gross  OR  GF slip gross
         it = createSegment(it,ibad,which+string(" slip gross"));

            // mark it
         data[ibad].flag |= (which == string("WL") ? WLDETECT : GFDETECT);

            // change the bias in the new segment
         if(which == "WL") {
            wlbias = data[ibad].P1;
            it->bias1 = long(wlbias+(wlbias > 0 ? 0.5 : -0.5));   // WL bias (NWL)
         }
         if(which == "GF")
            it->bias2 = data[ibad].L2;                            // GFP bias

            // prep for next point
         nok = 2;
         outlier = false;
         igood = ibad;
      }
      else 
         igood = i;
   }
   it->npts = nok;

   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); }
}

//------------------------------------------------------------------------------------
// compute first differences of data array(s) for WL and GF gross slip detection.
// for WL difference the WLbias (P1); for GF, the GFP (L2) and the GFR (P2)
// Store results in A1, and for GF put the range difference in A2
int GDCPass::firstDifferences(string which) throw(Exception)
{
try {
   if(A1.size() != data.size()) return FatalProblem;

   int i,iprev = -1;

   for(i=0; i<data.size(); i++) {
      // ignore bad data
      if(!(data[i].flag & OK)) {
         A1[i] = A2[i] = 0.0;
         continue;
      }

      // compute first differences - 'change the arrays' A1 and A2
      if(which == string("WL")) {
         if(iprev == -1)
            A1[i] = 0.0;
         else
            A1[i] = (data[i].P1 - data[iprev].P1) / (data[i].ndt-data[iprev].ndt);
      }
      else if(which == string("GF")) {
         if(iprev == -1)            // first difference not defined at first point
            A1[i] = A2[i] = 0.0;
         else {
            // compute first difference of L1 = raw residual GFP-GFR
            A1[i] = (data[i].L1 - data[iprev].L1) / (data[i].ndt-data[iprev].ndt);
            // compute first difference of GFP = L2
            A2[i] = (data[i].L2 - data[iprev].L2) / (data[i].ndt-data[iprev].ndt);
         }
      }

      // go to next point
      iprev = i;
   }

   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 one segment, compute conventional statistics on the
// WL bias and count the number of good points
void GDCPass::WLcomputeStats(list<Segment>::iterator& it) throw(Exception)
{
try {
   // compute WLStats
   it->WLStats.Reset();
   it->npts = 0;

   // loop over data, adding to Stats, and counting good points
   for(int i=it->nbeg; i<=it->nend; i++) {
      if(!(data[i].flag & OK)) continue;
      it->WLStats.Add(data[i].P1 - it->bias1);
      it->npts++;
   }

   // eliminate segments with too few points
   if(it->npts < int(cfg(MinPts)))
      deleteSegment(it,"insufficient data in segment");
}
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 one segment, compute conventional statistics on the
// WL bias, remove small segments, and mark bad points that lie outside N*sigma
void GDCPass::WLsigmaStrip(list<Segment>::iterator& it) throw(Exception)
{
try {
   bool outlier,haveslip;
   unsigned short slip;
   int i,slipindex;
   double wlbias,nsigma,ave;

   // use robust stats on small segments, for big ones stick with conventional
   // 'change the arrays' A1 and A2; they will be used again by WLstatSweep
   if(it->npts < cfg(WLNptsOutlierStats)) {   // robust
      int j;
      double median,mad;
      // put wlbias in A1, but without gaps - let j index good points only from nbeg
      for(j=i=it->nbeg; i<=it->nend; i++) {
         if(!(data[i].flag & OK)) continue;
         wlbias = data[i].P1 - it->bias1;
         A1[j] = wlbias;
         j++;
      }

      mad = Robust::MAD(&(A1[it->nbeg]),j-it->nbeg,median,true);
      nsigma = cfg(WLNSigmaStrip) * mad;
      // change the array : A1 is wlbias, A2 will contain the weights
      ave = Robust::MEstimate(&(A1[it->nbeg]),j-it->nbeg,median,mad,&(A2[it->nbeg]));

      haveslip = false;
      for(j=i=it->nbeg; i<=it->nend; i++) {
         if(!(data[i].flag & OK)) continue;

         wlbias = data[i].P1 - it->bias1;

         // TD ? use weights at all? they remove a lot of points
         // TD add absolute limit?
         if(fabs(wlbias-ave) > nsigma || A2[j] < cfg(WLRobustWeightLimit))
            outlier = true;
         else
            outlier = false;

         // remove points by sigma stripping
         if(outlier) {
            if(data[i].flag & DETECT || i == it->nbeg) {
               haveslip = true;
               slipindex = i;        // mark
               slip = data[i].flag; // save to put on first good point
               //log << "Warning - marking a slip point BAD in WL sigma strip "
               //   << GDCUnique << " " << sat
               //   << " " << time(i).printf(outFormat) << " " << i << endl;
            }
            data[i].flag = BAD;
            learn["points deleted: WL sigma stripping"]++;
            it->npts--;
            it->WLStats.Subtract(wlbias);
         }
         else if(haveslip) {
            data[i].flag = slip;
            haveslip = false;
         }

         if(cfg(Debug) >= 6) {
            log << "DSCWLR " << GDCUnique << " " << sat
            << " " << it->nseg
            << " " << time(i).printf(outFormat)
            << fixed << setprecision(3)
            << " " << setw(3) << data[i].flag
            << " " << setw(13) << A1[j]         // wlbias
            << " " << setw(13) << fabs(wlbias-ave)
            << " " << setw(5) << A2[j]          // 0 <= weight <= 1
            << " " << setw(3) << i
            << (outlier ? " outlier" : "");
            if(i == it->nbeg) log
               << " " << setw(13) << it->bias1
               << " " << setw(13) << it->bias2;
            log << endl;
         }

         j++;
      }

   }
   else {                                             // conventional

      //nsigma = WLsigmaStrippingNsigmaLimit * it->WLStats.StdDev();
      nsigma = cfg(WLNSigmaStrip) * it->WLStats.StdDev();

      haveslip = false;
      ave = it->WLStats.Average();
      for(i=it->nbeg; i<=it->nend; i++) {
         if(!(data[i].flag & OK)) continue;

         wlbias = data[i].P1 - it->bias1;

         // remove points by sigma stripping
         if(fabs(wlbias-ave) > nsigma) { // TD add absolute limit?
            if(data[i].flag & DETECT) {
               haveslip = true;
               slipindex = i;        // mark
               slip = data[i].flag; // save to put on first good point
               //log << "Warning - marking a slip point BAD in WL sigma strip "
               //   << GDCUnique << " " << sat
               //   << " " << time(i).printf(outFormat) << " " << i << endl;
            }
            data[i].flag = BAD;
            learn["points deleted: WL sigma stripping"]++;
            it->npts--;
            it->WLStats.Subtract(wlbias);
         }
         else if(haveslip) {
            data[i].flag = slip;
            haveslip = false;
         }

      }  // loop over points in segment
   }

   // change nbeg, but don't change the bias
   if(haveslip) {
      it->nbeg = slipindex;
      //wlbias = data[slipindex].P1;
      //it->bias1 = long(wlbias+(wlbias > 0 ? 0.5 : -0.5));
   }

   // again
   if(it->npts < int(cfg(MinPts)))
      deleteSegment(it,"WL sigma stripping");
   else {
      // update nbeg and nend // TD add limit 0 data.size()
      while(it->nbeg < it->nend && !(data[it->nbeg].flag & OK)) it->nbeg++;
      while(it->nend > it->nbeg && !(data[it->nend].flag & OK)) it->nend--;
   }

}
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); }
}

//------------------------------------------------------------------------------------
// in the given segment, compute statistics on the WL bias using a
// 'two-paned sliding window', each pane of width 'width' good points.
// store the results in the parallel (to SatPass::data) arrays A1,A2.
int GDCPass::WLstatSweep(list<Segment>::iterator& it, int width) throw(Exception)
{
try {
   int iminus,i,iplus;
   double wlbias,test,limit;
   Stats<double> pastStats, futureStats;

   // ignore empty segments
   if(it->npts == 0) return ReturnOK;
   it->WLsweep = true;

      // Cartoon of the 'two-pane moving window'
      // windows:  'past window'      'future window'
      // stats  :  --- pastStats----  ----futureStats--
      // data   : (x x x x x x x x x)(x x x x x x x x x) x ...
      //           |               |  |                  |
      // indexes: iminus          i-1 i                 iplus

   // start with the window 'squashed' to one point - the first one
   iminus = i = iplus = it->nbeg;

   // fill up the future window to size 'width', but don't go beyond the segment
   while(futureStats.N() < width && iplus <= it->nend) {
      if(data[iplus].flag & OK) {                // add only good data
         futureStats.Add(data[iplus].P1 - it->bias1);
      }