disccorr.cpp

linux的gps应用

   // fix the slip between left and right, making data in 'right' part of 'left'
   if(which == string("WL"))
      WLslipFix(left,right);
   else
      GFslipFix(left,right);

   left->npts += right->npts;
   left->nend = right->nend;

   // always delete right, otherwise on return kt(==left) will be invalid
   // (ignore return value = iterator to first element after the one erased)
   SegList.erase(right);

   return;
}
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 fixOneSlip
void GDCPass::WLslipFix(list<Segment>::iterator& left,
                        list<Segment>::iterator& right)
throw(Exception)
{
try {
   int i;

   GDCUniqueFix++;

   // full slip
   double dwl = right->bias1 + right->WLStats.Average()
         - (left->bias1 + left->WLStats.Average());
   long nwl = long(dwl + (dwl > 0 ? 0.5 : -0.5));

      // TD ? test gap size?
   //if(cfg(DT)*(right->nbeg - left->nend) > cfg(MaxGap)) break;

      // test that total variance is small
   //if(sqrt(left->WLStats.Variance() + right->WLStats.Variance())
   //   / (left->WLStats.N() + right->WLStats.N()) < cfg(WLFixSigma)) {
   //   log << "Cannot fix WL slip (noisy) at " << right->nbeg
   //      << " " << time(right->nbeg).printf(outFormat)
   //      << endl;
   //   break;
   //}

      // TD ? test fractional part of offset fabs
   //if(fabs(dwl-nwl) > cfg(WLFixFrac)) break;

   if(cfg(Debug) >= 6) log << "Fix " << GDCUnique << " " << sat << " " << GDCUniqueFix
      << " WL " << time(right->nbeg).printf(outFormat)
      << " " << left->nseg << "-" << right->nseg
      << fixed << setprecision(2)
      << " right: " << right->bias1 << " + " << right->WLStats.Average()
      << " - left: " << left->bias1 << " + " << left->WLStats.Average()
      << " = " << dwl << " " << nwl << " "
      << endl;

   // now do the fixing - change the data in the right segment to match left's
   for(i=right->nbeg; i<=right->nend; i++) {
      //if(!(data[i].flag & OK)) continue;
      data[i].P1 -= nwl;                                 // WLbias
      data[i].L2 -= nwl * wl2;                           // GFP
      // add to WLStats
      //if(!(data[i].flag & OK)) continue;
      //left->WLStats.Add(data[i].P1 - left->bias1);
   }

   // fix the slips beyond the 'right' segment.
   // change the data in the GFP, and change the both the data and the bias in the WL.
   // this way, WLStats is still valid, but if we change the GF bias, we will lose
   // that information before the GF slips get fixed.
   list<Segment>::iterator it = right;
   for(it++; it != SegList.end(); it++) {
      // Use real, not int, nwl b/c rounding error in a pass with many slips
      // can build up and produce errors.
      it->bias1 -= dwl;
      for(i=it->nbeg; i<=it->nend; i++) {
         //if(!(data[i].flag & OK)) continue;                 // TD don't?
         data[i].P1 -= nwl;                                 // WLbias
         data[i].L2 -= nwl * wl2;                           // GFP
      }
   }

   // Add to slip list
   Slip newSlip(right->nbeg);
   newSlip.NWL = nwl;
   newSlip.msg = "WL";
   SlipList.push_back(newSlip);

   // mark it
   data[right->nbeg].flag |= WLFIX;

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

//------------------------------------------------------------------------------------
// fix one slip in the geometry-free phase
// called by fixOneSlip
void GDCPass::GFslipFix(list<Segment>::iterator& left,
                        list<Segment>::iterator& right) throw(Exception)
{
try {
      // use this number of data points on each side of slip
   const int Npts=int(cfg(GFFixNpts));
   int i,nb,ne,nl,nr,ilast;
   long n1,nadj;              // slip magnitude (cycles)
   double dn1,dnGFR;
   Stats<double> Lstats,Rstats;

   GDCUniqueFix++;

   // find Npts points on each side of slip
   nb = left->nend;
   i = 1;
   nl = 0;
   ilast = -1;                               // ilast is last good point before slip
   while(nb > left->nbeg && i < Npts) {
      if(data[nb].flag & OK) {
         if(ilast == -1) ilast = nb;
         i++; nl++;
         Lstats.Add(data[nb].L1);
      }
      nb--;
   }
   ne = right->nbeg;
   i = 1;
   nr = 0;
   while(ne < right->nend && i < Npts) {
      if(data[ne].flag & OK) {
         i++; nr++;
         Rstats.Add(data[ne].L1);
      }
      ne++;
   }

   // first estimate of n1, without biases
   // need to use the GFR-GFP estimate here, and limit |nadj| to be well within
   // sigmas on the stats, b/c when ionosphere is very active, GFP and GFR will both
   // vary sharply, and fitting a polynomial to GFP is a BAD thing to do....
   // ultimately, GFR-GFP is accurate but noisy.
   // rms rof should tell you how much weight to put on rof
   // larger rof -> smaller npts and larger degree
   dn1 = data[right->nbeg].L2 - right->bias2 - (data[ilast].L2 - left->bias2);
   // this screws up most fixes
   //dn1 = Rstats.Average() - right->bias2 - (Lstats.Average() - left->bias2);
   n1 = long(dn1 + (dn1 > 0 ? 0.5 : -0.5));

   // TD worry about too small pieces - nr or nl too small

   // estimate the slip using polynomial fits
   nadj = EstimateGFslipFix(left,right,nb,ne,n1);

   // adjust the adjustment if it is not consistent with Lstats vs Rstats
   // dn1+nadj                       - a. current best estimate
   // Rstats.Averge()-Lstats.Average - b. estimate from stats on GFR-GFP across slip
   // difference should be consistent with R/Lstats.StdDev
   // if not, replace nadj with b. - dn1
   dnGFR = Rstats.Average() - Lstats.Average();
   if(fabs(n1+nadj-dnGFR) > (Rstats.StdDev()+Lstats.StdDev())) {
      if(cfg(Debug) >= 6)
         log << "GFRadjust " << GDCUnique << " " << sat << " " << GDCUniqueFix
         << " GF " << time(right->nbeg).printf(outFormat)
         << fixed << setprecision(2)
         << " dbias(GFR): " << Rstats.Average() - Lstats.Average()
         << " n1+nadj: " << n1+nadj;

      nadj = long(dnGFR+(dnGFR > 0 ? 0.5 : -0.5)) - n1;

      if(cfg(Debug) >= 6)
         log << " new n1+nadj: " << n1+nadj << endl;
   }

   // output result
   if(cfg(Debug) >= 6) {
      log << "Fix " << GDCUnique << " " << sat << " " << GDCUniqueFix
      << " GF " << time(right->nbeg).printf(outFormat)
      << fixed << setprecision(2)
      << " dbias: " << right->bias2 - left->bias2
      << ", dn1: " << dn1 << ", n1: " << n1 << ", adj: " << nadj
      << " indexes " << nb << " " << ne << " " << nl << " " << nr
      << " segs " << left->nseg << " " << right->nseg
      << " GFR-GFP "
      << Lstats.N() << " " << Lstats.Average() << " " << Lstats.StdDev()
      << "    "
      << Rstats.N() << " " << Rstats.Average() << " " << Rstats.StdDev()
      << " tests " << n1+nadj-dnGFR << " " << Rstats.StdDev()+Lstats.StdDev()
      << endl;
   }
   
   // full slip, including biases
   dn1 += right->bias2 - left->bias2;
   n1 = long(dn1 + (dn1 > 0 ? 0.5 : -0.5));
   n1 += nadj;

   // now do the fixing : 'change the data' within right segment
   // and through the end of the pass, to fix the slip
   for(i=right->nbeg; i<data.size(); i++) { //=right->nend; i++) {
      //if(!(data[i].flag & OK)) continue;                 // TD? don't?
      //data[i].P1 -= nwl;                           // no change to WLbias
      data[i].L2 -= n1;                              // GFP
      data[i].L1 -= n1;                              // GFR+GFP
   }
   // 'change the bias'  although right is about to be deleted....
   //right->bias2 = left->bias2;

   // Add to slip list, but if one exists with same time tag, use it instead
   list<Slip>::iterator jt;
   for(jt=SlipList.begin(); jt != SlipList.end(); jt++)
      if(jt->index == right->nbeg) break;

   if(jt == SlipList.end()) {
      Slip newSlip(right->nbeg);
      newSlip.N1 = -n1;
      newSlip.msg = "GF only";
      SlipList.push_back(newSlip);
   }
   else {
      jt->N1 = -n1;
      jt->msg += string(" GF");
   }

   // mark it
   data[right->nbeg].flag |= GFFIX;

   return;
}
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 GFslipFix
// estimate GF slip using polynomial fit to data around it
long GDCPass::EstimateGFslipFix(list<Segment>::iterator& left,
                               list<Segment>::iterator& right,
                               int nb, int ne, long n1)
throw(Exception)
{
try {
   bool quit;
   int i,k,in[3];
   double rof,rmsrof[3];
   PolyFit<double> PF[3];

   // start at zero and limit |nadj| to ...TD
   long nadj = 0;

   // use a little indirect indexing array to avoid having to copy PolyFit objects....
   for(k=0; k<3; k++) {
      in[k]=k;
      PF[in[k]].Reset(int(cfg(GFFixDegree)));
   }

   while(1) {
      // compute 3 polynomial fits to this data, with slips of
      // (nadj-1, nadj and nadj+1) wavelengths added to left segment
      for(k=0; k<3; k++) {
         if(PF[in[k]].N() > 0) continue;

         // add all the data
         for(i=nb; i<=ne; i++) {
            if(!(data[i].flag & OK)) continue;
            PF[in[k]].Add(
            // data    - (either          left bias - poss. slip : right bias)
            data[i].L2 - (i < right->nbeg ? left->bias2-n1-(nadj+k-1) : right->bias2),
            //  use debiased count
            data[i].ndt - data[nb].ndt
            );
         }

         // TD check that it not singular

         // compute RMS residual of fit
         rmsrof[in[k]] = 0.0;
         for(i=nb; i<=ne; i++) {
            if(!(data[i].flag & OK)) continue;
            rof =    // data minus fit
            data[i].L2 - (i < right->nbeg ? left->bias2-n1-(nadj+k-1) : right->bias2)
            - PF[in[k]].Evaluate(data[i].ndt - data[nb].ndt);
            rmsrof[in[k]] += rof*rof;
         }
         rmsrof[in[k]] = sqrt(rmsrof[in[k]]);

      }  // end loop over fits

      // the value of this is questionable, b/c with active ionosphere the real
      // GFP is NOT smooth 
      for(quit=false,k=0; k<3; k++) if(rmsrof[in[k]] > cfg(GFFixMaxRMS)) {
         log << "Warning - large RMS ROF in GF slip fix at in,k = "
            << in[k] << " " << k << " " << rmsrof[in[k]] << " abort.\n";
         quit = true;
      }
      if(quit) break;

      //if(cfg(Debug) >= 6) {
      //   log << "Fix GF slip RMSROF : adj: " << nadj;
      //   for(i=0; i<3; i++) log << " " << rmsrof[in[i]];
      //   // below log << endl;
      //}

      // three cases: (TD - exceptions?) :
      // rmsrof: 0 > 1 < 2   good
      //         0 > 1 > 2   shift 0,1,2 to 1,2,3
      //         0 < 1 < 2   shift 0,1,2 to -1,0,1
      //         0 < 1 > 2   local max! - ??
      if(rmsrof[in[0]] > rmsrof[in[1]]) {
         if(rmsrof[in[1]] < rmsrof[in[2]]) { // local min - done
            //if(cfg(Debug) >= 6) log << " done." << endl;
            break;
         }
         else {                              // shift 0,1,2 to 1,2,3
            k = in[0];
            in[0] = in[1];
            in[1] = in[2];
            in[2] = k;
            PF[in[2]].Reset();
            nadj += 1;
            //if(cfg(Debug) >= 6) log << " shift left" << endl;
         }
      }
      else {
         if(rmsrof[in[1]] < rmsrof[in[2]]) { // shift 0,1,2 to -1,0,1
            k = in[2];
            in[2] = in[1];
            in[1] = in[0];
            in[0] = k;
            PF[in[0]].Reset();
            nadj -= 1;
            //if(cfg(Debug) >= 6) log << " shift right" << endl;
         }
         else {                              // local max
            log << "Warning - local maximum in RMS residuals in EstimateGFslipFix"
               << endl;
            // TD do something
            break;
         }
      }

   }  // end while loop

   // dump the raw data with all the fits
   for(i=nb; i<=ne; i++) {
      if(!(data[i].flag & OK)) continue;
      log << "GFE " << GDCUnique << " " << sat
         << " " << GDCUniqueFix
         << " " << time(i).printf(outFormat)
         << " " << setw(2) << data[i].flag << fixed << setprecision(3);
      for(k=0; k<3; k++) log << " "
         << data[i].L2 - (i < right->nbeg ? left->bias2-n1-(nadj+k-1) : right->bias2)
         << " " << PF[in[k]].Evaluate(data[i].ndt - data[nb].ndt);
      log << " " << setw(3) << data[i].ndt << endl;
   }

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

//------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------