editdds.cpp

linux的gps应用

   const double tolerance = 0.5;         // cycles
   // 3.4 const double tol = 0.5;        // cycles -- TD make input
   // 3.2 revert tol = 0.9;        // cycles
//#define turnoffslips 1

      // iterate
   for(iter=1; iter<=iter_limit; iter++) {
      if(iter == 1) tol = tolerance;
      if(iter >  1) tol = 0.6 * tolerance;
      td.clear();
      slipsize.clear();
      slipindex.clear();
      tsstats.Reset();

         // -------------------------------------- find slips
         // compute triple differences
         // j is the index of the previous good point
      for(k=0,j=-1,i=0; i<dddata.count.size(); i++) {
         if(mark[i] == 0) {
            //oflog << "Data 1 marked at count " << dddata.count[i] << endl;
            continue;
         }
         if(j == -1) { j = i; continue; }
         //tdcount = dddata.count[i];
         tt = FirstEpoch + CI.DataInterval * dddata.count[i];
         tddt = dddata.count[i]-dddata.count[j];
         td.push_back(
            (frequency == 1 ? dddata.DDL1[i] - dddata.DDL1[j]
                            : dddata.DDL2[i] - dddata.DDL2[j]  )
               - (dddata.DDER[i] - dddata.DDER[j])
         );
         tsstats.Add(dddata.count[i],td[k]);
            // slip in cycles
         slip = td[k]/(frequency == 1 ? wl1 : wl2);
            // fractional part of slip
         fslip = fabs(fmod(slip,1.0));
         if(fslip > 0.5) fslip = 1.0-fslip;
#ifndef turnoffslips
            // look for slips
            // if frac > 0.2, call it a slip anyway and hope it will be combined
         if(fabs(slip) > tol) {  // || fslip > 0.2) 
            oflog << " Warning - DD " << ddid << " L" << frequency << fixed
               << " slip " << setprecision(3) << setw(8) << slip << " cycles, at "
               << tt.printf(" %4F %10.3g = %Y/%02m/%02d %2H:%02M:%6.3f")
               << " = count " << dddata.count[i] << " on iteration " << iter
               << endl;

               // first see if it can be combined with previous slip
            n = slipindex.size();
            if(n>0 && dddata.count[i]-dddata.count[slipindex[n-1]] < CI.MaxGap)  {
                  // combine these slips
               slipsize[n-1] += slip;
                  // mark all points from old slip to pt before this as bad
               for(m=slipindex[n-1]; m<i; m++) {
                  mark[m] = 0;
                  ngood--;
                  nbad++;
               }
               slipindex[n-1] = i;
               oflog << " Warning - DD " << ddid << " L" << frequency << fixed
                     << " last two slips combined (iter " << iter << ")"
                     << endl;
            }
            else {
               slipindex.push_back(i);
               slipsize.push_back(slip);
            }
         }
#endif
         if(tddofs) {
            tddofs << "TDS " << ddid << " L" << frequency << fixed
               << " " << iter
               << " " << setw(4) << dddata.count[i]
               << " " << tt.printf("%4F %10.3g")
               << " " << setw(3) << tddt << setprecision(6)
               << " " << setw(11) << td[k]
               << " " << setw(11) << slip << setprecision(3)
               << " " << setw(8) << fslip
               << endl; }

         k++;
         j = i;
      } // end for loop over dddata to compute TDs

         // if too small, delete the whole pass
      if(td.size() < 10) return -1;

         // print stats to log
      if(CI.Verbose) {
         double median,mad,mest;
         vector<double> weights;
         weights.resize(td.size());
         mad = Robust::MedianAbsoluteDeviation(&td[0], td.size(), median);
         mest = Robust::MEstimate(&td[0], td.size(), median, mad, &weights[0]);

         oflog << " TUR " << ddid << " L" << frequency << fixed << setprecision(3)
            << " " << iter
            << " " << setw(4) << tsstats.N()
            << " " << setw(7) << tsstats.AverageY()
            << " " << setw(7) << tsstats.StdDevY()
            << " " << setw(7) << tsstats.SigmaYX()
            << "  " << setw(7) << median
            << " " << setw(7) << mest
            << " " << setw(7) << mad
            << endl;
      }

         // if no slips found, normal return
      if(slipindex.size() == 0) return 0;
         // if on last iteration, don't bother to fix...
      if(iter == iter_limit) break;
         // TD check for too many slips -> reject the whole pass

      //if(CI.Verbose) for(i=0; i<slipindex.size(); i++)
      //   oflog << "Slip " << " L" << frequency << setprecision(3) << slipsize[i]
      //      << " found at count " << dddata.count[slipindex[i]] << endl;

         // -------------------------------------- remove slips
         // add a dummy..
      slipindex.push_back(99999);

         // ii is slip count, k is current correction in cycles,
         // j is index of previous good point
      for(k=0,j=-1,ii=0,i=0; i<dddata.count.size(); i++) {
         if(mark[i] == 0) {
            //oflog << "Data 2 marked at " << dddata.count[i] << endl;
            continue;
         }
         tt = FirstEpoch + CI.DataInterval * dddata.count[i];
            // fix
         if(i == slipindex[ii]) {     // new slip on this count
            k += int(slipsize[ii] + (slipsize[ii]>0 ? 0.5 : -0.5));
            if(CI.Verbose) oflog << " Fix L" << frequency << " slip at count "
               << dddata.count[i]
               << " " << tt.printf("%4F %10.3g")
               << " total mag " << k << " iteration " << iter
               << endl;
            ii++;
         }
            // fix double differences using accumulated net slip
         if(k != 0) {
            if(frequency == 1) dddata.DDL1[i] -= k * wl1;
            else               dddata.DDL2[i] -= k * wl2;
         }
            // output the slip-edited DDs and TDs
         if(tddofs) {
            tddofs << "SED " << ddid << fixed
               << " L" << frequency
               << " " << iter
               << " " << setw(4) << dddata.count[i]
               << " " << tt.printf("%4F %10.3g")
               << " " << setw(11) << setprecision(6)           // DD in m
               << (frequency == 1 ?  dddata.DDL1[i] : dddata.DDL2[i])
                     - dddata.DDER[i]
               << " " << setw(11)
               << (j == -1 ? 0.0 :                             // TD in m
                     (frequency == 1 ?  dddata.DDL1[i] - dddata.DDL1[j] :
                                        dddata.DDL2[i] - dddata.DDL2[j])
                        - (dddata.DDER[i] - dddata.DDER[j])
                  )
               << endl;

            j = i;

         }  // end output

      }  // end for loop over data to fix slips

   } // end for loop over iterations

      // failed - return non-zero to delete the whole segment
   oflog << " Warning - Delete " << ddid << " L" << frequency
      << ": unable to fix slips" << endl;

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

//------------------------------------------------------------------------------------
// No - use sigma stripping
// Process using robust least squares fit to a polynomial
// TD consider using a straight line if size of the dataset is small.
// Form vector of data = phase residual = raw DD phase minus DD ephemeris range
// Compute a robust LS fit to a polynomial, compute statistics on residuals of fit.
// Use weights and residuals normalized by RMSROF to mark outliers.
// Data used to test this algorithm come from T202:
// ASWA CTRA G11 G14  T202B
// ASWA CTRA G16 G25  T202D
// ASWA CTRA G20 G25  T202D
int EditDDOutliers(const DDid& ddid, DDData& dddata, int frequency)
{
try {
   int i,j,n,tol;
   int N,M;                            // number of parameters, number of data
   int len = int(dddata.count.size()); // length of the buffers
   double median,mad,mest;
   Vector<double> cnt;
   Vector<double> dat,residuals,weights;
   TwoSampleStats<double> tsstats;

   if(len < 10) return -1;

   int tolsigstrip = 10; // limit on ratio of ddph to MAD  10  1000
   double tolsigyx = 0.02;  // limit on conditional sigma    0.02 0.5

   for(int iter=1; iter<=2; iter++) {
      dat.resize(len);
      cnt.resize(len);

         // pull out the good data, count it and ...
      for(M=0,i=0; i<len; i++) {
         if(mark[i] == 0) continue;             // skip the bad points

         if(frequency == 1)
            dat[M] = dddata.DDL1[i] - dddata.DDER[i];
         else
            dat[M] = dddata.DDL2[i] - dddata.DDER[i];
            // pull out the corresponding counts
         cnt[M] = double(dddata.count[i]);
            // count the number of good points
         M++;
      }
      if(M != len) {
         dat.resize(M);
         cnt.resize(M);      // important -- see LSPolyFunc()
      }

         // ... compute stats on it
      tsstats.Reset();
      tsstats.Add(cnt,dat);
      mad = Robust::MedianAbsoluteDeviation(&dat[0], dat.size(), median);
      mest = Robust::MEstimate(&dat[0], dat.size(), median, mad, &weights[0]);

         // print stats to log
      if(CI.Verbose) {
         oflog << " SUR " << ddid << " L" << frequency << " " << iter
            << fixed << setprecision(3)
            << " " << setw(4) << tsstats.N()
            << " " << setw(7) << tsstats.AverageY()
            << " " << setw(7) << tsstats.StdDevY()
            << " " << setw(7) << tsstats.SigmaYX()
            << "  " << setw(7) << median
            << " " << setw(7) << mest
            << " " << setw(7) << mad
            << endl;
      }

         // only continue if the conditional sigma is high...
      if(tsstats.SigmaYX() <= tolsigyx) return 0; // success

      oflog << " Warning - high sigma (" << iter << ") for "
         << ddid << " L" << frequency << " : " << fixed
         << setprecision(3) << setw(7) << tsstats.SigmaYX() << endl;

         // if this is the second iteration, failure
      if(iter == 2) break;

         // sigma stripping ... robust fit to quadratic is too slow...
      for(n=j=0,i=0; i<len; i++) {
         if(mark[i] == 0) continue;              // skip the bad points

         //oflog << "HIS " << ddid
         //   << " L" << frequency << " " << setw(3) << i
         //   << " " << setw(3) << dddata.count[i]
         //   << fixed << setprecision(3)
         //   << " " << setw(8) << dat[j]
         //   << endl;

         if(fabs(dat[j]) > tolsigstrip*mad) {
            if(CI.Verbose) oflog << " Warning - mark outlier " << ddid
               << " L" << frequency << fixed << setprecision(3)
               << " count " << dddata.count[i]
               << " ddph " << dat[j]
               << " res/sig " << fabs(dat[j])/(tolsigstrip*mad)
               << endl;
            mark[i] = 0;
            ngood--;
            nbad++;
            n++;
         }
         j++;
      }

   }  // end iteration loop

      // failed - return non-zero to delete the whole segment
   oflog << " Warning - Delete " << ddid << " L" << frequency
      << " : unable to sigma strip" << endl;

   return -1;

/* this is too slow...
      // leave LSdata unchanged; SRIFilter will take data input, output weights.
   weights = LSdata;

      // compute a robust least squares fit to a polynomial
   N = 3;    // degree of polynomial .. TD consider 2 if dataset is small
   Vector<double> sol(N);
   Matrix<double> cov(N,N);
   SRIFilter robfit(N);
   //robfit.doVerbose = true;   // temp
   robfit.doRobust = true;
   robfit.doLinearize = false;
   robfit.doWeight = false;
   robfit.doSequential = false;
   robfit.iterationsLimit = 10;
   robfit.convergenceLimit = 1.e-2;
   sol = 0.0;

      // robust LS will return weights in data Vector = weights
   i = robfit.leastSquaresEstimation(weights,sol,cov,&LSPolyFunc);
   if(i) {
      oflog << " Warning - outlier check: robust fit for " << ddid
         << " returned " << i << endl;
      if(i==-1) return i;     // underdetermined
      if(i==-2) return i;     // singular
      if(i==-3) return i;     // failed to converge
      return i;   // ??
   }
   //if(!robfit.isValid())      // probably did not converge

      // compute post-fit residual weighted statistics
   Vector<double> f(M);
   Matrix<double> partials(M,N);
   LSPolyFunc(sol,f,partials);     // so f = solution evaluated at each point

   residuals = LSdata - f;         // residuals of fit

   stats.Reset();
   stats.Add(residuals,weights);   // compute weighted stats

      // Loop over counts (epochs)
   for(j=0,i=0; i<len; i++) {

      if(mark[i] == 0) continue;              // skip the bad points

      double resnorm = fabs(residuals[j]/stats.StdDev());

      if(CI.Verbose) oflog << "FIT " << ddid    // TD debug?
         << " " << setw(3) << i
         << " " << setw(3) << dddata.count[i]
         << fixed << setprecision(3)
         << " " << setw(5) << weights[j]
         << " " << setw(8) << LSdata[j]
         << " " << setw(8) << f[j]
         << " " << setw(8) << residuals[j]
         << " " << setw(8) << resnorm
         << endl;

      if(weights[j] <= 0.25 && resnorm > 4.0) {
         if(CI.Verbose) oflog << " Warning - mark outlier " << ddid
            << fixed << setprecision(3)
            << " count " << dddata.count[i]
            << " weight " << weights[j]
            << " res/sig " << resnorm
            << endl;
         mark[i] = 0;
         ngood--;
         nbad++;
      }

      j++;

   }  // end loop over counts
*/

   return 0;
}
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 the partials matrix P and the solution at each data point (Vector f),
// given the solution Vector X. Called by SRIFilter::leastSquaresEstimation()
//void LSPolyFunc(Vector<double>& X, Vector<double>& f, Matrix<double>& P)
//{
//   try {
//      for(int i=0; i<LScount.size(); i++) {
//         double t = LScount[i] - LScount[0];
//         P(i,0) = 1.0;
//         for(int j=1; j<X.size(); j++) P(i,j) = P(i,j-1) * t;
//      }
//      f = P * X;
//   }
//   catch(Exception& e) { GPSTK_RETHROW(e); }
//}

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