disccorr.cpp

linux的gps应用

      iplus++;
   }

   // now loop over all points in the segment
   for(i=it->nbeg; i<= it->nend; i++) {
      if(!(data[i].flag & OK))                      // add only good data
         continue;

      // compute test and limit
      test = 0;
      if(pastStats.N() > 0 && futureStats.N() > 0)
         test = fabs(futureStats.Average()-pastStats.Average());
      limit = sqrt(futureStats.Variance() + pastStats.Variance());
      // 'change the arrays' A1 and A2
      A1[i] = test;
      A2[i] = limit;

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

      // dump the stats
      if(cfg(Debug) >= 6) log << "WLS " << GDCUnique
         << " " << sat << " " << it->nseg
         << " " << time(i).printf(outFormat)
         << fixed << setprecision(3)
         << " " << setw(3) << pastStats.N()
         << " " << setw(7) << pastStats.Average()
         << " " << setw(7) << pastStats.StdDev()
         << " " << setw(3) << futureStats.N()
         << " " << setw(7) << futureStats.Average()
         << " " << setw(7) << futureStats.StdDev()
         << " " << setw(9) << A1[i]
         << " " << setw(9) << A2[i]
         << " " << setw(9) << wlbias
         << " " << setw(3) << i
         << endl;

      // update stats :
      // move point i from future to past, ...
      futureStats.Subtract(wlbias);
      pastStats.Add(wlbias);
      // ... and move iplus up by one (good) point, ...
      while(futureStats.N() < width && iplus <= it->nend) {
         if(data[iplus].flag & OK) {
            futureStats.Add(data[iplus].P1 - it->bias1);
         }
         iplus++;
      }
      // ... and move iminus up by one good point
      while(pastStats.N() > width && iminus <= it->nend) {// <= nend not really nec.
         if(data[iminus].flag & OK) {
            pastStats.Subtract(data[iminus].P1 - it->bias1);
         }
         iminus++;
      }
   
   }  // end loop over i=all points in segment

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

//------------------------------------------------------------------------------------
// look for slips in the WL using the results of WLstatSweep
// if slip is close to either end (< window width), just chop off the small segment
// recompute WLstats; when a slip is found, create a new segment
int GDCPass::detectWLsmallSlips(void) throw(Exception)
{
try {
   int i,k,nok;
   double wlbias;
   list<Segment>::iterator it;

   // find first segment for which WLstatSweep was called
   it = SegList.begin();
   while(!it->WLsweep) {
      it++;
      if(it == SegList.end()) return ReturnOK;
   }
   it->WLStats.Reset();

   // loop over the data arrays - all segments
   i = it->nbeg;
   nok = 0;
   while(i < data.size())
   {
      // must skip segments for which WLstatSweep was not called
      while(i > it->nend || !it->WLsweep) {
         if(i > it->nend) {
            it->npts = nok;
            nok = 0;
         }
         it++;
         if(it == SegList.end()) return ReturnOK;
         i = it->nbeg;
         if(it->WLsweep) {
            it->WLStats.Reset();
         }
      }

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

         if(nok == 1) {                         // change the bias, as WLStats reset
            wlbias = data[i].P1;
            it->bias1 = long(wlbias+(wlbias > 0 ? 0.5 : -0.5));
         }

         //  condition 3 - near ends of segment?
         if(nok < 2 || (it->npts - nok) < 2 ) {       // TD input 1/2 width
            // failed test 3 - near ends of segment
            // consider chopping off this end of segment - large limit?
            // TD must do something here ... 
            log << "too near end " << GDCUnique
               << " " << i << " " << nok << " " << it->npts-nok
               << " " << time(i).printf(outFormat)
               << " " << A1[i] << " " << A2[i]
               << endl;



         }
         else if(foundWLsmallSlip(it,i)) { // met condition 3
            // create new segment
            // TD what if nok < MinPts? -- cf detectGFsmallSlips
            k = it->npts;
            it->npts = nok;
            //log << "create new segment at i = " << i << " " << nok << "pts\n";
            it = createSegment(it,i,"WL slip small");

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

            // prep for next segment
            // biases remain the same in the new segment
            it->npts = k - nok;
            nok = 0;
            it->WLStats.Reset();
            wlbias = data[i].P1; // change the bias, as WLStats reset
            it->bias1 = long(wlbias+(wlbias > 0 ? 0.5 : -0.5));
         }

         it->WLStats.Add(data[i].P1 - it->bias1);

      } // end if good data

      i++;

   }  // end loop over points in the pass

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

//------------------------------------------------------------------------------------
// determine if a slip has been found at index i, in segment it
// CONDITIONs for a slip to be detected:
// 1. test must be >= ~0.67 wlwl
// 2. limit must be much smaller than test
// 3. slip must be far (>1/2 window) from either end
// 4. test must be at a local maximum within ~ window width
// 5. limit must be at a local minimum (")
// 6. (test-limit)/limit > 1.2
// large limit (esp near end of a pass) means too much noise
bool GDCPass::foundWLsmallSlip(list<Segment>::iterator& it, int i)
   throw(Exception)
{
try {
   const int minMaxWidth=int(cfg(WLSlipEdge)); // test 4,5, = ~~1/2 WLWindowWidth
   int j,jp,jm,pass4,pass5,Pass;

   // A1 = test = fabs(futureStats.Average() - pastStats.Average());
   // A2 = limit = sqrt(futureStats.Variance() + pastStats.Variance());
   // all units WL cycles

   // CONDITION 1     CONDITION 2
   if(A1[i] <= cfg(WLSlipSize) || A1[i]-A2[i] <= cfg(WLSlipExcess)) {
      return false;
   }

   // Debug print
   if(cfg(Debug) >= 6) log << "WLslip " << GDCUnique
      << " " << sat << " " << it->nseg
      << " " << setw(3) << i
      << " " << time(i).printf(outFormat)
      //<< " " << it->npts << "pt"
      << fixed << setprecision(2)
      << " test " << setw(4) << A1[i] << (A1[i]>0.67?">":"<=") << "0.67"
      << ", " << setw(4) << A1[i]-A2[i]
      << (A1[i]-A2[i]>0.1 ? ">" : "<=") << "0.1"
      << ", lim " << setw(4) << A2[i]
      << " (" << (A1[i]-A2[i])/A2[i];
      // no endl

   // CONDITIONs 4 and 5
   // do for 3 points on each side of point - best score is pass=6
   j = pass4 = pass5 = 0;
   jp = jm = i;
   do {
      // find next good point in future
      do { jp++; } while(jp < it->nend && !(data[jp].flag & OK));
         // CONDITION 4: test(A1) is a local maximum
      if(A1[i]-A1[jp] > 0) pass4++;
         // CONDITION 5: limit(A2) is a local maximum
      if(A2[i]-A2[jp] < 0) pass5++;

      // find next good point in past
      do { jm--; } while(jm > it->nbeg && !(data[jm].flag & OK));
         // CONDITION 4: test(A1) is a local maximum
      if(A1[i]-A1[jp] > 0) pass4++;
         // CONDITION 5: limit(A2) is a local maximum
      if(A2[i]-A2[jp] < 0) pass5++;

   } while(++j < minMaxWidth);

   // perfect = 2*minMaxWidth; allow 1 miss...?
   Pass = 0;
   if(pass4 >= 2*minMaxWidth-1) { Pass++; if(cfg(Debug) >= 6) log << " tst_max"; }
   if(pass5 >= 2*minMaxWidth-1) { Pass++; if(cfg(Debug) >= 6) log << " lim_min"; }

   // CONDITION 6
   if( (A1[i]-A2[i])/A2[i] > cfg(WLSlipSeparation) ) {
      Pass++;
      if(cfg(Debug) >= 6) log << " tst_lim_separation";
   }

   if(cfg(Debug) >= 6) log << ")";

   if(Pass == 3) {
		if(cfg(Debug) >= 6) log << " possible WL slip" << endl;
		return true;
	}
   if(cfg(Debug) >= 6) log << endl;

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

//------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------
// estimate slips and adjust biases appropriately - ie fix slips - for both WL and GF
// merge all data into one segment
int GDCPass::fixAllSlips(string which) throw(Exception)
{
try {
   // find the largest segment and start there, always combine the largest with its
   // largest neighbor
   int i,nmax,ifirst;
   list<Segment>::iterator it, kt;

   // loop over all segments, erasing empty ones
   it = SegList.begin();
   while(it != SegList.end()) {
      if(it->npts == 0)
         it = SegList.erase(it);
      else
         it++;
   }

   if(SegList.empty())
      return NoData;

   // find the largest segment
   for(kt=SegList.end(),nmax=0,it=SegList.begin(); it != SegList.end(); it++) {
      if(it->npts > nmax) {
         nmax = it->npts;
         kt = it;
      }
   }

   // fix all the slips, starting with the largest segment
   // this will merge all segments into one
   GDCUniqueFix = 0;
   while(kt != SegList.end()) {
      fixOneSlip(kt,which);
   }

   // TD here to return should be a separate call...

   // now compute stats for the WL for the (single segment) whole pass
   kt = SegList.begin();
   if(which == string("WL")) {
      WLPassStats.Reset();
      for(i=kt->nbeg; i <= kt->nend; i++) {
         if(!(data[i].flag & OK)) continue;
         WLPassStats.Add(data[i].P1 - kt->bias1);
      }
      // NB Now you have a measure of range noise for the whole pass :
      // sigma(WLbias) ~ sigma(WLrange) = 0.71*sigma(range), so
      // range noise = WLPassStats.StdDev() * wlwl / 0.71;  // meters
      // 0.71 / wlwl = 0.83

      // TD mark the first slip 'fixed' - unmark it - or something
   }
   // change the biases - reset the GFP bias so that it matches the GFR
   // (NB dumpSegments does not remove a bias from L1)
   else {
      ifirst = -1;
      for(i=kt->nbeg; i <= kt->nend; i++) {
         if(!(data[i].flag & OK)) continue;
         if(ifirst == -1) {
            ifirst = i;
            kt->bias2 = data[ifirst].L2 - data[ifirst].P2;
            kt->bias1 = data[ifirst].P1;
         }
         // change the arrays - recompute GFR-GFP so it has one consistent bias
         data[i].L1 = data[i].L2 - kt->bias2 - data[i].P2;
      }
   }

   if(cfg(Debug) >= 3) dumpSegments(which + string("F"),2,true);   // WLF GFF

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

//------------------------------------------------------------------------------------
// called by fixAllSlips
// assume there are no empty segments in the list
void GDCPass::fixOneSlip(list<Segment>::iterator& kt, string which) throw(Exception)
{
try {
   if(kt->npts == 0) { kt++; return; }

   list<Segment>::iterator left,right,it;

   // kt points to the biggest segment
   // define left and right to be the two segments on each side of the slip to be
   // fixed; assume there are no empty segments in the list
   right = left = kt;

   // choose the next segment on the right of kt
   right++;

   // choose the next segment on the left of kt
   if(kt != SegList.begin())
      left--;
   else
      left = SegList.end();            // nothing on the left

   // no segment left of kt and no segment right of kt - nothing to do
   if(left == SegList.end() && right == SegList.end()) {
      kt++;
      return;
   }

   // Always define kt to == left, as it will be returned and right will be erased.
   if(left == SegList.end()) {         // no segment on left
      left = kt;
   }
   else if(right == SegList.end()      // no segment on right
      || left->npts >= right->npts) {  // or left is the bigger segment
      right = kt;
      kt = left;                       // fix between left and kt
   }
   else {                              // left and right exist, and right is bigger
      left = kt;                       // fix between kt and right
   }